diff --git a/.gitignore b/.gitignore index 915a798c7..b25a8f8b0 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ venv/ +.clang-format .DS_Store .tags .ipynb_checkpoints @@ -6,7 +7,7 @@ venv/ .overlay_init .overlay_consistent .sconsign.dblite -.vscode +.vscode* model2.png a.out @@ -31,6 +32,7 @@ a.out *.vcd config.json clcache +compile_commands.json persist board/obj/ @@ -42,6 +44,7 @@ selfdrive/ui/_ui selfdrive/test/longitudinal_maneuvers/out selfdrive/visiond/visiond selfdrive/loggerd/loggerd +selfdrive/loggerd/bootlog selfdrive/sensord/_gpsd selfdrive/sensord/_sensord selfdrive/camerad/camerad @@ -68,3 +71,6 @@ flycheck_* cppcheck_report.txt comma.sh + +selfdrive/modeld/thneed/compile +models/*.thneed diff --git a/Jenkinsfile b/Jenkinsfile index a9479a111..32bdc4965 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,25 +1,39 @@ def phone(String ip, String step_label, String cmd) { - def ci_env = "CI=1 TEST_DIR=${env.TEST_DIR} GIT_BRANCH=${env.GIT_BRANCH} GIT_COMMIT=${env.GIT_COMMIT}" - withCredentials([file(credentialsId: 'id_rsa_public', variable: 'key_file')]) { - sh label: step_label, - script: """ - ssh -tt -o StrictHostKeyChecking=no -i ${key_file} -p 8022 root@${ip} '${ci_env} /usr/bin/bash -le' <<'EOF' -echo \$\$ > /dev/cpuset/app/tasks || true -echo \$PPID > /dev/cpuset/app/tasks || true -mkdir -p /dev/shm -chmod 777 /dev/shm + def ssh_cmd = """ +ssh -tt -o StrictHostKeyChecking=no -i ${key_file} -p 8022 'comma@${ip}' /usr/bin/bash <<'EOF' + +set -e + +export CI=1 +export TEST_DIR=${env.TEST_DIR} +export GIT_BRANCH=${env.GIT_BRANCH} +export GIT_COMMIT=${env.GIT_COMMIT} + +source ~/.bash_profile + +ln -snf ${env.TEST_DIR} /data/pythonpath + +if [ -f /EON ]; then + echo \$\$ > /dev/cpuset/app/tasks || true + echo \$PPID > /dev/cpuset/app/tasks || true + mkdir -p /dev/shm + chmod 777 /dev/shm +fi + cd ${env.TEST_DIR} || true ${cmd} exit 0 + EOF""" + + sh script: ssh_cmd, label: step_label } } def phone_steps(String device_type, steps) { lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) { timeout(time: 60, unit: 'MINUTES') { - phone(device_ip, "kill old processes", "pkill -f comma || true") phone(device_ip, "git checkout", readFile("selfdrive/test/setup_device_ci.sh"),) steps.each { item -> phone(device_ip, item[0], item[1]) @@ -40,7 +54,7 @@ pipeline { stages { - stage('Release Build') { + stage('Build release2') { agent { docker { image 'python:3.7.3' @@ -104,16 +118,17 @@ pipeline { stages { stage('parallel tests') { parallel { - stage('Devel Build') { environment { CI_PUSH = "${env.BRANCH_NAME == 'master' ? 'master-ci' : ' '}" } steps { - phone_steps("eon", [ + phone_steps("eon-build", [ + ["build", "SCONS_CACHE=1 scons -j4"], + ["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"], + ["test manager", "python selfdrive/test/test_manager.py"], + ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], ["build devel", "cd release && CI_PUSH=${env.CI_PUSH} ./build_devel.sh"], - ["test openpilot", "nosetests -s selfdrive/test/test_openpilot.py"], - ["test cpu usage", "cd selfdrive/test/ && ./test_cpu_usage.py"], ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], ["test spinner build", "cd selfdrive/ui/spinner && make clean && make"], ["test text window build", "cd selfdrive/ui/text && make clean && make"], @@ -124,7 +139,8 @@ pipeline { stage('Replay Tests') { steps { phone_steps("eon2", [ - ["camerad/modeld replay", "QCOM_REPLAY=1 scons -j4 && cd selfdrive/test/process_replay && ./camera_replay.py"], + ["build QCOM_REPLAY", "SCONS_CACHE=1 QCOM_REPLAY=1 scons -j4"], + ["camerad/modeld replay", "cd selfdrive/test/process_replay && ./camera_replay.py"], ]) } } @@ -135,13 +151,30 @@ pipeline { ["build", "SCONS_CACHE=1 scons -j4"], ["test sounds", "nosetests -s selfdrive/test/test_sounds.py"], ["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"], - ["test loggerd", "CI=1 python selfdrive/loggerd/tests/test_loggerd.py"], - //["test camerad", "CI=1 python selfdrive/camerad/test/test_camerad.py"], // wait for shelf refactor + ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], + ["test encoder", "python selfdrive/loggerd/tests/test_encoder.py"], + ["test camerad", "python selfdrive/camerad/test/test_camerad.py"], + ["test logcatd", "python selfdrive/logcatd/tests/test_logcatd_android.py"], //["test updater", "python installer/updater/test_updater.py"], ]) } } + stage('Tici Build') { + environment { + R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" + } + steps { + phone_steps("tici", [ + ["build", "SCONS_CACHE=1 scons -j16"], + ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"], + ["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"], + ["test camerad", "python selfdrive/camerad/test/test_camerad.py"], + //["build release3-staging", "cd release && PUSH=${env.R3_PUSH} ./build_release3.sh"], + ]) + } + } + } } } @@ -158,3 +191,4 @@ pipeline { } } } + diff --git a/README.md b/README.md index 4eb640f96..92545d0a5 100644 --- a/README.md +++ b/README.md @@ -77,16 +77,17 @@ Supported Cars | Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | | Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | HR-V 2019-20 | Honda Sensing | openpilot | 25mph1 | 12mph | -| Honda | Insight 2019-20 | All | Stock | 0mph | 3mph | +| Honda | Insight 2019-21 | All | Stock | 0mph | 3mph | | Honda | Inspire 2018 | All | Stock | 0mph | 3mph | | Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph1 | 0mph | | Honda | Passport 2019 | All | openpilot | 25mph1 | 12mph | | Honda | Pilot 2016-19 | Honda Sensing | openpilot | 25mph1 | 12mph | | Honda | Ridgeline 2017-20 | Honda Sensing | openpilot | 25mph1 | 12mph | -| Hyundai | Palisade 2020 | All | Stock | 0mph | 0mph | +| Hyundai | Palisade 2020-21 | All | Stock | 0mph | 0mph | | Hyundai | Sonata 2020-21 | All | Stock | 0mph | 0mph | | Lexus | CT Hybrid 2017-18 | LSS | Stock3| 0mph | 0mph | | Lexus | ES 2019-20 | All | openpilot | 0mph | 0mph | +| Lexus | ES Hybrid 2018 | LSS | Stock3| 0mph | 0mph | | Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | | Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | | Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | @@ -96,15 +97,16 @@ Supported Cars | Lexus | RX 2020-21 | All | openpilot | 0mph | 0mph | | Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | | Lexus | RX Hybrid 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Avalon 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | +| Toyota | Avalon 2016-18, 2021 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Camry 2018-20 | All | Stock | 0mph4 | 0mph | | Toyota | Camry 2021 | All | openpilot | 0mph | 0mph | | Toyota | Camry Hybrid 2018-20 | All | Stock | 0mph4 | 0mph | -| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | +| Toyota | Camry Hybrid 2021 | All | openpilot | 0mph | 0mph | +| Toyota | C-HR 2017-20 | All | Stock | 0mph | 0mph | | Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | | Toyota | Corolla 2017-19 | All | Stock3| 20mph1 | 0mph | | Toyota | Corolla 2020-21 | All | openpilot | 0mph | 0mph | -| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla Hatchback 2019-21 | All | openpilot | 0mph | 0mph | | Toyota | Corolla Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Toyota | Highlander 2017-19 | All | Stock3| 0mph | 0mph | | Toyota | Highlander 2020-21 | All | openpilot | 0mph | 0mph | @@ -113,6 +115,7 @@ Supported Cars | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | +| Toyota | Prius Prime 2021 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Rav4 2019-21 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 Hybrid 2016-18 | TSS-P | Stock3| 0mph | 0mph | @@ -129,6 +132,7 @@ Community Maintained Cars and Features | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| +| Audi | A3 2015, 2017 | Prestige | Stock | 0mph | 0mph | | Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | @@ -136,7 +140,7 @@ Community Maintained Cars and Features | Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | | Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph | | Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | +| Chrysler | Pacifica Hybrid 2019-21 | Adaptive Cruise | Stock | 0mph | 39mph | | Genesis | G70 2018 | All | Stock | 0mph | 0mph | | Genesis | G80 2018 | All | Stock | 0mph | 0mph | | Genesis | G90 2018 | All | Stock | 0mph | 0mph | @@ -148,17 +152,18 @@ Community Maintained Cars and Features | Hyundai | Ioniq Electric 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Kona 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Kona EV 2019 | SCC + LKAS | Stock | 0mph | 0mph | -| Hyundai | Santa Fe 2019 | All | Stock | 0mph | 0mph | -| Hyundai | Sonata 2019 | SCC + LKAS | Stock | 0mph | 0mph | +| Hyundai | Santa Fe 2019-20 | All | Stock | 0mph | 0mph | +| Hyundai | Sonata 2018-2019 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Veloster 2019 | SCC + LKAS | Stock | 5mph | 0mph | | Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | | Jeep | Grand Cherokee 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | -| Kia | Forte 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Forte 2018-19, 2021 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Niro EV 2020 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Optima 2017 | SCC + LKAS | Stock | 0mph | 32mph | | Kia | Optima 2019 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Sorento 2018 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | +| Nissan | Altima 2020 | ProPILOT | Stock | 0mph | 0mph | | Nissan | Leaf 2018-20 | ProPILOT | Stock | 0mph | 0mph | | Nissan | Rogue 2018-19 | ProPILOT | Stock | 0mph | 0mph | | Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | @@ -263,7 +268,7 @@ By using openpilot, you agree to [our Privacy Policy](https://my.comma.ai/privac Safety and Testing ---- -* openpilot observes ISO26262 guidelines, see [SAFETY.md](SAFETY.md) for more detail. +* openpilot observes ISO26262 guidelines, see [SAFETY.md](SAFETY.md) for more details. * openpilot has software in the loop [tests](.github/workflows/test.yaml) that run on every commit. * The safety model code lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details. * panda has software in the loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety). @@ -331,8 +336,6 @@ Directory Structure ├── test # Unit tests, system tests and a car simulator └── ui # The UI -To understand how the services interact, see `cereal/service_list.yaml`. - Licensing ------ diff --git a/RELEASES.md b/RELEASES.md index c427e60dd..035958c8f 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,20 @@ +Version 0.8.2 (2021-02-26) +======================== + * Use model points directly in MPC (no more polyfits), making lateral planning more accurate + * Use model heading prediction for smoother lateral control + * Smarter actuator delay compensation + * Improve qcamera resolution for improved video in explorer and connect + * Adjust maximum engagement speed to better fit the model's training distribution + * New driver monitoring model trained with 3x more diverse data + * Improved face detection with masks + * More predictable DM alerts when visibility is bad + * Rewritten video streaming between openpilot processes + * Improved longitudinal tuning on TSS2 Corolla and Rav4 thanks to briskspirit! + * Audi A3 2015 and 2017 support thanks to keeleysam! + * Nissan Altima 2020 support thanks to avolmensky! + * Lexus ES Hybrid 2018 support thanks to TheInventorMan! + * Toyota Camry Hybrid 2021 support thanks to alancyau! + Version 0.8.1 (2020-12-21) ======================== * Original EON is deprecated, upgrade to comma two diff --git a/SConstruct b/SConstruct index ddca0fe59..4330052df 100644 --- a/SConstruct +++ b/SConstruct @@ -17,6 +17,28 @@ AddOption('--asan', action='store_true', help='turn on ASAN') +AddOption('--ubsan', + action='store_true', + help='turn on UBSan') + +AddOption('--clazy', + action='store_true', + help='build with clazy') + +AddOption('--compile_db', + action='store_true', + help='build clang compilation database') + +AddOption('--mpc-generate', + action='store_true', + help='regenerates the mpc sources') + +AddOption('--external-sconscript', + action='store', + metavar='FILE', + dest='external_sconscript', + help='add an external SConscript to the build') + real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() if platform.system() == "Darwin": arch = "Darwin" @@ -27,11 +49,12 @@ if arch == "aarch64" and TICI: USE_WEBCAM = os.getenv("USE_WEBCAM") is not None QCOM_REPLAY = arch == "aarch64" and os.getenv("QCOM_REPLAY") is not None +lenv = { + "PATH": os.environ['PATH'], +} + if arch == "aarch64" or arch == "larch64": - lenv = { - "LD_LIBRARY_PATH": '/data/data/com.termux/files/usr/lib', - "PATH": os.environ['PATH'], - } + lenv["LD_LIBRARY_PATH"] = '/data/data/com.termux/files/usr/lib' if arch == "aarch64": # android @@ -43,6 +66,7 @@ if arch == "aarch64" or arch == "larch64": ] libpath = [ + "/usr/local/lib", "/usr/lib", "/system/vendor/lib64", "/system/comma/usr/lib", @@ -71,17 +95,10 @@ if arch == "aarch64" or arch == "larch64": if QCOM_REPLAY: cflags += ["-DQCOM_REPLAY"] cxxflags += ["-DQCOM_REPLAY"] - else: cflags = [] cxxflags = [] - - lenv = { - "PATH": "#external/bin:" + os.environ['PATH'], - } - cpppath = [ - "#external/tensorflow/include", - ] + cpppath = [] if arch == "Darwin": libpath = [ @@ -89,15 +106,16 @@ else: "#cereal", "#selfdrive/common", "/usr/local/lib", + "/usr/local/opt/openssl/lib", "/System/Library/Frameworks/OpenGL.framework/Libraries", ] cflags += ["-DGL_SILENCE_DEPRECATION"] cxxflags += ["-DGL_SILENCE_DEPRECATION"] + cpppath += ["/usr/local/opt/openssl/include"] else: libpath = [ "#phonelibs/snpe/x86_64-linux-clang", "#phonelibs/libyuv/x64/lib", - "#external/tensorflow/lib", "#cereal", "#selfdrive/common", "/usr/lib", @@ -106,7 +124,6 @@ else: rpath = [ "phonelibs/snpe/x86_64-linux-clang", - "external/tensorflow/lib", "cereal", "selfdrive/common" ] @@ -115,11 +132,14 @@ else: rpath = [os.path.join(os.getcwd(), x) for x in rpath] if GetOption('asan'): - ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] - ldflags_asan = ["-fsanitize=address"] + ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"] + ldflags = ["-fsanitize=address"] +elif GetOption('ubsan'): + ccflags = ["-fsanitize=undefined"] + ldflags = ["-fsanitize=undefined"] else: - ccflags_asan = [] - ldflags_asan = [] + ccflags = [] + ldflags = [] # change pythonpath to this lenv["PYTHONPATH"] = Dir("#").path @@ -138,11 +158,12 @@ env = Environment( "-Wno-inconsistent-missing-override", "-Wno-c99-designator", "-Wno-reorder-init-list", - ] + cflags + ccflags_asan, + ] + cflags + ccflags, CPPPATH=cpppath + [ "#", "#selfdrive", + "#phonelibs/catch2/include", "#phonelibs/bzip2", "#phonelibs/libyuv/include", "#phonelibs/openmax/include", @@ -155,6 +176,7 @@ env = Environment( "#phonelibs/linux/include", "#phonelibs/snpe/include", "#phonelibs/nanovg", + "#phonelibs/qrcode", "#selfdrive/boardd", "#selfdrive/common", "#selfdrive/camerad", @@ -163,14 +185,15 @@ env = Environment( "#selfdrive/modeld", "#selfdrive/sensord", "#selfdrive/ui", - "#cereal/messaging", "#cereal", + "#cereal/messaging", + "#cereal/visionipc", "#opendbc/can", ], CC='clang', CXX='clang++', - LINKFLAGS=ldflags_asan, + LINKFLAGS=ldflags, RPATH=rpath, @@ -188,7 +211,7 @@ env = Environment( tools=["default", "cython", "compilation_db"], ) -if GetOption('test'): +if GetOption('compile_db'): env.CompilationDatabase('compile_commands.json') if os.environ.get('SCONS_CACHE'): @@ -239,6 +262,64 @@ else: Export('envCython') +# Qt build environment +qt_env = None +if arch in ["x86_64", "Darwin", "larch64"]: + qt_env = env.Clone() + + qt_modules = ["Widgets", "Gui", "Core", "DBus", "Multimedia", "Network", "Concurrent", "WebEngine", "WebEngineWidgets"] + + qt_libs = [] + if arch == "Darwin": + qt_env['QTDIR'] = "/usr/local/opt/qt" + QT_BASE = "/usr/local/opt/qt/" + qt_dirs = [ + QT_BASE + "include/", + ] + qt_dirs += [f"{QT_BASE}include/Qt{m}" for m in qt_modules] + qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"] + qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"] + else: + qt_env['QTDIR'] = "/usr" + qt_dirs = [ + f"/usr/include/{real_arch}-linux-gnu/qt5", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui", + f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui", + ] + qt_dirs += [f"/usr/include/{real_arch}-linux-gnu/qt5/Qt{m}" for m in qt_modules] + + qt_libs = [f"Qt5{m}" for m in qt_modules] + if arch == "larch64": + qt_libs += ["GLESv2", "wayland-client"] + elif arch != "Darwin": + qt_libs += ["GL"] + + qt_env.Tool('qt') + qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"] + qt_flags = [ + "-D_REENTRANT", + "-DQT_NO_DEBUG", + "-DQT_WIDGETS_LIB", + "-DQT_GUI_LIB", + "-DQT_CORE_LIB" + ] + qt_env['CXXFLAGS'] += qt_flags + qt_env['LIBPATH'] += ['#selfdrive/ui'] + qt_env['LIBS'] = qt_libs + + if GetOption("clazy"): + checks = [ + "level0", + "level1", + "no-range-loop", + "no-non-pod-global-static", + ] + qt_env['CXX'] = 'clazy' + qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0] + qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks) +Export('qt_env') + + # still needed for apks zmq = 'zmq' Export('env', 'arch', 'real_arch', 'zmq', 'SHARED', 'USE_WEBCAM', 'QCOM_REPLAY') @@ -251,26 +332,32 @@ if SHARED: else: cereal = [File('#cereal/libcereal.a')] messaging = [File('#cereal/libmessaging.a')] + visionipc = [File('#cereal/libvisionipc.a')] + Export('cereal', 'messaging') SConscript(['selfdrive/common/SConscript']) -Import('_common', '_visionipc', '_gpucommon', '_gpu_libs') +Import('_common', '_gpucommon', '_gpu_libs') if SHARED: - common, visionipc, gpucommon = abspath(common), abspath(visionipc), abspath(gpucommon) + common, gpucommon = abspath(common), abspath(gpucommon) else: common = [_common, 'json11'] - visionipc = _visionipc gpucommon = [_gpucommon] + _gpu_libs -Export('common', 'visionipc', 'gpucommon') +Export('common', 'gpucommon', 'visionipc') + +# Build openpilot + +SConscript(['cereal/SConscript']) SConscript(['opendbc/can/SConscript']) +SConscript(['phonelibs/SConscript']) + SConscript(['common/SConscript']) SConscript(['common/kalman/SConscript']) SConscript(['common/transformations/SConscript']) -SConscript(['phonelibs/SConscript']) SConscript(['selfdrive/camerad/SConscript']) SConscript(['selfdrive/modeld/SConscript']) @@ -294,6 +381,10 @@ SConscript(['selfdrive/ui/SConscript']) if arch != "Darwin": SConscript(['selfdrive/logcatd/SConscript']) -if arch == "x86_64": +if real_arch == "x86_64": + SConscript(['tools/nui/SConscript']) SConscript(['tools/lib/index_log/SConscript']) +external_sconscript = GetOption('external_sconscript') +if external_sconscript: + SConscript([external_sconscript]) diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 90570253e..a99da3ca7 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/cereal/.gitignore b/cereal/.gitignore index 40b416ea6..14cd02eff 100644 --- a/cereal/.gitignore +++ b/cereal/.gitignore @@ -6,10 +6,16 @@ package-lock.json __pycache__ .*.swp .*.swo -libcereal*.a +*.os +*.o +*.a + +test_runner + libmessaging.* libmessaging_shared.* services.h .sconsign.dblite libcereal_shared.* .mypy_cache/ +catch2/ diff --git a/cereal/SConscript b/cereal/SConscript index 192e26c90..bd2cde9c6 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch', 'zmq') +Import('env', 'envCython', 'arch', 'zmq', 'QCOM_REPLAY') import shutil @@ -6,30 +6,28 @@ cereal_dir = Dir('.') gen_dir = Dir('gen') messaging_dir = Dir('messaging') -# TODO: remove src-prefix and cereal from command string. can we set working directory? +# Build cereal + +schema_files = ['log.capnp', 'car.capnp', 'legacy.capnp'] env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") -env.Command(['gen/cpp/car.capnp.c++', 'gen/cpp/log.capnp.c++', 'gen/cpp/car.capnp.h', 'gen/cpp/log.capnp.h'], - ['car.capnp', 'log.capnp'], +env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s in schema_files], + schema_files, f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/") if shutil.which('capnpc-java'): env.Command(['gen/java/Car.java', 'gen/java/Log.java'], - ['car.capnp', 'log.capnp'], + schema_files, f"capnpc $SOURCES --src-prefix={cereal_dir.path} -o java:{gen_dir.path}/java/") # TODO: remove non shared cereal and messaging -cereal_objects = env.SharedObject([ - 'gen/cpp/car.capnp.c++', - 'gen/cpp/log.capnp.c++', -]) +cereal_objects = env.SharedObject([f'gen/cpp/{s}.c++' for s in schema_files]) env.Library('cereal', cereal_objects) env.SharedLibrary('cereal_shared', cereal_objects) -cereal_dir = Dir('.') -services_h = env.Command(['services.h'], - ['service_list.yaml', 'services.py'], - 'python3 ' + cereal_dir.path + '/services.py > $TARGET') +# Build messaging + +services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET') messaging_objects = env.SharedObject([ 'messaging/messaging.cc', @@ -52,10 +50,33 @@ if arch == "aarch64": env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq']) Depends('messaging/bridge.cc', services_h) -# different target? -#env.Program('messaging/demo', ['messaging/demo.cc'], LIBS=[messaging_lib, 'zmq']) - envCython.Program('messaging/messaging_pyx.so', 'messaging/messaging_pyx.pyx', LIBS=envCython["LIBS"]+[messaging_lib, "zmq"]) + +# Build Vision IPC +vipc_sources = [ + 'visionipc/ipc.cc', + 'visionipc/visionipc_server.cc', + 'visionipc/visionipc_client.cc', + 'visionipc/visionbuf.cc', +] + +if arch in ["aarch64", "larch64"] and (not QCOM_REPLAY): + vipc_sources += ['visionipc/visionbuf_ion.cc'] +else: + vipc_sources += ['visionipc/visionbuf_cl.cc'] + +vipc_objects = env.SharedObject(vipc_sources) +vipc = env.Library('visionipc', vipc_objects) + + +libs = envCython["LIBS"]+["OpenCL", "zmq", vipc, messaging_lib] +if arch == "Darwin": + del libs[libs.index('OpenCL')] + envCython['FRAMEWORKS'] += ['OpenCL'] +envCython.Program('visionipc/visionipc_pyx.so', 'visionipc/visionipc_pyx.pyx', LIBS=libs) + + if GetOption('test'): env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib]) + env.Program('visionipc/test_runner', ['visionipc/test_runner.cc', 'visionipc/visionipc_tests.cc'], LIBS=[vipc, messaging_lib, 'zmq', 'pthread', 'OpenCL']) diff --git a/cereal/car.capnp b/cereal/car.capnp index a9f2e9034..9a8743f82 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -11,6 +11,8 @@ $Java.outerClassname("Car"); struct CarEvent @0x9b1657f34caf3ad3 { name @0 :EventName; + + # event types enable @1 :Bool; noEntry @2 :Bool; warning @3 :Bool; # alerts presented only when enabled or soft disabling @@ -21,7 +23,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { permanent @8 :Bool; # alerts presented regardless of openpilot state enum EventName @0xbaa8c5d505f727de { - # TODO: copy from error list canError @0; steerUnavailable @1; brakeUnavailable @2; @@ -36,7 +37,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { buttonEnable @12; pedalPressed @13; cruiseDisabled @14; - radarCanError @15; speedTooLow @17; outOfSpace @18; overheat @19; @@ -73,13 +73,11 @@ struct CarEvent @0x9b1657f34caf3ad3 { preLaneChangeLeft @57; preLaneChangeRight @58; laneChange @59; - internetConnectivityNeeded @61; communityFeatureDisallowed @62; lowMemory @63; stockAeb @64; ldw @65; carUnrecognized @66; - radarCommIssue @67; driverMonitorLowAcc @68; invalidLkasSetting @69; speedTooHigh @70; @@ -100,9 +98,12 @@ struct CarEvent @0x9b1657f34caf3ad3 { deviceFalling @90; fanMalfunction @91; cameraMalfunction @92; - + gpsMalfunction @94; startupOneplus @82; + processNotRunning @95; + radarCanErrorDEPRECATED @15; + radarCommIssueDEPRECATED @67; gasUnavailableDEPRECATED @3; dataNeededDEPRECATED @16; modelCommIssueDEPRECATED @27; @@ -113,6 +114,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { calibrationProgressDEPRECATED @47; invalidGiraffeHondaDEPRECATED @49; invalidGiraffeToyotaDEPRECATED @60; + internetConnectivityNeededDEPRECATED @61; whitePandaUnsupportedDEPRECATED @81; commIssueWarningDEPRECATED @83; focusRecoverActiveDEPRECATED @86; @@ -125,7 +127,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { # all speeds in m/s struct CarState { - errorsDEPRECATED @0 :List(CarEvent.EventName); events @13 :List(CarEvent); # car speed @@ -146,8 +147,8 @@ struct CarState { brakeLights @19 :Bool; # steering wheel - steeringAngle @7 :Float32; # deg - steeringRate @15 :Float32; # deg/s + steeringAngleDeg @7 :Float32; + steeringRateDeg @15 :Float32; steeringTorque @8 :Float32; # TODO: standardize units steeringTorqueEps @27 :Float32; # TODO: standardize units steeringPressed @9 :Bool; # if the user is using the steering wheel @@ -235,6 +236,8 @@ struct CarState { gapAdjustCruise @11; } } + + errorsDEPRECATED @0 :List(CarEvent.EventName); } # ******* radar state @ 20hz ******* @@ -278,10 +281,6 @@ struct CarControl { enabled @0 :Bool; active @7 :Bool; - gasDEPRECATED @1 :Float32; - brakeDEPRECATED @2 :Float32; - steeringTorqueDEPRECATED @3 :Float32; - actuators @6 :Actuators; cruiseControl @4 :CruiseControl; @@ -293,7 +292,7 @@ struct CarControl { brake @1: Float32; # range from -1.0 - 1.0 steer @2: Float32; - steerAngle @3: Float32; + steeringAngleDeg @3: Float32; } struct CruiseControl { @@ -329,8 +328,6 @@ struct CarControl { } enum AudibleAlert { - # these are the choices from the Honda - # map as good as you can for your car none @0; chimeEngage @1; chimeDisengage @2; @@ -342,6 +339,10 @@ struct CarControl { chimeWarning2Repeat @8; } } + + gasDEPRECATED @1 :Float32; + brakeDEPRECATED @2 :Float32; + steeringTorqueDEPRECATED @3 :Float32; } # ****** car param ****** @@ -358,6 +359,7 @@ struct CarParams { minEnableSpeed @7 :Float32; minSteerSpeed @8 :Float32; + maxSteeringAngleDeg @54 :Float32; safetyModel @9 :SafetyModel; safetyModelPassive @42 :SafetyModel = silent; safetyParam @10 :Int16; @@ -406,7 +408,6 @@ struct CarParams { steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control? carVin @38 :Text; # VIN number queried during fingerprinting - isPandaBlack @39: Bool; dashcamOnly @41: Bool; transmissionType @43 :TransmissionType; carFw @44 :List(CarFw); @@ -438,10 +439,19 @@ struct CarParams { } struct LateralINDITuning { - outerLoopGain @0 :Float32; - innerLoopGain @1 :Float32; - timeConstant @2 :Float32; - actuatorEffectiveness @3 :Float32; + outerLoopGainBP @4 :List(Float32); + outerLoopGainV @5 :List(Float32); + innerLoopGainBP @6 :List(Float32); + innerLoopGainV @7 :List(Float32); + timeConstantBP @8 :List(Float32); + timeConstantV @9 :List(Float32); + actuatorEffectivenessBP @10 :List(Float32); + actuatorEffectivenessV @11 :List(Float32); + + outerLoopGainDEPRECATED @0 :Float32; + innerLoopGainDEPRECATED @1 :Float32; + timeConstantDEPRECATED @2 :Float32; + actuatorEffectivenessDEPRECATED @3 :Float32; } struct LateralLQRTuning { @@ -539,4 +549,6 @@ struct CarParams { fwdCamera @0; # Standard/default integration at LKAS camera gateway @1; # Integration at vehicle's CAN gateway } + + isPandaBlackDEPRECATED @39: Bool; } diff --git a/cereal/legacy.capnp b/cereal/legacy.capnp new file mode 100644 index 000000000..6474f4411 --- /dev/null +++ b/cereal/legacy.capnp @@ -0,0 +1,850 @@ +using Cxx = import "./include/c++.capnp"; +$Cxx.namespace("cereal"); + +using Java = import "./include/java.capnp"; +$Java.package("ai.comma.openpilot.cereal"); +$Java.outerClassname("Legacy"); + +@0x80ef1ec4889c2a63; + +# legacy.capnp: a home for deprecated structs + +struct LogRotate @0x9811e1f38f62f2d1 { + segmentNum @0 :Int32; + path @1 :Text; +} + +struct LiveUI @0xc08240f996aefced { + rearViewCam @0 :Bool; + alertText1 @1 :Text; + alertText2 @2 :Text; + awarenessStatus @3 :Float32; +} + +struct OrbslamCorrection @0x8afd33dc9b35e1aa { + correctionMonoTime @0 :UInt64; + prePositionECEF @1 :List(Float64); + postPositionECEF @2 :List(Float64); + prePoseQuatECEF @3 :List(Float32); + postPoseQuatECEF @4 :List(Float32); + numInliers @5 :UInt32; +} + +struct EthernetPacket @0xa99a9d5b33cf5859 { + pkt @0 :Data; + ts @1 :Float32; +} + +struct CellInfo @0xcff7566681c277ce { + timestamp @0 :UInt64; + repr @1 :Text; # android toString() for now +} + +struct WifiScan @0xd4df5a192382ba0b { + bssid @0 :Text; + ssid @1 :Text; + capabilities @2 :Text; + frequency @3 :Int32; + level @4 :Int32; + timestamp @5 :Int64; + + centerFreq0 @6 :Int32; + centerFreq1 @7 :Int32; + channelWidth @8 :ChannelWidth; + operatorFriendlyName @9 :Text; + venueName @10 :Text; + is80211mcResponder @11 :Bool; + passpoint @12 :Bool; + + distanceCm @13 :Int32; + distanceSdCm @14 :Int32; + + enum ChannelWidth @0xcb6a279f015f6b51 { + w20Mhz @0; + w40Mhz @1; + w80Mhz @2; + w160Mhz @3; + w80Plus80Mhz @4; + } +} + +struct LiveEventData @0x94b7baa90c5c321e { + name @0 :Text; + value @1 :Int32; +} + +struct ModelData @0xb8aad62cffef28a9 { + frameId @0 :UInt32; + frameAge @12 :UInt32; + frameDropPerc @13 :Float32; + timestampEof @9 :UInt64; + modelExecutionTime @14 :Float32; + gpuExecutionTime @16 :Float32; + rawPred @15 :Data; + + path @1 :PathData; + leftLane @2 :PathData; + rightLane @3 :PathData; + lead @4 :LeadData; + freePath @6 :List(Float32); + + settings @5 :ModelSettings; + leadFuture @7 :LeadData; + speed @8 :List(Float32); + meta @10 :MetaData; + longitudinal @11 :LongitudinalData; + + struct PathData @0x8817eeea389e9f08 { + points @0 :List(Float32); + prob @1 :Float32; + std @2 :Float32; + stds @3 :List(Float32); + poly @4 :List(Float32); + validLen @5 :Float32; + } + + struct LeadData @0xd1c9bef96d26fa91 { + dist @0 :Float32; + prob @1 :Float32; + std @2 :Float32; + relVel @3 :Float32; + relVelStd @4 :Float32; + relY @5 :Float32; + relYStd @6 :Float32; + relA @7 :Float32; + relAStd @8 :Float32; + } + + struct ModelSettings @0xa26e3710efd3e914 { + bigBoxX @0 :UInt16; + bigBoxY @1 :UInt16; + bigBoxWidth @2 :UInt16; + bigBoxHeight @3 :UInt16; + boxProjection @4 :List(Float32); + yuvCorrection @5 :List(Float32); + inputTransform @6 :List(Float32); + } + + struct MetaData @0x9744f25fb60f2bf8 { + engagedProb @0 :Float32; + desirePrediction @1 :List(Float32); + brakeDisengageProb @2 :Float32; + gasDisengageProb @3 :Float32; + steerOverrideProb @4 :Float32; + desireState @5 :List(Float32); + } + + struct LongitudinalData @0xf98f999c6a071122 { + distances @2 :List(Float32); + speeds @0 :List(Float32); + accelerations @1 :List(Float32); + } +} + +struct ECEFPoint @0xc25bbbd524983447 { + x @0 :Float64; + y @1 :Float64; + z @2 :Float64; +} + +struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 { + x @0 :Float32; + y @1 :Float32; + z @2 :Float32; +} + +struct GPSPlannerPoints @0xab54c59699f8f9f3 { + curPosDEPRECATED @0 :ECEFPointDEPRECATED; + pointsDEPRECATED @1 :List(ECEFPointDEPRECATED); + curPos @6 :ECEFPoint; + points @7 :List(ECEFPoint); + valid @2 :Bool; + trackName @3 :Text; + speedLimit @4 :Float32; + accelTarget @5 :Float32; +} + +struct GPSPlannerPlan @0xf5ad1d90cdc1dd6b { + valid @0 :Bool; + poly @1 :List(Float32); + trackName @2 :Text; + speed @3 :Float32; + acceleration @4 :Float32; + pointsDEPRECATED @5 :List(ECEFPointDEPRECATED); + points @6 :List(ECEFPoint); + xLookahead @7 :Float32; +} + +struct UiNavigationEvent @0x90c8426c3eaddd3b { + type @0: Type; + status @1: Status; + distanceTo @2: Float32; + endRoadPointDEPRECATED @3: ECEFPointDEPRECATED; + endRoadPoint @4: ECEFPoint; + + enum Type @0xe8db07dcf8fcea05 { + none @0; + laneChangeLeft @1; + laneChangeRight @2; + mergeLeft @3; + mergeRight @4; + turnLeft @5; + turnRight @6; + } + + enum Status @0xb9aa88c75ef99a1f { + none @0; + passive @1; + approaching @2; + active @3; + } +} + +struct LiveLocationData @0xb99b2bc7a57e8128 { + status @0 :UInt8; + + # 3D fix + lat @1 :Float64; + lon @2 :Float64; + alt @3 :Float32; # m + + # speed + speed @4 :Float32; # m/s + + # NED velocity components + vNED @5 :List(Float32); + + # roll, pitch, heading (x,y,z) + roll @6 :Float32; # WRT to center of earth? + pitch @7 :Float32; # WRT to center of earth? + heading @8 :Float32; # WRT to north? + + # what are these? + wanderAngle @9 :Float32; + trackAngle @10 :Float32; + + # car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png + + # gyro, in car frame, deg/s + gyro @11 :List(Float32); + + # accel, in car frame, m/s^2 + accel @12 :List(Float32); + + accuracy @13 :Accuracy; + + source @14 :SensorSource; + # if we are fixing a location in the past + fixMonoTime @15 :UInt64; + + gpsWeek @16 :Int32; + timeOfWeek @17 :Float64; + + positionECEF @18 :List(Float64); + poseQuatECEF @19 :List(Float32); + pitchCalibration @20 :Float32; + yawCalibration @21 :Float32; + imuFrame @22 :List(Float32); + + struct Accuracy @0x943dc4625473b03f { + pNEDError @0 :List(Float32); + vNEDError @1 :List(Float32); + rollError @2 :Float32; + pitchError @3 :Float32; + headingError @4 :Float32; + ellipsoidSemiMajorError @5 :Float32; + ellipsoidSemiMinorError @6 :Float32; + ellipsoidOrientationError @7 :Float32; + } + + enum SensorSource @0xc871d3cc252af657 { + applanix @0; + kalman @1; + orbslam @2; + timing @3; + dummy @4; + } +} + +struct OrbOdometry @0xd7700859ed1f5b76 { + # timing first + startMonoTime @0 :UInt64; + endMonoTime @1 :UInt64; + + # fundamental matrix and error + f @2: List(Float64); + err @3: Float64; + + # number of inlier points + inliers @4: Int32; + + # for debug only + # indexed by endMonoTime features + # value is startMonoTime feature match + # -1 if no match + matches @5: List(Int16); +} + +struct OrbFeatures @0xcd60164a8a0159ef { + timestampEof @0 :UInt64; + # transposed arrays of normalized image coordinates + # len(xs) == len(ys) == len(descriptors) * 32 + xs @1 :List(Float32); + ys @2 :List(Float32); + descriptors @3 :Data; + octaves @4 :List(Int8); + + # match index to last OrbFeatures + # -1 if no match + timestampLastEof @5 :UInt64; + matches @6: List(Int16); +} + +struct OrbFeaturesSummary @0xd500d30c5803fa4f { + timestampEof @0 :UInt64; + timestampLastEof @1 :UInt64; + + featureCount @2 :UInt16; + matchCount @3 :UInt16; + computeNs @4 :UInt64; +} + +struct OrbKeyFrame @0xc8233c0345e27e24 { + # this is a globally unique id for the KeyFrame + id @0: UInt64; + + # this is the location of the KeyFrame + pos @1: ECEFPoint; + + # these are the features in the world + # len(dpos) == len(descriptors) * 32 + dpos @2 :List(ECEFPoint); + descriptors @3 :Data; +} + +struct KalmanOdometry @0x92e21bb7ea38793a { + trans @0 :List(Float32); # m/s in device frame + rot @1 :List(Float32); # rad/s in device frame + transStd @2 :List(Float32); # std m/s in device frame + rotStd @3 :List(Float32); # std rad/s in device frame +} + +struct OrbObservation @0x9b326d4e436afec7 { + observationMonoTime @0 :UInt64; + normalizedCoordinates @1 :List(Float32); + locationECEF @2 :List(Float64); + matchDistance @3: UInt32; +} + +struct CalibrationFeatures @0x8fdfadb254ea867a { + frameId @0 :UInt32; + + p0 @1 :List(Float32); + p1 @2 :List(Float32); + status @3 :List(Int8); +} + +struct NavStatus @0xbd8822120928120c { + isNavigating @0 :Bool; + currentAddress @1 :Address; + + struct Address @0xce7cd672cacc7814 { + title @0 :Text; + lat @1 :Float64; + lng @2 :Float64; + house @3 :Text; + address @4 :Text; + street @5 :Text; + city @6 :Text; + state @7 :Text; + country @8 :Text; + } +} + +struct NavUpdate @0xdb98be6565516acb { + isNavigating @0 :Bool; + curSegment @1 :Int32; + segments @2 :List(Segment); + + struct LatLng @0x9eaef9187cadbb9b { + lat @0 :Float64; + lng @1 :Float64; + } + + struct Segment @0xa5b39b4fc4d7da3f { + from @0 :LatLng; + to @1 :LatLng; + updateTime @2 :Int32; + distance @3 :Int32; + crossTime @4 :Int32; + exitNo @5 :Int32; + instruction @6 :Instruction; + + parts @7 :List(LatLng); + + enum Instruction @0xc5417a637451246f { + turnLeft @0; + turnRight @1; + keepLeft @2; + keepRight @3; + straight @4; + roundaboutExitNumber @5; + roundaboutExit @6; + roundaboutTurnLeft @7; + unkn8 @8; + roundaboutStraight @9; + unkn10 @10; + roundaboutTurnRight @11; + unkn12 @12; + roundaboutUturn @13; + unkn14 @14; + arrive @15; + exitLeft @16; + exitRight @17; + unkn18 @18; + uturn @19; + # ... + } + } +} + +struct TrafficEvent @0xacfa74a094e62626 { + type @0 :Type; + distance @1 :Float32; + action @2 :Action; + resuming @3 :Bool; + + enum Type @0xd85d75253435bf4b { + stopSign @0; + lightRed @1; + lightYellow @2; + lightGreen @3; + stopLight @4; + } + + enum Action @0xa6f6ce72165ccb49 { + none @0; + yield @1; + stop @2; + resumeReady @3; + } + +} + + +struct AndroidGnss @0xdfdf30d03fc485bd { + union { + measurements @0 :Measurements; + navigationMessage @1 :NavigationMessage; + } + + struct Measurements @0xa20710d4f428d6cd { + clock @0 :Clock; + measurements @1 :List(Measurement); + + struct Clock @0xa0e27b453a38f450 { + timeNanos @0 :Int64; + hardwareClockDiscontinuityCount @1 :Int32; + + hasTimeUncertaintyNanos @2 :Bool; + timeUncertaintyNanos @3 :Float64; + + hasLeapSecond @4 :Bool; + leapSecond @5 :Int32; + + hasFullBiasNanos @6 :Bool; + fullBiasNanos @7 :Int64; + + hasBiasNanos @8 :Bool; + biasNanos @9 :Float64; + + hasBiasUncertaintyNanos @10 :Bool; + biasUncertaintyNanos @11 :Float64; + + hasDriftNanosPerSecond @12 :Bool; + driftNanosPerSecond @13 :Float64; + + hasDriftUncertaintyNanosPerSecond @14 :Bool; + driftUncertaintyNanosPerSecond @15 :Float64; + } + + struct Measurement @0xd949bf717d77614d { + svId @0 :Int32; + constellation @1 :Constellation; + + timeOffsetNanos @2 :Float64; + state @3 :Int32; + receivedSvTimeNanos @4 :Int64; + receivedSvTimeUncertaintyNanos @5 :Int64; + cn0DbHz @6 :Float64; + pseudorangeRateMetersPerSecond @7 :Float64; + pseudorangeRateUncertaintyMetersPerSecond @8 :Float64; + accumulatedDeltaRangeState @9 :Int32; + accumulatedDeltaRangeMeters @10 :Float64; + accumulatedDeltaRangeUncertaintyMeters @11 :Float64; + + hasCarrierFrequencyHz @12 :Bool; + carrierFrequencyHz @13 :Float32; + hasCarrierCycles @14 :Bool; + carrierCycles @15 :Int64; + hasCarrierPhase @16 :Bool; + carrierPhase @17 :Float64; + hasCarrierPhaseUncertainty @18 :Bool; + carrierPhaseUncertainty @19 :Float64; + hasSnrInDb @20 :Bool; + snrInDb @21 :Float64; + + multipathIndicator @22 :MultipathIndicator; + + enum Constellation @0x9ef1f3ff0deb5ffb { + unknown @0; + gps @1; + sbas @2; + glonass @3; + qzss @4; + beidou @5; + galileo @6; + } + + enum State @0xcbb9490adce12d72 { + unknown @0; + codeLock @1; + bitSync @2; + subframeSync @3; + towDecoded @4; + msecAmbiguous @5; + symbolSync @6; + gloStringSync @7; + gloTodDecoded @8; + bdsD2BitSync @9; + bdsD2SubframeSync @10; + galE1bcCodeLock @11; + galE1c2ndCodeLock @12; + galE1bPageSync @13; + sbasSync @14; + } + + enum MultipathIndicator @0xc04e7b6231d4caa8 { + unknown @0; + detected @1; + notDetected @2; + } + } + } + + struct NavigationMessage @0xe2517b083095fd4e { + type @0 :Int32; + svId @1 :Int32; + messageId @2 :Int32; + submessageId @3 :Int32; + data @4 :Data; + status @5 :Status; + + enum Status @0xec1ff7996b35366f { + unknown @0; + parityPassed @1; + parityRebuilt @2; + } + } +} + +struct QcomGnss @0xde94674b07ae51c1 { + logTs @0 :UInt64; + union { + measurementReport @1 :MeasurementReport; + clockReport @2 :ClockReport; + drMeasurementReport @3 :DrMeasurementReport; + drSvPoly @4 :DrSvPolyReport; + rawLog @5 :Data; + } + + enum MeasurementSource @0xd71a12b6faada7ee { + gps @0; + glonass @1; + beidou @2; + } + + enum SVObservationState @0xe81e829a0d6c83e9 { + idle @0; + search @1; + searchVerify @2; + bitEdge @3; + trackVerify @4; + track @5; + restart @6; + dpo @7; + glo10msBe @8; + glo10msAt @9; + } + + struct MeasurementStatus @0xe501010e1bcae83b { + subMillisecondIsValid @0 :Bool; + subBitTimeIsKnown @1 :Bool; + satelliteTimeIsKnown @2 :Bool; + bitEdgeConfirmedFromSignal @3 :Bool; + measuredVelocity @4 :Bool; + fineOrCoarseVelocity @5 :Bool; + lockPointValid @6 :Bool; + lockPointPositive @7 :Bool; + lastUpdateFromDifference @8 :Bool; + lastUpdateFromVelocityDifference @9 :Bool; + strongIndicationOfCrossCorelation @10 :Bool; + tentativeMeasurement @11 :Bool; + measurementNotUsable @12 :Bool; + sirCheckIsNeeded @13 :Bool; + probationMode @14 :Bool; + + glonassMeanderBitEdgeValid @15 :Bool; + glonassTimeMarkValid @16 :Bool; + + gpsRoundRobinRxDiversity @17 :Bool; + gpsRxDiversity @18 :Bool; + gpsLowBandwidthRxDiversityCombined @19 :Bool; + gpsHighBandwidthNu4 @20 :Bool; + gpsHighBandwidthNu8 @21 :Bool; + gpsHighBandwidthUniform @22 :Bool; + multipathIndicator @23 :Bool; + + imdJammingIndicator @24 :Bool; + lteB13TxJammingIndicator @25 :Bool; + freshMeasurementIndicator @26 :Bool; + + multipathEstimateIsValid @27 :Bool; + directionIsValid @28 :Bool; + } + + struct MeasurementReport @0xf580d7d86b7b8692 { + source @0 :MeasurementSource; + + fCount @1 :UInt32; + + gpsWeek @2 :UInt16; + glonassCycleNumber @3 :UInt8; + glonassNumberOfDays @4 :UInt16; + + milliseconds @5 :UInt32; + timeBias @6 :Float32; + clockTimeUncertainty @7 :Float32; + clockFrequencyBias @8 :Float32; + clockFrequencyUncertainty @9 :Float32; + + sv @10 :List(SV); + + struct SV @0xf10c595ae7bb2c27 { + svId @0 :UInt8; + observationState @2 :SVObservationState; + observations @3 :UInt8; + goodObservations @4 :UInt8; + gpsParityErrorCount @5 :UInt16; + glonassFrequencyIndex @1 :Int8; + glonassHemmingErrorCount @6 :UInt8; + filterStages @7 :UInt8; + carrierNoise @8 :UInt16; + latency @9 :Int16; + predetectInterval @10 :UInt8; + postdetections @11 :UInt16; + + unfilteredMeasurementIntegral @12 :UInt32; + unfilteredMeasurementFraction @13 :Float32; + unfilteredTimeUncertainty @14 :Float32; + unfilteredSpeed @15 :Float32; + unfilteredSpeedUncertainty @16 :Float32; + measurementStatus @17 :MeasurementStatus; + multipathEstimate @18 :UInt32; + azimuth @19 :Float32; + elevation @20 :Float32; + carrierPhaseCyclesIntegral @21 :Int32; + carrierPhaseCyclesFraction @22 :UInt16; + fineSpeed @23 :Float32; + fineSpeedUncertainty @24 :Float32; + cycleSlipCount @25 :UInt8; + } + + } + + struct ClockReport @0xca965e4add8f4f0b { + hasFCount @0 :Bool; + fCount @1 :UInt32; + + hasGpsWeek @2 :Bool; + gpsWeek @3 :UInt16; + hasGpsMilliseconds @4 :Bool; + gpsMilliseconds @5 :UInt32; + gpsTimeBias @6 :Float32; + gpsClockTimeUncertainty @7 :Float32; + gpsClockSource @8 :UInt8; + + hasGlonassYear @9 :Bool; + glonassYear @10 :UInt8; + hasGlonassDay @11 :Bool; + glonassDay @12 :UInt16; + hasGlonassMilliseconds @13 :Bool; + glonassMilliseconds @14 :UInt32; + glonassTimeBias @15 :Float32; + glonassClockTimeUncertainty @16 :Float32; + glonassClockSource @17 :UInt8; + + bdsWeek @18 :UInt16; + bdsMilliseconds @19 :UInt32; + bdsTimeBias @20 :Float32; + bdsClockTimeUncertainty @21 :Float32; + bdsClockSource @22 :UInt8; + + galWeek @23 :UInt16; + galMilliseconds @24 :UInt32; + galTimeBias @25 :Float32; + galClockTimeUncertainty @26 :Float32; + galClockSource @27 :UInt8; + + clockFrequencyBias @28 :Float32; + clockFrequencyUncertainty @29 :Float32; + frequencySource @30 :UInt8; + gpsLeapSeconds @31 :UInt8; + gpsLeapSecondsUncertainty @32 :UInt8; + gpsLeapSecondsSource @33 :UInt8; + + gpsToGlonassTimeBiasMilliseconds @34 :Float32; + gpsToGlonassTimeBiasMillisecondsUncertainty @35 :Float32; + gpsToBdsTimeBiasMilliseconds @36 :Float32; + gpsToBdsTimeBiasMillisecondsUncertainty @37 :Float32; + bdsToGloTimeBiasMilliseconds @38 :Float32; + bdsToGloTimeBiasMillisecondsUncertainty @39 :Float32; + gpsToGalTimeBiasMilliseconds @40 :Float32; + gpsToGalTimeBiasMillisecondsUncertainty @41 :Float32; + galToGloTimeBiasMilliseconds @42 :Float32; + galToGloTimeBiasMillisecondsUncertainty @43 :Float32; + galToBdsTimeBiasMilliseconds @44 :Float32; + galToBdsTimeBiasMillisecondsUncertainty @45 :Float32; + + hasRtcTime @46 :Bool; + systemRtcTime @47 :UInt32; + fCountOffset @48 :UInt32; + lpmRtcCount @49 :UInt32; + clockResets @50 :UInt32; + } + + struct DrMeasurementReport @0x8053c39445c6c75c { + + reason @0 :UInt8; + seqNum @1 :UInt8; + seqMax @2 :UInt8; + rfLoss @3 :UInt16; + + systemRtcValid @4 :Bool; + fCount @5 :UInt32; + clockResets @6 :UInt32; + systemRtcTime @7 :UInt64; + + gpsLeapSeconds @8 :UInt8; + gpsLeapSecondsUncertainty @9 :UInt8; + gpsToGlonassTimeBiasMilliseconds @10 :Float32; + gpsToGlonassTimeBiasMillisecondsUncertainty @11 :Float32; + + gpsWeek @12 :UInt16; + gpsMilliseconds @13 :UInt32; + gpsTimeBiasMs @14 :UInt32; + gpsClockTimeUncertaintyMs @15 :UInt32; + gpsClockSource @16 :UInt8; + + glonassClockSource @17 :UInt8; + glonassYear @18 :UInt8; + glonassDay @19 :UInt16; + glonassMilliseconds @20 :UInt32; + glonassTimeBias @21 :Float32; + glonassClockTimeUncertainty @22 :Float32; + + clockFrequencyBias @23 :Float32; + clockFrequencyUncertainty @24 :Float32; + frequencySource @25 :UInt8; + + source @26 :MeasurementSource; + + sv @27 :List(SV); + + struct SV @0xf08b81df8cbf459c { + svId @0 :UInt8; + glonassFrequencyIndex @1 :Int8; + observationState @2 :SVObservationState; + observations @3 :UInt8; + goodObservations @4 :UInt8; + filterStages @5 :UInt8; + predetectInterval @6 :UInt8; + cycleSlipCount @7 :UInt8; + postdetections @8 :UInt16; + + measurementStatus @9 :MeasurementStatus; + + carrierNoise @10 :UInt16; + rfLoss @11 :UInt16; + latency @12 :Int16; + + filteredMeasurementFraction @13 :Float32; + filteredMeasurementIntegral @14 :UInt32; + filteredTimeUncertainty @15 :Float32; + filteredSpeed @16 :Float32; + filteredSpeedUncertainty @17 :Float32; + + unfilteredMeasurementFraction @18 :Float32; + unfilteredMeasurementIntegral @19 :UInt32; + unfilteredTimeUncertainty @20 :Float32; + unfilteredSpeed @21 :Float32; + unfilteredSpeedUncertainty @22 :Float32; + + multipathEstimate @23 :UInt32; + azimuth @24 :Float32; + elevation @25 :Float32; + dopplerAcceleration @26 :Float32; + fineSpeed @27 :Float32; + fineSpeedUncertainty @28 :Float32; + + carrierPhase @29 :Float64; + fCount @30 :UInt32; + + parityErrorCount @31 :UInt16; + goodParity @32 :Bool; + } + } + + struct DrSvPolyReport @0xb1fb80811a673270 { + svId @0 :UInt16; + frequencyIndex @1 :Int8; + + hasPosition @2 :Bool; + hasIono @3 :Bool; + hasTropo @4 :Bool; + hasElevation @5 :Bool; + polyFromXtra @6 :Bool; + hasSbasIono @7 :Bool; + + iode @8 :UInt16; + t0 @9 :Float64; + xyz0 @10 :List(Float64); + xyzN @11 :List(Float64); + other @12 :List(Float32); + + positionUncertainty @13 :Float32; + ionoDelay @14 :Float32; + ionoDot @15 :Float32; + sbasIonoDelay @16 :Float32; + sbasIonoDot @17 :Float32; + tropoDelay @18 :Float32; + elevation @19 :Float32; + elevationDot @20 :Float32; + elevationUncertainty @21 :Float32; + velocityCoeff @22 :List(Float64); + } +} + +struct LidarPts @0xe3d6685d4e9d8f7a { + r @0 :List(UInt16); # uint16 m*500.0 + theta @1 :List(UInt16); # uint16 deg*100.0 + reflect @2 :List(UInt8); # uint8 0-255 + + # For storing out of file. + idx @3 :UInt64; + + # For storing in file + pkt @4 :Data; +} + + diff --git a/cereal/log.capnp b/cereal/log.capnp index 11f49fa12..e717de08d 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -6,6 +6,7 @@ $Java.package("ai.comma.openpilot.cereal"); $Java.outerClassname("Log"); using Car = import "car.capnp"; +using Legacy = import "legacy.capnp"; @0xf3b1f17e25a4285b; @@ -32,11 +33,7 @@ struct InitData { gitBranch @11 :Text; gitRemote @13 :Text; - androidBuildInfo @5 :AndroidBuildInfo; - androidSensors @6 :List(AndroidSensor); androidProperties @16 :Map(Text, Text); - chffrAndroidExtra @7 :ChffrAndroidExtra; - iosBuildInfo @14 :IosBuildInfo; pandaInfo @8 :PandaInfo; @@ -53,6 +50,19 @@ struct InitData { pc @5; } + struct PandaInfo { + hasPanda @0 :Bool; + dongleId @1 :Text; + stVersion @2 :Text; + espVersion @3 :Text; + } + + # ***** deprecated stuff ***** + androidBuildInfo @5 :AndroidBuildInfo; + androidSensorsDEPRECATED @6 :List(AndroidSensor); + chffrAndroidExtraDEPRECATED @7 :ChffrAndroidExtra; + iosBuildInfoDEPRECATED @14 :IosBuildInfo; + struct AndroidBuildInfo { board @0 :Text; bootloader @1 :Text; @@ -107,13 +117,6 @@ struct InitData { osVersion @2 :Text; deviceModel @3 :Text; } - - struct PandaInfo { - hasPanda @0 :Bool; - dongleId @1 :Text; - stVersion @2 :Text; - espVersion @3 :Text; - } } struct FrameData { @@ -229,7 +232,7 @@ struct GpsLocationData { speed @4 :Float32; # Represents heading in degrees. - bearing @5 :Float32; + bearingDeg @5 :Float32; # Represents expected accuracy in meters. (presumably 1 sigma?) accuracy @6 :Float32; @@ -247,7 +250,7 @@ struct GpsLocationData { verticalAccuracy @10 :Float32; # Represents bearing accuracy in degrees. (presumably 1 sigma?) - bearingAccuracy @11 :Float32; + bearingAccuracyDeg @11 :Float32; # Represents velocity accuracy in m/s. (presumably 1 sigma?) speedAccuracy @12 :Float32; @@ -271,51 +274,41 @@ struct CanData { src @3 :UInt8; } -struct ThermalData { - # Deprecated - cpu0DEPRECATED @0 :UInt16; - cpu1DEPRECATED @1 :UInt16; - cpu2DEPRECATED @2 :UInt16; - cpu3DEPRECATED @3 :UInt16; - memDEPRECATED @4 :UInt16; - gpuDEPRECATED @5 :UInt16; - batDEPRECATED @6 :UInt32; - pa0DEPRECATED @21 :UInt16; +struct DeviceState @0xa4d8b5af2aa492eb { + freeSpacePercent @7 :Float32; + memoryUsagePercent @19 :Int8; + cpuUsagePercent @20 :Int8; + usbOnline @12 :Bool; + networkType @22 :NetworkType; + offroadPowerUsageUwh @23 :UInt32; + networkStrength @24 :NetworkStrength; + carBatteryCapacityUwh @25 :UInt32; - # not thermal - freeSpace @7 :Float32; + fanSpeedPercentDesired @10 :UInt16; + started @11 :Bool; + startedMonoTime @13 :UInt64; + + # power batteryPercent @8 :Int16; batteryStatus @9 :Text; batteryCurrent @15 :Int32; batteryVoltage @16 :Int32; - usbOnline @12 :Bool; - networkType @22 :NetworkType; - offroadPowerUsage @23 :UInt32; # Power usage since going offroad in uWh - networkStrength @24 :NetworkStrength; - carBatteryCapacity @25 :UInt32; # Estimated remaining car battery capacity in uWh - - fanSpeed @10 :UInt16; - started @11 :Bool; - startedTs @13 :UInt64; - - thermalStatus @14 :ThermalStatus; chargingError @17 :Bool; chargingDisabled @18 :Bool; - memUsedPercent @19 :Int8; - cpuPerc @20 :Int8; - - cpu @26 :List(Float32); - gpu @27 :List(Float32); - mem @28 :Float32; - bat @29 :Float32; - ambient @30 :Float32; + # device thermals + cpuTempC @26 :List(Float32); + gpuTempC @27 :List(Float32); + memoryTempC @28 :Float32; + batteryTempC @29 :Float32; + ambientTempC @30 :Float32; + thermalStatus @14 :ThermalStatus; enum ThermalStatus { - green @0; # all processes run - yellow @1; # critical processes run (kill uploader), engage still allowed - red @2; # no engage, will disengage - danger @3; # immediate process shutdown + green @0; + yellow @1; + red @2; + danger @3; } enum NetworkType { @@ -334,22 +327,31 @@ struct ThermalData { good @3; great @4; } + + # deprecated + cpu0DEPRECATED @0 :UInt16; + cpu1DEPRECATED @1 :UInt16; + cpu2DEPRECATED @2 :UInt16; + cpu3DEPRECATED @3 :UInt16; + memDEPRECATED @4 :UInt16; + gpuDEPRECATED @5 :UInt16; + batDEPRECATED @6 :UInt32; + pa0DEPRECATED @21 :UInt16; } -struct HealthData { +struct PandaState @0xa7649e2575e4591e { # from can health voltage @0 :UInt32; current @1 :UInt32; ignitionLine @2 :Bool; controlsAllowed @3 :Bool; gasInterceptorDetected @4 :Bool; - startedSignalDetectedDeprecated @5 :Bool; hasGps @6 :Bool; canSendErrs @7 :UInt32; canFwdErrs @8 :UInt32; canRxErrs @19 :UInt32; gmlanSendErrs @9 :UInt32; - hwType @10 :HwType; + pandaType @10 :PandaType; fanSpeedRpm @11 :UInt16; usbPowerMode @12 :UsbPowerMode; ignitionCan @13 :Bool; @@ -391,7 +393,7 @@ struct HealthData { # Update max fault type in boardd when adding faults } - enum HwType { + enum PandaType @0x8a58adf93e5b3751 { unknown @0; whitePanda @1; greyPanda @2; @@ -407,29 +409,16 @@ struct HealthData { cdp @2; dcp @3; } -} -struct LiveUI { - rearViewCam @0 :Bool; - alertText1 @1 :Text; - alertText2 @2 :Text; - awarenessStatus @3 :Float32; + startedSignalDetectedDEPRECATED @5 :Bool; } struct RadarState @0x9a185389d6fdd05f { canMonoTimes @10 :List(UInt64); mdMonoTime @6 :UInt64; - ftMonoTimeDEPRECATED @7 :UInt64; - controlsStateMonoTime @11 :UInt64; + carStateMonoTime @11 :UInt64; radarErrors @12 :List(Car.RadarData.Error); - # all deprecated - warpMatrixDEPRECATED @0 :List(Float32); - angleOffsetDEPRECATED @1 :Float32; - calStatusDEPRECATED @2 :Int8; - calCycleDEPRECATED @8 :Int32; - calPercDEPRECATED @9 :Int8; - leadOne @3 :LeadData; leadTwo @4 :LeadData; cumLagMs @5 :Float32; @@ -440,7 +429,6 @@ struct RadarState @0x9a185389d6fdd05f { vRel @2 :Float32; aRel @3 :Float32; vLead @4 :Float32; - aLeadDEPRECATED @5 :Float32; dPath @6 :Float32; vLat @7 :Float32; vLeadK @8 :Float32; @@ -450,17 +438,20 @@ struct RadarState @0x9a185389d6fdd05f { aLeadTau @12 :Float32; modelProb @13 :Float32; radar @14 :Bool; + + aLeadDEPRECATED @5 :Float32; } + + # deprecated + ftMonoTimeDEPRECATED @7 :UInt64; + warpMatrixDEPRECATED @0 :List(Float32); + angleOffsetDEPRECATED @1 :Float32; + calStatusDEPRECATED @2 :Int8; + calCycleDEPRECATED @8 :Int32; + calPercDEPRECATED @9 :Int8; } struct LiveCalibrationData { - # deprecated - warpMatrix @0 :List(Float32); - - # camera_frame_from_model_frame - warpMatrix2 @5 :List(Float32); - warpMatrixBig @6 :List(Float32); - calStatus @1 :Int8; calCycle @2 :Int32; calPerc @3 :Int8; @@ -472,6 +463,10 @@ struct LiveCalibrationData { # the direction of travel vector in device frame rpyCalib @7 :List(Float32); rpyCalibSpread @8 :List(Float32); + + warpMatrixDEPRECATED @0 :List(Float32); + warpMatrix2DEPRECATED @5 :List(Float32); + warpMatrixBigDEPRECATED @6 :List(Float32); } struct LiveTracks { @@ -488,67 +483,38 @@ struct LiveTracks { } struct ControlsState @0x97ff69c53601abf1 { - canMonoTimeDEPRECATED @16 :UInt64; + startMonoTime @48 :UInt64; canMonoTimes @21 :List(UInt64); - radarStateMonoTimeDEPRECATED @17 :UInt64; - mdMonoTimeDEPRECATED @18 :UInt64; - planMonoTime @28 :UInt64; - pathPlanMonoTime @50 :UInt64; + longitudinalPlanMonoTime @28 :UInt64; + lateralPlanMonoTime @50 :UInt64; state @31 :OpenpilotState; - vEgo @0 :Float32; - vEgoRaw @32 :Float32; - aEgoDEPRECATED @1 :Float32; + enabled @19 :Bool; + active @36 :Bool; + longControlState @30 :LongControlState; vPid @2 :Float32; vTargetLead @3 :Float32; + vCruise @22 :Float32; upAccelCmd @4 :Float32; uiAccelCmd @5 :Float32; ufAccelCmd @33 :Float32; - yActualDEPRECATED @6 :Float32; - yDesDEPRECATED @7 :Float32; - upSteerDEPRECATED @8 :Float32; - uiSteerDEPRECATED @9 :Float32; - ufSteerDEPRECATED @34 :Float32; - aTargetMinDEPRECATED @10 :Float32; - aTargetMaxDEPRECATED @11 :Float32; aTarget @35 :Float32; - jerkFactor @12 :Float32; - angleSteers @13 :Float32; # Steering angle in degrees. - angleSteersDes @29 :Float32; - curvature @37 :Float32; # path curvature from vehicle model - hudLeadDEPRECATED @14 :Int32; - cumLagMs @15 :Float32; - startMonoTime @48 :UInt64; - mapValid @49 :Bool; + steeringAngleDesiredDeg @29 :Float32; + curvature @37 :Float32; # path curvature from vehicle model forceDecel @51 :Bool; - enabled @19 :Bool; - active @36 :Bool; - steerOverride @20 :Bool; - - vCruise @22 :Float32; - - rearViewCam @23 :Bool; + # UI alerts alertText1 @24 :Text; alertText2 @25 :Text; alertStatus @38 :AlertStatus; alertSize @39 :AlertSize; alertBlinkingRate @42 :Float32; alertType @44 :Text; - alertSoundDEPRECATED @45 :Text; alertSound @56 :Car.CarControl.HUDControl.AudibleAlert; - awarenessStatus @26 :Float32; - angleModelBiasDEPRECATED @27 :Float32; - gpsPlannerActive @40 :Bool; engageable @41 :Bool; # can OP be engaged? - driverMonitoringOn @43 :Bool; - # maps - vCurvature @46 :Float32; - decelForTurn @47 :Bool; - - decelForModel @54 :Bool; + cumLagMs @15 :Float32; canErrorCounter @57 :UInt32; lateralControlState :union { @@ -586,9 +552,9 @@ struct ControlsState @0x97ff69c53601abf1 { struct LateralINDIState { active @0 :Bool; - steerAngle @1 :Float32; - steerRate @2 :Float32; - steerAccel @3 :Float32; + steeringAngleDeg @1 :Float32; + steeringRateDeg @2 :Float32; + steeringAccelDeg @3 :Float32; rateSetPoint @4 :Float32; accelSetPoint @5 :Float32; accelError @6 :Float32; @@ -600,8 +566,8 @@ struct ControlsState @0x97ff69c53601abf1 { struct LateralPIDState { active @0 :Bool; - steerAngle @1 :Float32; - steerRate @2 :Float32; + steeringAngleDeg @1 :Float32; + steeringRateDeg @2 :Float32; angleError @3 :Float32; p @4 :Float32; i @5 :Float32; @@ -612,88 +578,43 @@ struct ControlsState @0x97ff69c53601abf1 { struct LateralLQRState { active @0 :Bool; - steerAngle @1 :Float32; + steeringAngleDeg @1 :Float32; i @2 :Float32; output @3 :Float32; lqrOutput @4 :Float32; saturated @5 :Bool; } + + # deprecated + vEgoDEPRECATED @0 :Float32; + vEgoRawDEPRECATED @32 :Float32; + aEgoDEPRECATED @1 :Float32; + canMonoTimeDEPRECATED @16 :UInt64; + radarStateMonoTimeDEPRECATED @17 :UInt64; + mdMonoTimeDEPRECATED @18 :UInt64; + yActualDEPRECATED @6 :Float32; + yDesDEPRECATED @7 :Float32; + upSteerDEPRECATED @8 :Float32; + uiSteerDEPRECATED @9 :Float32; + ufSteerDEPRECATED @34 :Float32; + aTargetMinDEPRECATED @10 :Float32; + aTargetMaxDEPRECATED @11 :Float32; + rearViewCamDEPRECATED @23 :Bool; + driverMonitoringOnDEPRECATED @43 :Bool; + hudLeadDEPRECATED @14 :Int32; + alertSoundDEPRECATED @45 :Text; + angleModelBiasDEPRECATED @27 :Float32; + gpsPlannerActiveDEPRECATED @40 :Bool; + decelForTurnDEPRECATED @47 :Bool; + decelForModelDEPRECATED @54 :Bool; + awarenessStatusDEPRECATED @26 :Float32; + angleSteersDEPRECATED @13 :Float32; + vCurvatureDEPRECATED @46 :Float32; + mapValidDEPRECATED @49 :Bool; + jerkFactorDEPRECATED @12 :Float32; + steerOverrideDEPRECATED @20 :Bool; } -struct LiveEventData { - name @0 :Text; - value @1 :Int32; -} - -struct ModelData { - frameId @0 :UInt32; - frameAge @12 :UInt32; - frameDropPerc @13 :Float32; - timestampEof @9 :UInt64; - modelExecutionTime @14 :Float32; - gpuExecutionTime @16 :Float32; - rawPred @15 :Data; - - path @1 :PathData; - leftLane @2 :PathData; - rightLane @3 :PathData; - lead @4 :LeadData; - freePath @6 :List(Float32); - - settings @5 :ModelSettings; - leadFuture @7 :LeadData; - speed @8 :List(Float32); - meta @10 :MetaData; - longitudinal @11 :LongitudinalData; - - struct PathData { - points @0 :List(Float32); - prob @1 :Float32; - std @2 :Float32; - stds @3 :List(Float32); - poly @4 :List(Float32); - validLen @5 :Float32; - } - - struct LeadData { - dist @0 :Float32; - prob @1 :Float32; - std @2 :Float32; - relVel @3 :Float32; - relVelStd @4 :Float32; - relY @5 :Float32; - relYStd @6 :Float32; - relA @7 :Float32; - relAStd @8 :Float32; - } - - struct ModelSettings { - bigBoxX @0 :UInt16; - bigBoxY @1 :UInt16; - bigBoxWidth @2 :UInt16; - bigBoxHeight @3 :UInt16; - boxProjection @4 :List(Float32); - yuvCorrection @5 :List(Float32); - inputTransform @6 :List(Float32); - } - - struct MetaData { - engagedProb @0 :Float32; - desirePrediction @1 :List(Float32); - brakeDisengageProb @2 :Float32; - gasDisengageProb @3 :Float32; - steerOverrideProb @4 :Float32; - desireState @5 :List(Float32); - } - - struct LongitudinalData { - distances @2 :List(Float32); - speeds @0 :List(Float32); - accelerations @1 :List(Float32); - } -} - - struct ModelDataV2 { frameId @0 :UInt32; frameAge @1 :UInt32; @@ -701,21 +622,27 @@ struct ModelDataV2 { timestampEof @3 :UInt64; modelExecutionTime @15 :Float32; gpuExecutionTime @17 :Float32; - rawPred @16 :Data; + rawPredictions @16 :Data; + # predicted future position, orientation, etc.. position @4 :XYZTData; orientation @5 :XYZTData; velocity @6 :XYZTData; orientationRate @7 :XYZTData; + + # prediction lanelines and road edges laneLines @8 :List(XYZTData); laneLineProbs @9 :List(Float32); laneLineStds @13 :List(Float32); roadEdges @10 :List(XYZTData); roadEdgeStds @14 :List(Float32); + + # predicted lead cars leads @11 :List(LeadDataV2); meta @12 :MetaData; + # All SI units and in device frame struct XYZTData { x @0 :List(Float32); y @1 :List(Float32); @@ -727,8 +654,12 @@ struct ModelDataV2 { } struct LeadDataV2 { - prob @0 :Float32; + prob @0 :Float32; # probability that car is your lead at time t t @1 :Float32; + + # x and y are relative position in device frame + # v is norm relative speed + # a is norm relative acceleration xyva @2 :List(Float32); xyvaStd @3 :List(Float32); } @@ -743,15 +674,6 @@ struct ModelDataV2 { } } - -struct CalibrationFeatures { - frameId @0 :UInt32; - - p0 @1 :List(Float32); - p1 @2 :List(Float32); - status @3 :List(Int8); -} - struct EncodeIndex { # picture from camera frameId @0 :UInt32; @@ -787,64 +709,26 @@ struct AndroidLogEntry { message @6 :Text; } -struct LogRotate { - segmentNum @0 :Int32; - path @1 :Text; -} - -struct Plan { +struct LongitudinalPlan @0xe00b5b3eba12876c { mdMonoTime @9 :UInt64; radarStateMonoTime @10 :UInt64; - commIssue @31 :Bool; - eventsDEPRECATED @13 :List(Car.CarEvent); - - # lateral, 3rd order polynomial - lateralValidDEPRECATED @0 :Bool; - dPolyDEPRECATED @1 :List(Float32); - laneWidthDEPRECATED @11 :Float32; - - # longitudinal - longitudinalValidDEPRECATED @2 :Bool; vCruise @16 :Float32; aCruise @17 :Float32; vTarget @3 :Float32; vTargetFuture @14 :Float32; vMax @20 :Float32; - aTargetMinDEPRECATED @4 :Float32; - aTargetMaxDEPRECATED @5 :Float32; aTarget @18 :Float32; vStart @26 :Float32; aStart @27 :Float32; - jerkFactor @6 :Float32; hasLead @7 :Bool; - hasLeftLaneDEPRECATED @23 :Bool; - hasRightLaneDEPRECATED @24 :Bool; fcw @8 :Bool; longitudinalPlanSource @15 :LongitudinalPlanSource; - # gps trajectory in car frame - gpsTrajectory @12 :GpsTrajectory; - - gpsPlannerActive @19 :Bool; - - # maps - vCurvature @21 :Float32; - decelForTurn @22 :Bool; - mapValid @25 :Bool; - radarValid @28 :Bool; - radarCanError @30 :Bool; - processingDelay @29 :Float32; - - struct GpsTrajectory { - x @0 :List(Float32); - y @1 :List(Float32); - } - enum LongitudinalPlanSource { cruise @0; mpc1 @1; @@ -852,28 +736,44 @@ struct Plan { mpc3 @3; model @4; } + + # deprecated + jerkFactorDEPRECATED @6 :Float32; + hasLeftLaneDEPRECATED @23 :Bool; + hasRightLaneDEPRECATED @24 :Bool; + aTargetMinDEPRECATED @4 :Float32; + aTargetMaxDEPRECATED @5 :Float32; + lateralValidDEPRECATED @0 :Bool; + longitudinalValidDEPRECATED @2 :Bool; + dPolyDEPRECATED @1 :List(Float32); + laneWidthDEPRECATED @11 :Float32; + vCurvatureDEPRECATED @21 :Float32; + decelForTurnDEPRECATED @22 :Bool; + mapValidDEPRECATED @25 :Bool; + radarValidDEPRECATED @28 :Bool; + radarCanErrorDEPRECATED @30 :Bool; + commIssueDEPRECATED @31 :Bool; + eventsDEPRECATED @13 :List(Car.CarEvent); + gpsTrajectoryDEPRECATED @12 :GpsTrajectory; + gpsPlannerActiveDEPRECATED @19 :Bool; + + struct GpsTrajectory { + x @0 :List(Float32); + y @1 :List(Float32); + } } -struct PathPlan { +struct LateralPlan @0xe1e9318e2ae8b51e { laneWidth @0 :Float32; - - dPoly @1 :List(Float32); - cPoly @2 :List(Float32); - cProb @3 :Float32; - lPoly @4 :List(Float32); lProb @5 :Float32; - rPoly @6 :List(Float32); rProb @7 :Float32; + dPathPoints @20 :List(Float32); + dProb @21 :Float32; - angleSteers @8 :Float32; # deg - rateSteers @13 :Float32; # deg/s + steeringAngleDeg @8 :Float32; # deg + steeringRateDeg @13 :Float32; # deg/s mpcSolutionValid @9 :Bool; - paramsValid @10 :Bool; - modelValidDEPRECATED @12 :Bool; - angleOffset @11 :Float32; - sensorValid @14 :Bool; - commIssue @15 :Bool; - posenetValid @16 :Bool; + angleOffsetDeg @11 :Float32; desire @17 :Desire; laneChangeState @18 :LaneChangeState; laneChangeDirection @19 :LaneChangeDirection; @@ -900,6 +800,18 @@ struct PathPlan { left @1; right @2; } + + # deprecated + cProbDEPRECATED @3 :Float32; + dPolyDEPRECATED @1 :List(Float32); + cPolyDEPRECATED @2 :List(Float32); + lPolyDEPRECATED @4 :List(Float32); + rPolyDEPRECATED @6 :List(Float32); + modelValidDEPRECATED @12 :Bool; + commIssueDEPRECATED @15 :Bool; + posenetValidDEPRECATED @16 :Bool; + sensorValidDEPRECATED @14 :Bool; + paramsValidDEPRECATED @10 :Bool; } struct LiveLocationKalman { @@ -954,591 +866,6 @@ struct LiveLocationKalman { } } -struct LiveLocationData { - status @0 :UInt8; - - # 3D fix - lat @1 :Float64; - lon @2 :Float64; - alt @3 :Float32; # m - - # speed - speed @4 :Float32; # m/s - - # NED velocity components - vNED @5 :List(Float32); - - # roll, pitch, heading (x,y,z) - roll @6 :Float32; # WRT to center of earth? - pitch @7 :Float32; # WRT to center of earth? - heading @8 :Float32; # WRT to north? - - # what are these? - wanderAngle @9 :Float32; - trackAngle @10 :Float32; - - # car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png - - # gyro, in car frame, deg/s - gyro @11 :List(Float32); - - # accel, in car frame, m/s^2 - accel @12 :List(Float32); - - accuracy @13 :Accuracy; - - source @14 :SensorSource; - # if we are fixing a location in the past - fixMonoTime @15 :UInt64; - - gpsWeek @16 :Int32; - timeOfWeek @17 :Float64; - - positionECEF @18 :List(Float64); - poseQuatECEF @19 :List(Float32); - pitchCalibration @20 :Float32; - yawCalibration @21 :Float32; - imuFrame @22 :List(Float32); - - struct Accuracy { - pNEDError @0 :List(Float32); - vNEDError @1 :List(Float32); - rollError @2 :Float32; - pitchError @3 :Float32; - headingError @4 :Float32; - ellipsoidSemiMajorError @5 :Float32; - ellipsoidSemiMinorError @6 :Float32; - ellipsoidOrientationError @7 :Float32; - } - - enum SensorSource { - applanix @0; - kalman @1; - orbslam @2; - timing @3; - dummy @4; - } -} - -struct EthernetPacket { - pkt @0 :Data; - ts @1 :Float32; -} - -struct NavUpdate { - isNavigating @0 :Bool; - curSegment @1 :Int32; - segments @2 :List(Segment); - - struct LatLng { - lat @0 :Float64; - lng @1 :Float64; - } - - struct Segment { - from @0 :LatLng; - to @1 :LatLng; - updateTime @2 :Int32; - distance @3 :Int32; - crossTime @4 :Int32; - exitNo @5 :Int32; - instruction @6 :Instruction; - - parts @7 :List(LatLng); - - enum Instruction { - turnLeft @0; - turnRight @1; - keepLeft @2; - keepRight @3; - straight @4; - roundaboutExitNumber @5; - roundaboutExit @6; - roundaboutTurnLeft @7; - unkn8 @8; - roundaboutStraight @9; - unkn10 @10; - roundaboutTurnRight @11; - unkn12 @12; - roundaboutUturn @13; - unkn14 @14; - arrive @15; - exitLeft @16; - exitRight @17; - unkn18 @18; - uturn @19; - # ... - } - } -} - -struct NavStatus { - isNavigating @0 :Bool; - currentAddress @1 :Address; - - struct Address { - title @0 :Text; - lat @1 :Float64; - lng @2 :Float64; - house @3 :Text; - address @4 :Text; - street @5 :Text; - city @6 :Text; - state @7 :Text; - country @8 :Text; - } -} - -struct CellInfo { - timestamp @0 :UInt64; - repr @1 :Text; # android toString() for now -} - -struct WifiScan { - bssid @0 :Text; - ssid @1 :Text; - capabilities @2 :Text; - frequency @3 :Int32; - level @4 :Int32; - timestamp @5 :Int64; - - centerFreq0 @6 :Int32; - centerFreq1 @7 :Int32; - channelWidth @8 :ChannelWidth; - operatorFriendlyName @9 :Text; - venueName @10 :Text; - is80211mcResponder @11 :Bool; - passpoint @12 :Bool; - - distanceCm @13 :Int32; - distanceSdCm @14 :Int32; - - enum ChannelWidth { - w20Mhz @0; - w40Mhz @1; - w80Mhz @2; - w160Mhz @3; - w80Plus80Mhz @4; - } -} - -struct AndroidGnss { - union { - measurements @0 :Measurements; - navigationMessage @1 :NavigationMessage; - } - - struct Measurements { - clock @0 :Clock; - measurements @1 :List(Measurement); - - struct Clock { - timeNanos @0 :Int64; - hardwareClockDiscontinuityCount @1 :Int32; - - hasTimeUncertaintyNanos @2 :Bool; - timeUncertaintyNanos @3 :Float64; - - hasLeapSecond @4 :Bool; - leapSecond @5 :Int32; - - hasFullBiasNanos @6 :Bool; - fullBiasNanos @7 :Int64; - - hasBiasNanos @8 :Bool; - biasNanos @9 :Float64; - - hasBiasUncertaintyNanos @10 :Bool; - biasUncertaintyNanos @11 :Float64; - - hasDriftNanosPerSecond @12 :Bool; - driftNanosPerSecond @13 :Float64; - - hasDriftUncertaintyNanosPerSecond @14 :Bool; - driftUncertaintyNanosPerSecond @15 :Float64; - } - - struct Measurement { - svId @0 :Int32; - constellation @1 :Constellation; - - timeOffsetNanos @2 :Float64; - state @3 :Int32; - receivedSvTimeNanos @4 :Int64; - receivedSvTimeUncertaintyNanos @5 :Int64; - cn0DbHz @6 :Float64; - pseudorangeRateMetersPerSecond @7 :Float64; - pseudorangeRateUncertaintyMetersPerSecond @8 :Float64; - accumulatedDeltaRangeState @9 :Int32; - accumulatedDeltaRangeMeters @10 :Float64; - accumulatedDeltaRangeUncertaintyMeters @11 :Float64; - - hasCarrierFrequencyHz @12 :Bool; - carrierFrequencyHz @13 :Float32; - hasCarrierCycles @14 :Bool; - carrierCycles @15 :Int64; - hasCarrierPhase @16 :Bool; - carrierPhase @17 :Float64; - hasCarrierPhaseUncertainty @18 :Bool; - carrierPhaseUncertainty @19 :Float64; - hasSnrInDb @20 :Bool; - snrInDb @21 :Float64; - - multipathIndicator @22 :MultipathIndicator; - - enum Constellation { - unknown @0; - gps @1; - sbas @2; - glonass @3; - qzss @4; - beidou @5; - galileo @6; - } - - enum State { - unknown @0; - codeLock @1; - bitSync @2; - subframeSync @3; - towDecoded @4; - msecAmbiguous @5; - symbolSync @6; - gloStringSync @7; - gloTodDecoded @8; - bdsD2BitSync @9; - bdsD2SubframeSync @10; - galE1bcCodeLock @11; - galE1c2ndCodeLock @12; - galE1bPageSync @13; - sbasSync @14; - } - - enum MultipathIndicator { - unknown @0; - detected @1; - notDetected @2; - } - } - } - - struct NavigationMessage { - type @0 :Int32; - svId @1 :Int32; - messageId @2 :Int32; - submessageId @3 :Int32; - data @4 :Data; - status @5 :Status; - - enum Status { - unknown @0; - parityPassed @1; - parityRebuilt @2; - } - } -} - -struct QcomGnss { - logTs @0 :UInt64; - union { - measurementReport @1 :MeasurementReport; - clockReport @2 :ClockReport; - drMeasurementReport @3 :DrMeasurementReport; - drSvPoly @4 :DrSvPolyReport; - rawLog @5 :Data; - } - - enum MeasurementSource @0xd71a12b6faada7ee { - gps @0; - glonass @1; - beidou @2; - } - - enum SVObservationState @0xe81e829a0d6c83e9 { - idle @0; - search @1; - searchVerify @2; - bitEdge @3; - trackVerify @4; - track @5; - restart @6; - dpo @7; - glo10msBe @8; - glo10msAt @9; - } - - struct MeasurementStatus @0xe501010e1bcae83b { - subMillisecondIsValid @0 :Bool; - subBitTimeIsKnown @1 :Bool; - satelliteTimeIsKnown @2 :Bool; - bitEdgeConfirmedFromSignal @3 :Bool; - measuredVelocity @4 :Bool; - fineOrCoarseVelocity @5 :Bool; - lockPointValid @6 :Bool; - lockPointPositive @7 :Bool; - lastUpdateFromDifference @8 :Bool; - lastUpdateFromVelocityDifference @9 :Bool; - strongIndicationOfCrossCorelation @10 :Bool; - tentativeMeasurement @11 :Bool; - measurementNotUsable @12 :Bool; - sirCheckIsNeeded @13 :Bool; - probationMode @14 :Bool; - - glonassMeanderBitEdgeValid @15 :Bool; - glonassTimeMarkValid @16 :Bool; - - gpsRoundRobinRxDiversity @17 :Bool; - gpsRxDiversity @18 :Bool; - gpsLowBandwidthRxDiversityCombined @19 :Bool; - gpsHighBandwidthNu4 @20 :Bool; - gpsHighBandwidthNu8 @21 :Bool; - gpsHighBandwidthUniform @22 :Bool; - multipathIndicator @23 :Bool; - - imdJammingIndicator @24 :Bool; - lteB13TxJammingIndicator @25 :Bool; - freshMeasurementIndicator @26 :Bool; - - multipathEstimateIsValid @27 :Bool; - directionIsValid @28 :Bool; - } - - struct MeasurementReport { - source @0 :MeasurementSource; - - fCount @1 :UInt32; - - gpsWeek @2 :UInt16; - glonassCycleNumber @3 :UInt8; - glonassNumberOfDays @4 :UInt16; - - milliseconds @5 :UInt32; - timeBias @6 :Float32; - clockTimeUncertainty @7 :Float32; - clockFrequencyBias @8 :Float32; - clockFrequencyUncertainty @9 :Float32; - - sv @10 :List(SV); - - struct SV { - svId @0 :UInt8; - observationState @2 :SVObservationState; - observations @3 :UInt8; - goodObservations @4 :UInt8; - gpsParityErrorCount @5 :UInt16; - glonassFrequencyIndex @1 :Int8; - glonassHemmingErrorCount @6 :UInt8; - filterStages @7 :UInt8; - carrierNoise @8 :UInt16; - latency @9 :Int16; - predetectInterval @10 :UInt8; - postdetections @11 :UInt16; - - unfilteredMeasurementIntegral @12 :UInt32; - unfilteredMeasurementFraction @13 :Float32; - unfilteredTimeUncertainty @14 :Float32; - unfilteredSpeed @15 :Float32; - unfilteredSpeedUncertainty @16 :Float32; - measurementStatus @17 :MeasurementStatus; - multipathEstimate @18 :UInt32; - azimuth @19 :Float32; - elevation @20 :Float32; - carrierPhaseCyclesIntegral @21 :Int32; - carrierPhaseCyclesFraction @22 :UInt16; - fineSpeed @23 :Float32; - fineSpeedUncertainty @24 :Float32; - cycleSlipCount @25 :UInt8; - } - - } - - struct ClockReport { - hasFCount @0 :Bool; - fCount @1 :UInt32; - - hasGpsWeek @2 :Bool; - gpsWeek @3 :UInt16; - hasGpsMilliseconds @4 :Bool; - gpsMilliseconds @5 :UInt32; - gpsTimeBias @6 :Float32; - gpsClockTimeUncertainty @7 :Float32; - gpsClockSource @8 :UInt8; - - hasGlonassYear @9 :Bool; - glonassYear @10 :UInt8; - hasGlonassDay @11 :Bool; - glonassDay @12 :UInt16; - hasGlonassMilliseconds @13 :Bool; - glonassMilliseconds @14 :UInt32; - glonassTimeBias @15 :Float32; - glonassClockTimeUncertainty @16 :Float32; - glonassClockSource @17 :UInt8; - - bdsWeek @18 :UInt16; - bdsMilliseconds @19 :UInt32; - bdsTimeBias @20 :Float32; - bdsClockTimeUncertainty @21 :Float32; - bdsClockSource @22 :UInt8; - - galWeek @23 :UInt16; - galMilliseconds @24 :UInt32; - galTimeBias @25 :Float32; - galClockTimeUncertainty @26 :Float32; - galClockSource @27 :UInt8; - - clockFrequencyBias @28 :Float32; - clockFrequencyUncertainty @29 :Float32; - frequencySource @30 :UInt8; - gpsLeapSeconds @31 :UInt8; - gpsLeapSecondsUncertainty @32 :UInt8; - gpsLeapSecondsSource @33 :UInt8; - - gpsToGlonassTimeBiasMilliseconds @34 :Float32; - gpsToGlonassTimeBiasMillisecondsUncertainty @35 :Float32; - gpsToBdsTimeBiasMilliseconds @36 :Float32; - gpsToBdsTimeBiasMillisecondsUncertainty @37 :Float32; - bdsToGloTimeBiasMilliseconds @38 :Float32; - bdsToGloTimeBiasMillisecondsUncertainty @39 :Float32; - gpsToGalTimeBiasMilliseconds @40 :Float32; - gpsToGalTimeBiasMillisecondsUncertainty @41 :Float32; - galToGloTimeBiasMilliseconds @42 :Float32; - galToGloTimeBiasMillisecondsUncertainty @43 :Float32; - galToBdsTimeBiasMilliseconds @44 :Float32; - galToBdsTimeBiasMillisecondsUncertainty @45 :Float32; - - hasRtcTime @46 :Bool; - systemRtcTime @47 :UInt32; - fCountOffset @48 :UInt32; - lpmRtcCount @49 :UInt32; - clockResets @50 :UInt32; - } - - struct DrMeasurementReport { - - reason @0 :UInt8; - seqNum @1 :UInt8; - seqMax @2 :UInt8; - rfLoss @3 :UInt16; - - systemRtcValid @4 :Bool; - fCount @5 :UInt32; - clockResets @6 :UInt32; - systemRtcTime @7 :UInt64; - - gpsLeapSeconds @8 :UInt8; - gpsLeapSecondsUncertainty @9 :UInt8; - gpsToGlonassTimeBiasMilliseconds @10 :Float32; - gpsToGlonassTimeBiasMillisecondsUncertainty @11 :Float32; - - gpsWeek @12 :UInt16; - gpsMilliseconds @13 :UInt32; - gpsTimeBiasMs @14 :UInt32; - gpsClockTimeUncertaintyMs @15 :UInt32; - gpsClockSource @16 :UInt8; - - glonassClockSource @17 :UInt8; - glonassYear @18 :UInt8; - glonassDay @19 :UInt16; - glonassMilliseconds @20 :UInt32; - glonassTimeBias @21 :Float32; - glonassClockTimeUncertainty @22 :Float32; - - clockFrequencyBias @23 :Float32; - clockFrequencyUncertainty @24 :Float32; - frequencySource @25 :UInt8; - - source @26 :MeasurementSource; - - sv @27 :List(SV); - - struct SV { - svId @0 :UInt8; - glonassFrequencyIndex @1 :Int8; - observationState @2 :SVObservationState; - observations @3 :UInt8; - goodObservations @4 :UInt8; - filterStages @5 :UInt8; - predetectInterval @6 :UInt8; - cycleSlipCount @7 :UInt8; - postdetections @8 :UInt16; - - measurementStatus @9 :MeasurementStatus; - - carrierNoise @10 :UInt16; - rfLoss @11 :UInt16; - latency @12 :Int16; - - filteredMeasurementFraction @13 :Float32; - filteredMeasurementIntegral @14 :UInt32; - filteredTimeUncertainty @15 :Float32; - filteredSpeed @16 :Float32; - filteredSpeedUncertainty @17 :Float32; - - unfilteredMeasurementFraction @18 :Float32; - unfilteredMeasurementIntegral @19 :UInt32; - unfilteredTimeUncertainty @20 :Float32; - unfilteredSpeed @21 :Float32; - unfilteredSpeedUncertainty @22 :Float32; - - multipathEstimate @23 :UInt32; - azimuth @24 :Float32; - elevation @25 :Float32; - dopplerAcceleration @26 :Float32; - fineSpeed @27 :Float32; - fineSpeedUncertainty @28 :Float32; - - carrierPhase @29 :Float64; - fCount @30 :UInt32; - - parityErrorCount @31 :UInt16; - goodParity @32 :Bool; - } - } - - struct DrSvPolyReport { - svId @0 :UInt16; - frequencyIndex @1 :Int8; - - hasPosition @2 :Bool; - hasIono @3 :Bool; - hasTropo @4 :Bool; - hasElevation @5 :Bool; - polyFromXtra @6 :Bool; - hasSbasIono @7 :Bool; - - iode @8 :UInt16; - t0 @9 :Float64; - xyz0 @10 :List(Float64); - xyzN @11 :List(Float64); - other @12 :List(Float32); - - positionUncertainty @13 :Float32; - ionoDelay @14 :Float32; - ionoDot @15 :Float32; - sbasIonoDelay @16 :Float32; - sbasIonoDot @17 :Float32; - tropoDelay @18 :Float32; - elevation @19 :Float32; - elevationDot @20 :Float32; - elevationUncertainty @21 :Float32; - - velocityCoeff @22 :List(Float64); - - } -} - -struct LidarPts { - r @0 :List(UInt16); # uint16 m*500.0 - theta @1 :List(UInt16); # uint16 deg*100.0 - reflect @2 :List(UInt8); # uint8 0-255 - - # For storing out of file. - idx @3 :UInt64; - - # For storing in file - pkt @4 :Data; -} - struct ProcLog { cpuTimes @0 :List(CPUTimes); mem @1 :Mem; @@ -1589,7 +916,6 @@ struct ProcLog { inactive @6 :UInt64; shared @7 :UInt64; } - } struct UbloxGnss { @@ -1598,6 +924,7 @@ struct UbloxGnss { ephemeris @1 :Ephemeris; ionoData @2 :IonoData; hwStatus @3 :HwStatus; + hwStatus2 @4 :HwStatus2; } struct MeasurementReport { @@ -1743,6 +1070,24 @@ struct UbloxGnss { dontknow @2; } } + + struct HwStatus2 { + ofsI @0 :Int8; + magI @1 :UInt8; + ofsQ @2 :Int8; + magQ @3 :UInt8; + cfgSource @4 :ConfigSource; + lowLevCfg @5 :UInt32; + postStatus @6 :UInt32; + + enum ConfigSource { + undefined @0; + rom @1; + otp @2; + configpins @3; + flash @4; + } + } } struct Clocks { @@ -1777,105 +1122,6 @@ struct LiveLongitudinalMpcData { cost @10 :Float64; } - -struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 { - x @0 :Float32; - y @1 :Float32; - z @2 :Float32; -} - -struct ECEFPoint @0xc25bbbd524983447 { - x @0 :Float64; - y @1 :Float64; - z @2 :Float64; -} - -struct GPSPlannerPoints { - curPosDEPRECATED @0 :ECEFPointDEPRECATED; - pointsDEPRECATED @1 :List(ECEFPointDEPRECATED); - curPos @6 :ECEFPoint; - points @7 :List(ECEFPoint); - valid @2 :Bool; - trackName @3 :Text; - speedLimit @4 :Float32; - accelTarget @5 :Float32; -} - -struct GPSPlannerPlan { - valid @0 :Bool; - poly @1 :List(Float32); - trackName @2 :Text; - speed @3 :Float32; - acceleration @4 :Float32; - pointsDEPRECATED @5 :List(ECEFPointDEPRECATED); - points @6 :List(ECEFPoint); - xLookahead @7 :Float32; -} - -struct TrafficEvent @0xacfa74a094e62626 { - type @0 :Type; - distance @1 :Float32; - action @2 :Action; - resuming @3 :Bool; - - enum Type { - stopSign @0; - lightRed @1; - lightYellow @2; - lightGreen @3; - stopLight @4; - } - - enum Action { - none @0; - yield @1; - stop @2; - resumeReady @3; - } - -} - -struct OrbslamCorrection { - correctionMonoTime @0 :UInt64; - prePositionECEF @1 :List(Float64); - postPositionECEF @2 :List(Float64); - prePoseQuatECEF @3 :List(Float32); - postPoseQuatECEF @4 :List(Float32); - numInliers @5 :UInt32; -} - -struct OrbObservation { - observationMonoTime @0 :UInt64; - normalizedCoordinates @1 :List(Float32); - locationECEF @2 :List(Float64); - matchDistance @3: UInt32; -} - -struct UiNavigationEvent { - type @0: Type; - status @1: Status; - distanceTo @2: Float32; - endRoadPointDEPRECATED @3: ECEFPointDEPRECATED; - endRoadPoint @4: ECEFPoint; - - enum Type { - none @0; - laneChangeLeft @1; - laneChangeRight @2; - mergeLeft @3; - mergeRight @4; - turnLeft @5; - turnRight @6; - } - - enum Status { - none @0; - passive @1; - approaching @2; - active @3; - } -} - struct UiLayoutState { activeApp @0 :App; sidebarCollapsed @1 :Bool; @@ -1897,70 +1143,12 @@ struct Joystick { buttons @1: List(Bool); } -struct OrbOdometry { - # timing first - startMonoTime @0 :UInt64; - endMonoTime @1 :UInt64; - - # fundamental matrix and error - f @2: List(Float64); - err @3: Float64; - - # number of inlier points - inliers @4: Int32; - - # for debug only - # indexed by endMonoTime features - # value is startMonoTime feature match - # -1 if no match - matches @5: List(Int16); -} - -struct OrbFeatures { - timestampEof @0 :UInt64; - # transposed arrays of normalized image coordinates - # len(xs) == len(ys) == len(descriptors) * 32 - xs @1 :List(Float32); - ys @2 :List(Float32); - descriptors @3 :Data; - octaves @4 :List(Int8); - - # match index to last OrbFeatures - # -1 if no match - timestampLastEof @5 :UInt64; - matches @6: List(Int16); -} - -struct OrbFeaturesSummary { - timestampEof @0 :UInt64; - timestampLastEof @1 :UInt64; - - featureCount @2 :UInt16; - matchCount @3 :UInt16; - computeNs @4 :UInt64; -} - -struct OrbKeyFrame { - # this is a globally unique id for the KeyFrame - id @0: UInt64; - - # this is the location of the KeyFrame - pos @1: ECEFPoint; - - # these are the features in the world - # len(dpos) == len(descriptors) * 32 - dpos @2 :List(ECEFPoint); - descriptors @3 :Data; -} - struct DriverState { frameId @0 :UInt32; modelExecutionTime @14 :Float32; dspExecutionTime @16 :Float32; - rawPred @15 :Data; + rawPredictions @15 :Data; - descriptorDEPRECATED @1 :List(Float32); - stdDEPRECATED @2 :Float32; faceOrientation @3 :List(Float32); facePosition @4 :List(Float32); faceProb @5 :Float32; @@ -1968,19 +1156,24 @@ struct DriverState { rightEyeProb @7 :Float32; leftBlinkProb @8 :Float32; rightBlinkProb @9 :Float32; - irPwrDEPRECATED @10 :Float32; faceOrientationStd @11 :List(Float32); facePositionStd @12 :List(Float32); - sgProb @13 :Float32; + sunglassesProb @13 :Float32; + poorVision @17 :Float32; + partialFace @18 :Float32; + distractedPose @19 :Float32; + distractedEyes @20 :Float32; + + irPwrDEPRECATED @10 :Float32; + descriptorDEPRECATED @1 :List(Float32); + stdDEPRECATED @2 :Float32; } -struct DMonitoringState { - # TODO: deprecate old fields in controlsState +struct DriverMonitoringState @0xb83cda094a1da284 { events @0 :List(Car.CarEvent); faceDetected @1 :Bool; isDistracted @2 :Bool; awarenessStatus @3 :Float32; - isRHD @4 :Bool; posePitchOffset @6 :Float32; posePitchValidCount @7 :UInt32; poseYawOffset @8 :Float32; @@ -1990,8 +1183,10 @@ struct DMonitoringState { awarenessPassive @12 :Float32; isLowStd @13 :Bool; hiStdCount @14 :UInt32; - isPreview @15 :Bool; + isActiveMode @16 :Bool; + isRHDDEPRECATED @4 :Bool; + isPreviewDEPRECATED @15 :Bool; rhdCheckedDEPRECATED @5 :Bool; } @@ -2005,8 +1200,8 @@ struct Boot { struct LiveParametersData { valid @0 :Bool; gyroBias @1 :Float32; - angleOffset @2 :Float32; - angleOffsetAverage @3 :Float32; + angleOffsetDeg @2 :Float32; + angleOffsetAverageDeg @3 :Float32; stiffnessFactor @4 :Float32; steerRatio @5 :Float32; sensorValid @6 :Bool; @@ -2015,7 +1210,7 @@ struct LiveParametersData { posenetValid @9 :Bool; } -struct LiveMapData { +struct LiveMapDataDEPRECATED { speedLimitValid @0 :Bool; speedLimit @1 :Float32; speedAdvisoryValid @12 :Bool; @@ -2044,13 +1239,6 @@ struct CameraOdometry { rotStd @3 :List(Float32); # std rad/s in device frame } -struct KalmanOdometry { - trans @0 :List(Float32); # m/s in device frame - rot @1 :List(Float32); # rad/s in device frame - transStd @2 :List(Float32); # std m/s in device frame - rotStd @3 :List(Float32); # std rad/s in device frame -} - struct Sentinel { enum SentinelType { endOfSegment @0; @@ -2061,87 +1249,112 @@ struct Sentinel { type @0 :SentinelType; } +struct ManagerState { + processes @0 :List(ProcessState); + + struct ProcessState { + name @0 :Text; + pid @1 :Int32; + running @2 :Bool; + exitCode @3 :Int32; + } +} + struct Event { - # in nanoseconds? - logMonoTime @0 :UInt64; + logMonoTime @0 :UInt64; # nanoseconds valid @67 :Bool = true; union { + # *********** log metadata *********** initData @1 :InitData; - frame @2 :FrameData; + sentinel @73 :Sentinel; + + # *********** bootlog *********** + boot @60 :Boot; + + # ********** openpilot daemon msgs ********** gpsNMEA @3 :GPSNMEAData; - sensorEventDEPRECATED @4 :SensorEventData; can @5 :List(CanData); - thermal @6 :ThermalData; controlsState @7 :ControlsState; - liveEventDEPRECATED @8 :List(LiveEventData); - model @9 :ModelData; - features @10 :CalibrationFeatures; sensorEvents @11 :List(SensorEventData); - health @12 :HealthData; + pandaState @12 :PandaState; radarState @13 :RadarState; - liveUIDEPRECATED @14 :LiveUI; - encodeIdx @15 :EncodeIndex; liveTracks @16 :List(LiveTracks); sendcan @17 :List(CanData); - logMessage @18 :Text; liveCalibration @19 :LiveCalibrationData; - androidLog @20 :AndroidLogEntry; gpsLocation @21 :GpsLocationData; carState @22 :Car.CarState; carControl @23 :Car.CarControl; - plan @24 :Plan; - liveLocation @25 :LiveLocationData; - ethernetData @26 :List(EthernetPacket); - navUpdate @27 :NavUpdate; - cellInfo @28 :List(CellInfo); - wifiScan @29 :List(WifiScan); - androidGnss @30 :AndroidGnss; - qcomGnss @31 :QcomGnss; - lidarPts @32 :LidarPts; - procLog @33 :ProcLog; + longitudinalPlan @24 :LongitudinalPlan; + lateralPlan @64 :LateralPlan; ubloxGnss @34 :UbloxGnss; - clocks @35 :Clocks; liveMpc @36 :LiveMpcData; liveLongitudinalMpc @37 :LiveLongitudinalMpcData; - navStatus @38 :NavStatus; ubloxRaw @39 :Data; - gpsPlannerPoints @40 :GPSPlannerPoints; - gpsPlannerPlan @41 :GPSPlannerPlan; - applanixRaw @42 :Data; - trafficEvents @43 :List(TrafficEvent); - liveLocationTiming @44 :LiveLocationData; - orbslamCorrectionDEPRECATED @45 :OrbslamCorrection; - liveLocationCorrected @46 :LiveLocationData; - orbObservation @47 :List(OrbObservation); gpsLocationExternal @48 :GpsLocationData; - location @49 :LiveLocationData; - uiNavigationEvent @50 :UiNavigationEvent; - liveLocationKalmanDEPRECATED @51 :LiveLocationData; - testJoystick @52 :Joystick; - orbOdometry @53 :OrbOdometry; - orbFeatures @54 :OrbFeatures; - applanixLocation @55 :LiveLocationData; - orbKeyFrame @56 :OrbKeyFrame; uiLayoutState @57 :UiLayoutState; - orbFeaturesSummary @58 :OrbFeaturesSummary; driverState @59 :DriverState; - boot @60 :Boot; liveParameters @61 :LiveParametersData; - liveMapData @62 :LiveMapData; cameraOdometry @63 :CameraOdometry; - pathPlan @64 :PathPlan; - kalmanOdometry @65 :KalmanOdometry; thumbnail @66: Thumbnail; carEvents @68: List(Car.CarEvent); carParams @69: Car.CarParams; - frontFrame @70: FrameData; # driver facing camera - dMonitoringState @71: DMonitoringState; + driverMonitoringState @71: DriverMonitoringState; liveLocationKalman @72 :LiveLocationKalman; - sentinel @73 :Sentinel; - wideFrame @74: FrameData; modelV2 @75 :ModelDataV2; - frontEncodeIdx @76 :EncodeIndex; # driver facing camera - wideEncodeIdx @77 :EncodeIndex; + + # camera stuff, each camera state has a matching encode idx + roadCameraState @2 :FrameData; + driverCameraState @70: FrameData; + wideRoadCameraState @74: FrameData; + roadEncodeIdx @15 :EncodeIndex; + driverEncodeIdx @76 :EncodeIndex; + wideRoadEncodeIdx @77 :EncodeIndex; + + # systems stuff + androidLog @20 :AndroidLogEntry; + managerState @78 :ManagerState; + procLog @33 :ProcLog; + clocks @35 :Clocks; + deviceState @6 :DeviceState; + logMessage @18 :Text; + + + # *********** debug *********** + testJoystick @52 :Joystick; + + # *********** legacy + deprecated *********** + model @9 :Legacy.ModelData; # TODO: rename modelV2 and mark this as deprecated + liveLocationKalmanDEPRECATED @51 :Legacy.LiveLocationData; + orbslamCorrectionDEPRECATED @45 :Legacy.OrbslamCorrection; + liveUIDEPRECATED @14 :Legacy.LiveUI; + sensorEventDEPRECATED @4 :SensorEventData; + liveEventDEPRECATED @8 :List(Legacy.LiveEventData); + liveLocationDEPRECATED @25 :Legacy.LiveLocationData; + ethernetDataDEPRECATED @26 :List(Legacy.EthernetPacket); + cellInfoDEPRECATED @28 :List(Legacy.CellInfo); + wifiScanDEPRECATED @29 :List(Legacy.WifiScan); + uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent; + liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED; + gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints; + gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan; + applanixRawDEPRECATED @42 :Data; + androidGnssDEPRECATED @30 :Legacy.AndroidGnss; + qcomGnssDEPRECATD @31 :Legacy.QcomGnss; + lidarPtsDEPRECATED @32 :Legacy.LidarPts; + navStatusDEPRECATED @38 :Legacy.NavStatus; + trafficEventsDEPRECATED @43 :List(Legacy.TrafficEvent); + liveLocationTimingDEPRECATED @44 :Legacy.LiveLocationData; + liveLocationCorrectedDEPRECATED @46 :Legacy.LiveLocationData; + navUpdateDEPRECATED @27 :Legacy.NavUpdate; + orbObservationDEPRECATED @47 :List(Legacy.OrbObservation); + locationDEPRECATED @49 :Legacy.LiveLocationData; + orbOdometryDEPRECATED @53 :Legacy.OrbOdometry; + orbFeaturesDEPRECATED @54 :Legacy.OrbFeatures; + applanixLocationDEPRECATED @55 :Legacy.LiveLocationData; + orbKeyFrameDEPRECATED @56 :Legacy.OrbKeyFrame; + orbFeaturesSummaryDEPRECATED @58 :Legacy.OrbFeaturesSummary; + featuresDEPRECATED @10 :Legacy.CalibrationFeatures; + kalmanOdometryDEPRECATED @65 :Legacy.KalmanOdometry; } } diff --git a/cereal/messaging/impl_msgq.cc b/cereal/messaging/impl_msgq.cc index 7da9a227c..e211d6abb 100644 --- a/cereal/messaging/impl_msgq.cc +++ b/cereal/messaging/impl_msgq.cc @@ -5,9 +5,10 @@ #include #include - +#include "services.h" #include "impl_msgq.hpp" + volatile sig_atomic_t msgq_do_exit = 0; void sig_handler(int signal) { @@ -15,14 +16,21 @@ void sig_handler(int signal) { msgq_do_exit = 1; } +static bool service_exists(std::string path){ + for (const auto& it : services) { + if (it.name == path) { + return true; + } + } + return false; +} + static size_t get_size(std::string endpoint){ size_t sz = DEFAULT_SEGMENT_SIZE; -#if !defined(QCOM) && !defined(QCOM2) - if (endpoint == "frame" || endpoint == "frontFrame" || endpoint == "wideFrame"){ + if (endpoint == "roadCameraState" || endpoint == "driverCameraState" || endpoint == "wideRoadCameraState"){ sz *= 10; } -#endif return sz; } @@ -61,10 +69,14 @@ MSGQMessage::~MSGQMessage() { this->close(); } -int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){ +int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){ assert(context); assert(address == "127.0.0.1"); + if (check_endpoint && !service_exists(std::string(endpoint))){ + std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl; + } + q = new msgq_queue_t; int r = msgq_new_queue(q, endpoint.c_str(), get_size(endpoint)); if (r != 0){ @@ -150,9 +162,13 @@ MSGQSubSocket::~MSGQSubSocket(){ } } -int MSGQPubSocket::connect(Context *context, std::string endpoint){ +int MSGQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){ assert(context); + if (check_endpoint && !service_exists(std::string(endpoint))){ + std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl; + } + q = new msgq_queue_t; int r = msgq_new_queue(q, endpoint.c_str(), get_size(endpoint)); if (r != 0){ diff --git a/cereal/messaging/impl_msgq.hpp b/cereal/messaging/impl_msgq.hpp index 98e680945..e89994852 100644 --- a/cereal/messaging/impl_msgq.hpp +++ b/cereal/messaging/impl_msgq.hpp @@ -34,7 +34,7 @@ private: msgq_queue_t * q = NULL; int timeout; public: - int connect(Context *context, std::string endpoint, std::string address, bool conflate=false); + int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true); void setTimeout(int timeout); void * getRawSocket() {return (void*)q;} Message *receive(bool non_blocking=false); @@ -45,7 +45,7 @@ class MSGQPubSocket : public PubSocket { private: msgq_queue_t * q = NULL; public: - int connect(Context *context, std::string endpoint); + int connect(Context *context, std::string endpoint, bool check_endpoint=true); int sendMessage(Message *message); int send(char *data, size_t size); ~MSGQPubSocket(); diff --git a/cereal/messaging/impl_zmq.cc b/cereal/messaging/impl_zmq.cc index 195124de6..605165fcf 100644 --- a/cereal/messaging/impl_zmq.cc +++ b/cereal/messaging/impl_zmq.cc @@ -54,7 +54,7 @@ ZMQMessage::~ZMQMessage() { } -int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){ +int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){ sock = zmq_socket(context->getRawContext(), ZMQ_SUB); if (sock == NULL){ return -1; @@ -71,7 +71,11 @@ int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string ad zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl)); full_endpoint = "tcp://" + address + ":"; - full_endpoint += std::to_string(get_port(endpoint)); + if (check_endpoint){ + full_endpoint += std::to_string(get_port(endpoint)); + } else { + full_endpoint += endpoint; + } return zmq_connect(sock, full_endpoint.c_str()); } @@ -103,14 +107,18 @@ ZMQSubSocket::~ZMQSubSocket(){ zmq_close(sock); } -int ZMQPubSocket::connect(Context *context, std::string endpoint){ +int ZMQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){ sock = zmq_socket(context->getRawContext(), ZMQ_PUB); if (sock == NULL){ return -1; } full_endpoint = "tcp://*:"; - full_endpoint += std::to_string(get_port(endpoint)); + if (check_endpoint){ + full_endpoint += std::to_string(get_port(endpoint)); + } else { + full_endpoint += endpoint; + } return zmq_bind(sock, full_endpoint.c_str()); } diff --git a/cereal/messaging/impl_zmq.hpp b/cereal/messaging/impl_zmq.hpp index bfa9b74cd..f9f6deedb 100644 --- a/cereal/messaging/impl_zmq.hpp +++ b/cereal/messaging/impl_zmq.hpp @@ -32,7 +32,7 @@ private: void * sock; std::string full_endpoint; public: - int connect(Context *context, std::string endpoint, std::string address, bool conflate=false); + int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true); void setTimeout(int timeout); void * getRawSocket() {return sock;} Message *receive(bool non_blocking=false); @@ -44,7 +44,7 @@ private: void * sock; std::string full_endpoint; public: - int connect(Context *context, std::string endpoint); + int connect(Context *context, std::string endpoint, bool check_endpoint=true); int sendMessage(Message *message); int send(char *data, size_t size); ~ZMQPubSocket(); diff --git a/cereal/messaging/messaging.cc b/cereal/messaging/messaging.cc index 9e2ae409d..06bc66ae1 100644 --- a/cereal/messaging/messaging.cc +++ b/cereal/messaging/messaging.cc @@ -8,9 +8,13 @@ const bool MUST_USE_ZMQ = true; const bool MUST_USE_ZMQ = false; #endif +bool messaging_use_zmq(){ + return std::getenv("ZMQ") || MUST_USE_ZMQ; +} + Context * Context::create(){ Context * c; - if (std::getenv("ZMQ") || MUST_USE_ZMQ){ + if (messaging_use_zmq()){ c = new ZMQContext(); } else { c = new MSGQContext(); @@ -20,7 +24,7 @@ Context * Context::create(){ SubSocket * SubSocket::create(){ SubSocket * s; - if (std::getenv("ZMQ") || MUST_USE_ZMQ){ + if (messaging_use_zmq()){ s = new ZMQSubSocket(); } else { s = new MSGQSubSocket(); @@ -28,33 +32,9 @@ SubSocket * SubSocket::create(){ return s; } -SubSocket * SubSocket::create(Context * context, std::string endpoint){ +SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){ SubSocket *s = SubSocket::create(); - int r = s->connect(context, endpoint, "127.0.0.1"); - - if (r == 0) { - return s; - } else { - delete s; - return NULL; - } -} - -SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address){ - SubSocket *s = SubSocket::create(); - int r = s->connect(context, endpoint, address); - - if (r == 0) { - return s; - } else { - delete s; - return NULL; - } -} - -SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate){ - SubSocket *s = SubSocket::create(); - int r = s->connect(context, endpoint, address, conflate); + int r = s->connect(context, endpoint, address, conflate, check_endpoint); if (r == 0) { return s; @@ -66,7 +46,7 @@ SubSocket * SubSocket::create(Context * context, std::string endpoint, std::stri PubSocket * PubSocket::create(){ PubSocket * s; - if (std::getenv("ZMQ") || MUST_USE_ZMQ){ + if (messaging_use_zmq()){ s = new ZMQPubSocket(); } else { s = new MSGQPubSocket(); @@ -74,9 +54,9 @@ PubSocket * PubSocket::create(){ return s; } -PubSocket * PubSocket::create(Context * context, std::string endpoint){ +PubSocket * PubSocket::create(Context * context, std::string endpoint, bool check_endpoint){ PubSocket *s = PubSocket::create(); - int r = s->connect(context, endpoint); + int r = s->connect(context, endpoint, check_endpoint); if (r == 0) { return s; @@ -88,7 +68,7 @@ PubSocket * PubSocket::create(Context * context, std::string endpoint){ Poller * Poller::create(){ Poller * p; - if (std::getenv("ZMQ") || MUST_USE_ZMQ){ + if (messaging_use_zmq()){ p = new ZMQPoller(); } else { p = new MSGQPoller(); diff --git a/cereal/messaging/messaging.hpp b/cereal/messaging/messaging.hpp index 9ade8bf2b..6531de629 100644 --- a/cereal/messaging/messaging.hpp +++ b/cereal/messaging/messaging.hpp @@ -12,6 +12,8 @@ #define MSG_MULTIPLE_PUBLISHERS 100 +bool messaging_use_zmq(); + class Context { public: virtual void * getRawContext() = 0; @@ -32,24 +34,23 @@ public: class SubSocket { public: - virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false) = 0; + virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) = 0; virtual void setTimeout(int timeout) = 0; virtual Message *receive(bool non_blocking=false) = 0; virtual void * getRawSocket() = 0; static SubSocket * create(); - static SubSocket * create(Context * context, std::string endpoint); - static SubSocket * create(Context * context, std::string endpoint, std::string address); - static SubSocket * create(Context * context, std::string endpoint, std::string address, bool conflate); + static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true); virtual ~SubSocket(){}; }; class PubSocket { public: - virtual int connect(Context *context, std::string endpoint) = 0; + virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0; virtual int sendMessage(Message *message) = 0; virtual int send(char *data, size_t size) = 0; static PubSocket * create(); - static PubSocket * create(Context * context, std::string endpoint); + static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true); + static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true); virtual ~PubSocket(){}; }; diff --git a/cereal/messaging/msgq.cc b/cereal/messaging/msgq.cc index 30b0225cd..a51aef8e8 100644 --- a/cereal/messaging/msgq.cc +++ b/cereal/messaging/msgq.cc @@ -21,8 +21,6 @@ #include -#include "services.h" - #include "msgq.hpp" void sigusr2_handler(int signal) { @@ -83,20 +81,9 @@ void msgq_wait_for_subscriber(msgq_queue_t *q){ return; } -bool service_exists(std::string path){ - for (const auto& it : services) { - if (it.name == path) { - return true; - } - } - return false; -} int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){ assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes - if (!service_exists(std::string(path))){ - std::cout << "Warning, " << std::string(path) << " is not in service list." << std::endl; - } std::signal(SIGUSR2, sigusr2_handler); const char * prefix = "/dev/shm/"; diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml deleted file mode 100644 index b140251cf..000000000 --- a/cereal/service_list.yaml +++ /dev/null @@ -1,180 +0,0 @@ -# TODO: these port numbers are hardcoded in c, fix this - -# LogRotate: 8001 is a PUSH PULL socket between loggerd and visiond - -# all ZMQ pub sub: port, should_log, frequency, (qlog_decimation) - -# frame syncing packet -frame: [8002, true, 20., 1] -# accel, gyro, and compass -sensorEvents: [8003, true, 100., 100] -# GPS data, also global timestamp -gpsNMEA: [8004, true, 9.] # 9 msgs each sec -# CPU+MEM+GPU+BAT temps -thermal: [8005, true, 2., 1] -# List(CanData), list of can messages -can: [8006, true, 100.] -controlsState: [8007, true, 100., 100] -#liveEvent: [8008, true, 0.] -model: [8009, true, 20., 5] -features: [8010, true, 0.] -health: [8011, true, 2., 1] -radarState: [8012, true, 20., 5] -#liveUI: [8014, true, 0.] -encodeIdx: [8015, true, 20.] -liveTracks: [8016, true, 20.] -sendcan: [8017, true, 100.] -logMessage: [8018, true, 0.] -liveCalibration: [8019, true, 4., 4] -androidLog: [8020, true, 0.] -carState: [8021, true, 100., 10] -# 8022 is reserved for sshd -carControl: [8023, true, 100., 10] -plan: [8024, true, 20., 2] -liveLocation: [8025, true, 0., 1] -gpsLocation: [8026, true, 1., 1] -ethernetData: [8027, true, 0.] -navUpdate: [8028, true, 0.] -qcomGnss: [8029, true, 0.] -lidarPts: [8030, true, 0.] -procLog: [8031, true, 0.5] -gpsLocationExternal: [8032, true, 10., 1] -ubloxGnss: [8033, true, 10.] -clocks: [8034, true, 1., 1] -liveMpc: [8035, false, 20.] -liveLongitudinalMpc: [8036, false, 20.] -navStatus: [8038, true, 0.] -gpsLocationTrimble: [8039, true, 0.] -trimbleGnss: [8041, true, 0.] -ubloxRaw: [8042, true, 20.] -gpsPlannerPoints: [8043, true, 0.] -gpsPlannerPlan: [8044, true, 0.] -applanixRaw: [8046, true, 0.] -orbLocation: [8047, true, 0.] -trafficEvents: [8048, true, 0.] -liveLocationTiming: [8049, true, 0.] -orbslamCorrection: [8050, true, 0.] -liveLocationCorrected: [8051, true, 0.] -orbObservation: [8052, true, 0.] -applanixLocation: [8053, true, 0.] -liveLocationKalman: [8054, true, 20., 2] -uiNavigationEvent: [8055, true, 0.] -orbOdometry: [8057, true, 0.] -orbFeatures: [8058, false, 0.] -orbKeyFrame: [8059, true, 0.] -uiLayoutState: [8060, true, 0.] -frontEncodeIdx: [8061, true, 5.] # should be 20fps on tici -orbFeaturesSummary: [8062, true, 0.] -driverState: [8063, true, 5., 1] -liveParameters: [8064, true, 20., 2] -liveMapData: [8065, true, 0.] -cameraOdometry: [8066, true, 20., 5] -pathPlan: [8067, true, 20., 2] -kalmanOdometry: [8068, true, 0.] -thumbnail: [8069, true, 0.2, 1] -carEvents: [8070, true, 1., 1] -carParams: [8071, true, 0.02, 1] -frontFrame: [8072, true, 10.] -dMonitoringState: [8073, true, 5., 1] -offroadLayout: [8074, false, 0.] -wideEncodeIdx: [8075, true, 20.] -wideFrame: [8076, true, 20.] -modelV2: [8077, true, 20., 20] - -testModel: [8040, false, 0.] -testLiveLocation: [8045, false, 0.] -testJoystick: [8056, false, 0.] - -# 8080 is reserved for slave testing daemon -# 8762 is reserved for logserver - -# manager -- base process to manage starting and stopping of all others -# subscribes: thermal - -# **** processes that communicate with the outside world **** - -# thermald -- decides when to start and stop onroad -# subscribes: health, location -# publishes: thermal - -# boardd -- communicates with the car -# subscribes: sendcan -# publishes: can, health, ubloxRaw - -# sensord -- publishes IMU and Magnetometer -# publishes: sensorEvents - -# gpsd -- publishes EON's gps -# publishes: gpsNMEA - -# camerad -- publishes camera frames -# publishes: frame, frontFrame, thumbnail -# subscribes: driverState - -# dmonitoringmodeld -- runs face detection on camera frames -# publishes: driverState - -# **** stateful data transformers **** - -# modeld -- runs & publishes the model -# publishes: model, cameraOdometry -# subscribes: liveCalibration, pathPlan - -# plannerd -- decides where to drive the car -# subscribes: carState, model, radarState, controlsState, liveParameters -# publishes: plan, pathPlan, liveMpc, liveLongitudinalMpc - -# controlsd -- drives the car by sending CAN messages to panda -# subscribes: can, thermal, health, plan, pathPlan, dMonitoringState, liveCalibration, model -# publishes: carState, carControl, sendcan, controlsState, carEvents, carParams - -# dmonitoringd -- processes driver monitoring data and publishes driver awareness -# subscribes: driverState, liveCalibration, carState, model, gpsLocation -# publishes: dMonitoringState - -# radard -- processes the radar and vision data -# subscribes: can, controlsState, model, liveParameters -# publishes: radarState, liveTracks - -# params_learner -- learns vehicle params by observing the vehicle dynamics -# subscribes: controlsState, sensorEvents, cameraOdometry -# publishes: liveParameters - -# calibrationd -- reads posenet and applies a temporal filter on the frame region to look at -# subscribes: cameraOdometry -# publishes: liveCalibration - -# ubloxd -- read raw ublox data and converts them in readable format -# subscribes: ubloxRaw -# publishes: ubloxGnss - -# **** LOGGING SERVICE **** - -# loggerd -# subscribes: EVERYTHING - -# **** NON VITAL SERVICES **** - -# ui -# subscribes: thermal, model, controlsState, uiLayout, liveCalibration, radarState, liveMpc, plusFrame, liveMapData - -# uploader -# communicates through file system with loggerd - -# deleter -# communicates through file system with loggerd and uploader - -# logmessaged -- central logging service, can log to cloud -# publishes: logMessage - -# logcatd -- fetches logcat info from android -# publishes: androidLog - -# proclogd -- fetches process information -# publishes: procLog - -# tombstoned -- reports native crashes - -# athenad -- on request, open a sub socket and return the value - -# updated -- waits for network access and tries to update every hour diff --git a/cereal/services.py b/cereal/services.py index b22305945..daf6d6def 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -1,34 +1,84 @@ #!/usr/bin/env python3 import os -import yaml +from typing import Optional +EON = os.path.isfile('/EON') -class Service(): - def __init__(self, port, should_log, frequency, decimation=None): +class Service: + def __init__(self, port: int, should_log: bool, frequency: float, decimation: Optional[int] = None): self.port = port self.should_log = should_log self.frequency = frequency self.decimation = decimation +service_list = { + "roadCameraState": Service(8002, True, 20., 1), + "sensorEvents": Service(8003, True, 100., 100), + "gpsNMEA": Service(8004, True, 9.), + "deviceState": Service(8005, True, 2., 1), + "can": Service(8006, True, 100.), + "controlsState": Service(8007, True, 100., 100), + "features": Service(8010, True, 0.), + "pandaState": Service(8011, True, 2., 1), + "radarState": Service(8012, True, 20., 5), + "roadEncodeIdx": Service(8015, True, 20., 1), + "liveTracks": Service(8016, True, 20.), + "sendcan": Service(8017, True, 100.), + "logMessage": Service(8018, True, 0.), + "liveCalibration": Service(8019, True, 4., 4), + "androidLog": Service(8020, True, 0., 1), + "carState": Service(8021, True, 100., 10), + "carControl": Service(8023, True, 100., 10), + "longitudinalPlan": Service(8024, True, 20., 2), + "liveLocation": Service(8025, True, 0., 1), + "gpsLocation": Service(8026, True, 1., 1), + "procLog": Service(8031, True, 0.5), + "gpsLocationExternal": Service(8032, True, 10., 1), + "ubloxGnss": Service(8033, True, 10.), + "clocks": Service(8034, True, 1., 1), + "liveMpc": Service(8035, False, 20.), + "liveLongitudinalMpc": Service(8036, False, 20.), + "ubloxRaw": Service(8042, True, 20.), + "liveLocationKalman": Service(8054, True, 20., 2), + "uiLayoutState": Service(8060, True, 0.), + "liveParameters": Service(8064, True, 20., 2), + "cameraOdometry": Service(8066, True, 20., 5), + "lateralPlan": Service(8067, True, 20., 2), + "thumbnail": Service(8069, True, 0.2, 1), + "carEvents": Service(8070, True, 1., 1), + "carParams": Service(8071, True, 0.02, 1), + "driverCameraState": Service(8072, True, 10. if EON else 20., 1), + "driverEncodeIdx": Service(8061, True, 10. if EON else 20., 1), + "driverState": Service(8063, True, 10. if EON else 20., 1), + "driverMonitoringState": Service(8073, True, 10. if EON else 20., 1), + "offroadLayout": Service(8074, False, 0.), + "wideRoadEncodeIdx": Service(8075, True, 20., 1), + "wideRoadCameraState": Service(8076, True, 20., 1), + "modelV2": Service(8077, True, 20., 20), + "managerState": Service(8078, True, 2., 1), -service_list_path = os.path.join(os.path.dirname(__file__), "service_list.yaml") + "testModel": Service(8040, False, 0.), + "testLiveLocation": Service(8045, False, 0.), + "testJoystick": Service(8056, False, 0.), +} -service_list = {} -with open(service_list_path, "r") as f: - for k, v in yaml.safe_load(f).items(): - decimation = None - if len(v) == 4: - decimation = v[3] - service_list[k] = Service(v[0], v[1], v[2], decimation) +def build_header(): + h = "" + h += "/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT services.py */\n" + h += "#ifndef __SERVICES_H\n" + h += "#define __SERVICES_H\n" + h += "struct service { char name[0x100]; int port; bool should_log; int frequency; int decimation; };\n" + h += "static struct service services[] = {\n" + for k, v in service_list.items(): + should_log = "true" if v.should_log else "false" + decimation = -1 if v.decimation is None else v.decimation + h += ' { .name = "%s", .port = %d, .should_log = %s, .frequency = %d, .decimation = %d },\n' % \ + (k, v.port, should_log, v.frequency, decimation) + h += "};\n" + h += "#endif\n" + return h + if __name__ == "__main__": - print("/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT service_list.yaml */") - print("#ifndef __SERVICES_H") - print("#define __SERVICES_H") - print("struct service { char name[0x100]; int port; bool should_log; int frequency; int decimation; };") - print("static struct service services[] = {") - for k, v in service_list.items(): - print(' { .name = "%s", .port = %d, .should_log = %s, .frequency = %d, .decimation = %d },' % (k, v.port, "true" if v.should_log else "false", v.frequency, -1 if v.decimation is None else v.decimation)) - print("};") - print("#endif") + print(build_header()) diff --git a/cereal/visionipc/.gitignore b/cereal/visionipc/.gitignore new file mode 100644 index 000000000..fa5a2b9f3 --- /dev/null +++ b/cereal/visionipc/.gitignore @@ -0,0 +1,2 @@ +visionipc_pyx.cpp +*.so diff --git a/cereal/visionipc/__init__.py b/cereal/visionipc/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/common/ipc.c b/cereal/visionipc/ipc.cc similarity index 85% rename from selfdrive/common/ipc.c rename to cereal/visionipc/ipc.cc index a5993598d..29c4c9c37 100644 --- a/selfdrive/common/ipc.c +++ b/cereal/visionipc/ipc.cc @@ -1,6 +1,5 @@ #include #include -#include #include #include #include @@ -10,16 +9,19 @@ #include #include +#ifdef __APPLE__ +#define getsocket() socket(AF_UNIX, SOCK_STREAM, 0) +#else +#define getsocket() socket(AF_UNIX, SOCK_SEQPACKET, 0) +#endif + #include "ipc.h" int ipc_connect(const char* socket_path) { int err; -#ifdef __APPLE__ - int sock = socket(AF_UNIX, SOCK_STREAM, 0); -#else - int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); -#endif + int sock = getsocket(); + if (sock < 0) return -1; struct sockaddr_un addr = { .sun_family = AF_UNIX, @@ -39,11 +41,8 @@ int ipc_bind(const char* socket_path) { unlink(socket_path); -#ifdef __APPLE__ - int sock = socket(AF_UNIX, SOCK_STREAM, 0); -#else - int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); -#endif + int sock = getsocket(); + struct sockaddr_un addr = { .sun_family = AF_UNIX, }; @@ -87,7 +86,6 @@ int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fd cmsg->cmsg_type = SCM_RIGHTS; cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds); memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds); - // printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len); } return sendmsg(fd, &msg, 0); } else { @@ -102,8 +100,6 @@ int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fd recv_fds = (cmsg->cmsg_len - CMSG_LEN(0)); assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0); recv_fds /= sizeof(int); - // printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds); - // assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds)); assert(fds && recv_fds <= num_fds); memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds); diff --git a/selfdrive/common/ipc.h b/cereal/visionipc/ipc.h similarity index 61% rename from selfdrive/common/ipc.h rename to cereal/visionipc/ipc.h index 7ebfd7164..14bb61a52 100644 --- a/selfdrive/common/ipc.h +++ b/cereal/visionipc/ipc.h @@ -1,19 +1,7 @@ -#ifndef IPC_H -#define IPC_H - -#include - -#ifdef __cplusplus -extern "C" { -#endif +#pragma once +#include int ipc_connect(const char* socket_path); int ipc_bind(const char* socket_path); int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds, int *out_num_fds); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif \ No newline at end of file diff --git a/cereal/visionipc/test_runner.cc b/cereal/visionipc/test_runner.cc new file mode 100644 index 000000000..62bf7476a --- /dev/null +++ b/cereal/visionipc/test_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/cereal/visionipc/visionbuf.cc b/cereal/visionipc/visionbuf.cc new file mode 100644 index 000000000..64693b572 --- /dev/null +++ b/cereal/visionipc/visionbuf.cc @@ -0,0 +1,40 @@ +#include "visionbuf.h" + +#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1)) + +#ifdef QCOM +// from libadreno_utils.so +extern "C" void compute_aligned_width_and_height(int width, + int height, + int bpp, + int tile_mode, + int raster_mode, + int padding_threshold, + int *aligned_w, + int *aligned_h); +#endif + +void visionbuf_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) { +#if defined(QCOM) && !defined(QCOM_REPLAY) + compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h); +#else + *aligned_w = width; *aligned_h = height; +#endif +} + +void VisionBuf::init_rgb(size_t width, size_t height, size_t stride) { + this->rgb = true; + this->width = width; + this->height = height; + this->stride = stride; +} + +void VisionBuf::init_yuv(size_t width, size_t height){ + this->rgb = false; + this->width = width; + this->height = height; + + this->y = (uint8_t *)this->addr; + this->u = this->y + (width * height); + this->v = this->u + (width / 2 * height / 2); +} diff --git a/cereal/visionipc/visionbuf.h b/cereal/visionipc/visionbuf.h new file mode 100644 index 000000000..f6d84f4c1 --- /dev/null +++ b/cereal/visionipc/visionbuf.h @@ -0,0 +1,63 @@ +#pragma once +#include "visionipc.h" + +#define CL_USE_DEPRECATED_OPENCL_1_2_APIS +#ifdef __APPLE__ +#include +#else +#include +#endif + +#define VISIONBUF_SYNC_FROM_DEVICE 0 +#define VISIONBUF_SYNC_TO_DEVICE 1 + +enum VisionStreamType { + VISION_STREAM_RGB_BACK, + VISION_STREAM_RGB_FRONT, + VISION_STREAM_RGB_WIDE, + VISION_STREAM_YUV_BACK, + VISION_STREAM_YUV_FRONT, + VISION_STREAM_YUV_WIDE, + VISION_STREAM_MAX, +}; + +class VisionBuf { + public: + size_t len = 0; + size_t mmap_len = 0; + void * addr = nullptr; + int fd = 0; + + bool rgb = false; + size_t width = 0; + size_t height = 0; + size_t stride = 0; + + // YUV + uint8_t * y = nullptr; + uint8_t * u = nullptr; + uint8_t * v = nullptr; + + // Visionipc + uint64_t server_id = 0; + size_t idx = 0; + VisionStreamType type; + + // OpenCL + cl_mem buf_cl = nullptr; + cl_command_queue copy_q = nullptr; + + // ion + int handle = 0; + bool owner = false; + + void allocate(size_t len); + void import(); + void init_cl(cl_device_id device_id, cl_context ctx); + void init_rgb(size_t width, size_t height, size_t stride); + void init_yuv(size_t width, size_t height); + void sync(int dir); + void free(); +}; + +void visionbuf_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h); diff --git a/cereal/visionipc/visionbuf_cl.cc b/cereal/visionipc/visionbuf_cl.cc new file mode 100644 index 000000000..6fb2884b5 --- /dev/null +++ b/cereal/visionipc/visionbuf_cl.cc @@ -0,0 +1,86 @@ +#include "visionbuf.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic offset = 0; + +static void *malloc_with_fd(size_t len, int *fd) { + char full_path[0x100]; + +#ifdef __APPLE__ + snprintf(full_path, sizeof(full_path)-1, "/tmp/visionbuf_%d_%d", getpid(), offset++); +#else + snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++); +#endif + + *fd = open(full_path, O_RDWR | O_CREAT, 0777); + assert(*fd >= 0); + + unlink(full_path); + + ftruncate(*fd, len); + void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0); + assert(addr != MAP_FAILED); + + return addr; +} + +void VisionBuf::allocate(size_t len) { + int fd; + void *addr = malloc_with_fd(len, &fd); + + this->len = len; + this->mmap_len = len; + this->addr = addr; + this->fd = fd; +} + +void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx){ + int err; + + this->copy_q = clCreateCommandQueue(ctx, device_id, 0, &err); + assert(err == 0); + + this->buf_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, this->len, this->addr, &err); + assert(err == 0); +} + + +void VisionBuf::import(){ + assert(this->fd >= 0); + this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0); + assert(this->addr != MAP_FAILED); +} + + +void VisionBuf::sync(int dir) { + int err = 0; + if (!this->buf_cl) return; + + if (dir == VISIONBUF_SYNC_FROM_DEVICE) { + err = clEnqueueReadBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL); + } else { + err = clEnqueueWriteBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL); + } + assert(err == 0); + clFinish(this->copy_q); +} + +void VisionBuf::free() { + if (this->buf_cl){ + int err = clReleaseMemObject(this->buf_cl); + assert(err == 0); + + clReleaseCommandQueue(this->copy_q); + } + + munmap(this->addr, this->len); + close(this->fd); +} diff --git a/selfdrive/common/visionbuf_ion.c b/cereal/visionipc/visionbuf_ion.cc similarity index 60% rename from selfdrive/common/visionbuf_ion.c rename to cereal/visionipc/visionbuf_ion.cc index 5c26bea6a..31448daed 100644 --- a/selfdrive/common/visionbuf_ion.c +++ b/cereal/visionipc/visionbuf_ion.cc @@ -10,7 +10,7 @@ #include #include #include -#include "common/clutil.h" + #include #include "visionbuf.h" @@ -36,7 +36,7 @@ static void ion_init() { } } -VisionBuf visionbuf_allocate(size_t len) { +void VisionBuf::allocate(size_t len) { int err; ion_init(); @@ -62,46 +62,58 @@ VisionBuf visionbuf_allocate(size_t len) { memset(addr, 0, ion_alloc.len); - return (VisionBuf){ - .len = len, - .mmap_len = ion_alloc.len, - .addr = addr, - .handle = ion_alloc.handle, - .fd = ion_fd_data.fd, - }; + this->owner = true; + this->len = len; + this->mmap_len = ion_alloc.len; + this->addr = addr; + this->handle = ion_alloc.handle; + this->fd = ion_fd_data.fd; } -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { - VisionBuf buf = visionbuf_allocate(len); +void VisionBuf::import(){ + int err; + assert(this->fd >= 0); - assert(((uintptr_t)buf.addr % DEVICE_PAGE_SIZE_CL) == 0); + ion_init(); + + // Get handle + struct ion_fd_data fd_data = {0}; + fd_data.fd = this->fd; + err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data); + assert(err == 0); + + this->owner = false; + this->handle = fd_data.handle; + this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0); + assert(this->addr != MAP_FAILED); +} + +void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx) { + int err; + + assert(((uintptr_t)this->addr % DEVICE_PAGE_SIZE_CL) == 0); cl_mem_ion_host_ptr ion_cl = {0}; ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM; ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM; - ion_cl.ion_filedesc = buf.fd; - ion_cl.ion_hostptr = buf.addr; + ion_cl.ion_filedesc = this->fd; + ion_cl.ion_hostptr = this->addr; - buf.buf_cl = CL_CHECK_ERR(clCreateBuffer(ctx, + this->buf_cl = clCreateBuffer(ctx, CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM, - buf.len, &ion_cl, &err)); - return buf; + this->len, &ion_cl, &err); + assert(err == 0); } -void visionbuf_sync(const VisionBuf* buf, int dir) { +void VisionBuf::sync(int dir) { int err; - struct ion_fd_data fd_data = {0}; - fd_data.fd = buf->fd; - err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data); - assert(err == 0); - struct ion_flush_data flush_data = {0}; - flush_data.handle = fd_data.handle; - flush_data.vaddr = buf->addr; + flush_data.handle = this->handle; + flush_data.vaddr = this->addr; flush_data.offset = 0; - flush_data.length = buf->len; + flush_data.length = this->len; // ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE // ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE @@ -109,36 +121,28 @@ void visionbuf_sync(const VisionBuf* buf, int dir) { struct ion_custom_data custom_data = {0}; - switch (dir) { - case VISIONBUF_SYNC_FROM_DEVICE: - custom_data.cmd = ION_IOC_INV_CACHES; - break; - case VISIONBUF_SYNC_TO_DEVICE: - custom_data.cmd = ION_IOC_CLEAN_CACHES; - break; - default: - assert(0); - } + assert(dir == VISIONBUF_SYNC_FROM_DEVICE || dir == VISIONBUF_SYNC_TO_DEVICE); + custom_data.cmd = (dir == VISIONBUF_SYNC_FROM_DEVICE) ? + ION_IOC_INV_CACHES : ION_IOC_CLEAN_CACHES; custom_data.arg = (unsigned long)&flush_data; err = ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data); assert(err == 0); - - struct ion_handle_data handle_data = {0}; - handle_data.handle = fd_data.handle; - err = ioctl(ion_fd, ION_IOC_FREE, &handle_data); - assert(err == 0); } -void visionbuf_free(const VisionBuf* buf) { - if (buf->buf_cl) { - CL_CHECK(clReleaseMemObject(buf->buf_cl)); +void VisionBuf::free() { + if (this->buf_cl){ + int err = clReleaseMemObject(this->buf_cl); + assert(err == 0); + } + + munmap(this->addr, this->mmap_len); + close(this->fd); + + // Free the ION buffer if we also shared it + if (this->owner){ + struct ion_handle_data handle_data = {.handle = this->handle}; + int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data); + assert(ret == 0); } - munmap(buf->addr, buf->mmap_len); - close(buf->fd); - struct ion_handle_data handle_data = { - .handle = buf->handle, - }; - int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data); - assert(ret == 0); } diff --git a/cereal/visionipc/visionipc.h b/cereal/visionipc/visionipc.h new file mode 100644 index 000000000..8ecb7da16 --- /dev/null +++ b/cereal/visionipc/visionipc.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +constexpr int VISIONIPC_MAX_FDS = 64; + +struct VisionIpcBufExtra { + uint32_t frame_id; + uint64_t timestamp_sof; + uint64_t timestamp_eof; +}; + +struct VisionIpcPacket { + uint64_t server_id; + size_t idx; + struct VisionIpcBufExtra extra; +}; diff --git a/cereal/visionipc/visionipc.pxd b/cereal/visionipc/visionipc.pxd new file mode 100644 index 000000000..00a17762a --- /dev/null +++ b/cereal/visionipc/visionipc.pxd @@ -0,0 +1,29 @@ +# distutils: language = c++ +#cython: language_level=3 + +from libcpp.string cimport string +from libcpp.vector cimport vector +from libc.stdint cimport uint32_t, uint64_t +from libcpp cimport bool + +cdef extern from "visionbuf.h": + cdef enum VisionStreamType: + pass + + cdef cppclass VisionBuf: + void * addr + size_t len + +cdef extern from "visionipc.h": + struct VisionIpcBufExtra: + uint32_t frame_id + uint64_t timestamp_sof + uint64_t timestamp_eof + +cdef extern from "visionipc_server.h": + cdef cppclass VisionIpcServer: + VisionIpcServer(string, void*, void*) + void create_buffers(VisionStreamType, size_t, bool, size_t, size_t) + VisionBuf * get_buffer(VisionStreamType) + void send(VisionBuf *, VisionIpcBufExtra *, bool) + void start_listener() diff --git a/cereal/visionipc/visionipc_client.cc b/cereal/visionipc/visionipc_client.cc new file mode 100644 index 000000000..d46cc0e4a --- /dev/null +++ b/cereal/visionipc/visionipc_client.cc @@ -0,0 +1,119 @@ +#include +#include +#include +#include + +#include "ipc.h" +#include "visionipc_client.h" +#include "visionipc_server.h" + +VisionIpcClient::VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id, cl_context ctx) : name(name), type(type), device_id(device_id), ctx(ctx) { + msg_ctx = Context::create(); + sock = SubSocket::create(msg_ctx, get_endpoint_name(name, type), "127.0.0.1", conflate, false); + + poller = Poller::create(); + poller->registerSocket(sock); +} + +// Connect is not thread safe. Do not use the buffers while calling connect +bool VisionIpcClient::connect(bool blocking){ + connected = false; + + // Cleanup old buffers on reconnect + for (size_t i = 0; i < num_buffers; i++){ + buffers[i].free(); + } + num_buffers = 0; + + // Connect to server socket and ask for all FDs of type + std::string path = "/tmp/visionipc_" + name; + + int socket_fd = -1; + while (socket_fd < 0) { + socket_fd = ipc_connect(path.c_str()); + + if (socket_fd < 0) { + if (blocking){ + std::cout << "VisionIpcClient connecting" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } else { + return false; + } + } + } + + // Send stream type to server to request FDs + int r = ipc_sendrecv_with_fds(true, socket_fd, &type, sizeof(type), nullptr, 0, nullptr); + assert(r == sizeof(type)); + + // Get FDs + int fds[VISIONIPC_MAX_FDS]; + VisionBuf bufs[VISIONIPC_MAX_FDS]; + r = ipc_sendrecv_with_fds(false, socket_fd, &bufs, sizeof(bufs), fds, VISIONIPC_MAX_FDS, &num_buffers); + + assert(num_buffers > 0); + assert(r == sizeof(VisionBuf) * num_buffers); + + // Import buffers + for (size_t i = 0; i < num_buffers; i++){ + buffers[i] = bufs[i]; + buffers[i].fd = fds[i]; + buffers[i].import(); + if (buffers[i].rgb) { + buffers[i].init_rgb(buffers[i].width, buffers[i].height, buffers[i].stride); + } else { + buffers[i].init_yuv(buffers[i].width, buffers[i].height); + } + + if (device_id) buffers[i].init_cl(device_id, ctx); + } + + connected = true; + return true; +} + +VisionBuf * VisionIpcClient::recv(VisionIpcBufExtra * extra, const int timeout_ms){ + auto p = poller->poll(timeout_ms); + + if (!p.size()){ + return nullptr; + } + + Message * r = sock->receive(true); + if (r == nullptr){ + return nullptr; + } + + // Get buffer + assert(r->getSize() == sizeof(VisionIpcPacket)); + VisionIpcPacket *packet = (VisionIpcPacket*)r->getData(); + + assert(packet->idx < num_buffers); + VisionBuf * buf = &buffers[packet->idx]; + + if (buf->server_id != packet->server_id){ + connected = false; + delete r; + return nullptr; + } + + if (extra) { + *extra = packet->extra; + } + + buf->sync(VISIONBUF_SYNC_TO_DEVICE); + delete r; + return buf; +} + + + +VisionIpcClient::~VisionIpcClient(){ + for (size_t i = 0; i < num_buffers; i++){ + buffers[i].free(); + } + + delete sock; + delete poller; + delete msg_ctx; +} diff --git a/cereal/visionipc/visionipc_client.h b/cereal/visionipc/visionipc_client.h new file mode 100644 index 000000000..469976239 --- /dev/null +++ b/cereal/visionipc/visionipc_client.h @@ -0,0 +1,32 @@ +#pragma once +#include +#include +#include + +#include "messaging.hpp" +#include "visionipc.h" +#include "visionbuf.h" + +class VisionIpcClient { +private: + std::string name; + Context * msg_ctx; + SubSocket * sock; + Poller * poller; + + VisionStreamType type; + + cl_device_id device_id = nullptr; + cl_context ctx = nullptr; + + void init_msgq(bool conflate); + +public: + bool connected = false; + int num_buffers = 0; + VisionBuf buffers[VISIONIPC_MAX_FDS]; + VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id=nullptr, cl_context ctx=nullptr); + ~VisionIpcClient(); + VisionBuf * recv(VisionIpcBufExtra * extra=nullptr, const int timeout_ms=100); + bool connect(bool blocking=true); +}; diff --git a/cereal/visionipc/visionipc_pyx.pyx b/cereal/visionipc/visionipc_pyx.pyx new file mode 100644 index 000000000..fb8eed035 --- /dev/null +++ b/cereal/visionipc/visionipc_pyx.pyx @@ -0,0 +1,49 @@ +# distutils: language = c++ +# cython: c_string_encoding=ascii, language_level=3 + +import sys +from libcpp.string cimport string +from libcpp cimport bool +from libc.string cimport memcpy +from libc.stdint cimport uint32_t, uint64_t + +from .visionipc cimport VisionIpcServer as cppVisionIpcServer +from .visionipc cimport VisionBuf as cppVisionBuf +from .visionipc cimport VisionIpcBufExtra + +cpdef enum VisionStreamType: + VISION_STREAM_RGB_BACK + VISION_STREAM_RGB_FRONT + VISION_STREAM_RGB_WIDE + VISION_STREAM_YUV_BACK + VISION_STREAM_YUV_FRONT + VISION_STREAM_YUV_WIDE + +cdef class VisionIpcServer: + cdef cppVisionIpcServer * server + + def __init__(self, string name): + self.server = new cppVisionIpcServer(name, NULL, NULL) + + def create_buffers(self, VisionStreamType tp, size_t num_buffers, bool rgb, size_t width, size_t height): + self.server.create_buffers(tp, num_buffers, rgb, width, height) + + def send(self, VisionStreamType tp, bytes data, uint32_t frame_id=0, uint64_t timestamp_sof=0, uint64_t timestamp_eof=0): + cdef cppVisionBuf * buf = self.server.get_buffer(tp) + + # Populate buffer + assert buf.len == len(data) + memcpy(buf.addr, data, len(data)) + + cdef VisionIpcBufExtra extra + extra.frame_id = frame_id + extra.timestamp_sof = timestamp_sof + extra.timestamp_eof = timestamp_eof + + self.server.send(buf, &extra, False) + + def start_listener(self): + self.server.start_listener() + + def __dealloc__(self): + del self.server diff --git a/cereal/visionipc/visionipc_server.cc b/cereal/visionipc/visionipc_server.cc new file mode 100644 index 000000000..7c64cd839 --- /dev/null +++ b/cereal/visionipc/visionipc_server.cc @@ -0,0 +1,179 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "messaging.hpp" +#include "ipc.h" +#include "visionipc_server.h" + +std::string get_endpoint_name(std::string name, VisionStreamType type){ + if (messaging_use_zmq()){ + assert(name == "camerad"); + return std::to_string(9000 + static_cast(type)); + } else { + return "visionipc_" + name + "_" + std::to_string(type); + } +} + +VisionIpcServer::VisionIpcServer(std::string name, cl_device_id device_id, cl_context ctx) : name(name), device_id(device_id), ctx(ctx) { + msg_ctx = Context::create(); + + std::random_device rd("/dev/urandom"); + std::uniform_int_distribution distribution(0,std::numeric_limits::max()); + server_id = distribution(rd); +} + +void VisionIpcServer::create_buffers(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height){ + // TODO: assert that this type is not created yet + assert(num_buffers < VISIONIPC_MAX_FDS); + int aligned_w = 0, aligned_h = 0; + + size_t size = 0; + size_t stride = 0; // Only used for RGB + + if (rgb) { + visionbuf_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h); + size = (size_t)aligned_w * (size_t)aligned_h * 3; + stride = aligned_w * 3; + } else { + size = width * height * 3 / 2; + } + + + // Create map + alloc requested buffers + for (size_t i = 0; i < num_buffers; i++){ + VisionBuf* buf = new VisionBuf(); + buf->allocate(size); + buf->idx = i; + buf->type = type; + + if (device_id) buf->init_cl(device_id, ctx); + + rgb ? buf->init_rgb(width, height, stride) : buf->init_yuv(width, height); + + buffers[type].push_back(buf); + } + + cur_idx[type] = 0; + + // Create msgq publisher for each of the `name` + type combos + // TODO: compute port number directly if using zmq + sockets[type] = PubSocket::create(msg_ctx, get_endpoint_name(name, type), false); +} + + +void VisionIpcServer::start_listener(){ + listener_thread = std::thread(&VisionIpcServer::listener, this); +} + + +void VisionIpcServer::listener(){ + std::cout << "Starting listener for: " << name << std::endl; + + std::string path = "/tmp/visionipc_" + name; + int sock = ipc_bind(path.c_str()); + assert(sock >= 0); + + while (!should_exit){ + // Wait for incoming connection + struct pollfd polls[1] = {{0}}; + polls[0].fd = sock; + polls[0].events = POLLIN; + + int ret = poll(polls, 1, 100); + if (ret < 0) { + if (errno == EINTR || errno == EAGAIN) continue; + std::cout << "poll failed, stopping listener" << std::endl; + break; + } + + if (should_exit) break; + if (!polls[0].revents) { + continue; + } + + // Handle incoming request + int fd = accept(sock, NULL, NULL); + assert(fd >= 0); + + VisionStreamType type = VisionStreamType::VISION_STREAM_MAX; + int r = ipc_sendrecv_with_fds(false, fd, &type, sizeof(type), nullptr, 0, nullptr); + assert(r == sizeof(type)); + if (buffers.count(type) <= 0) { + std::cout << "got request for invalid buffer type: " << type << std::endl; + close(fd); + continue; + } + + int fds[VISIONIPC_MAX_FDS]; + int num_fds = buffers[type].size(); + VisionBuf bufs[VISIONIPC_MAX_FDS]; + + for (int i = 0; i < num_fds; i++){ + fds[i] = buffers[type][i]->fd; + bufs[i] = *buffers[type][i]; + + // Remove some private openCL/ion metadata + bufs[i].buf_cl = 0; + bufs[i].copy_q = 0; + bufs[i].handle = 0; + bufs[i].owner = false; + + bufs[i].server_id = server_id; + } + + r = ipc_sendrecv_with_fds(true, fd, &bufs, sizeof(VisionBuf) * num_fds, fds, num_fds, nullptr); + + close(fd); + } + + std::cout << "Stopping listener for: " << name << std::endl; + close(sock); +} + + + +VisionBuf * VisionIpcServer::get_buffer(VisionStreamType type){ + // Do we want to keep track if the buffer has been sent out yet and warn user? + assert(buffers.count(type)); + auto b = buffers[type]; + return b[cur_idx[type]++ % b.size()]; +} + +void VisionIpcServer::send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync){ + if (sync) buf->sync(VISIONBUF_SYNC_FROM_DEVICE); + assert(buffers.count(buf->type)); + assert(buf->idx < buffers[buf->type].size()); + + // Send over correct msgq socket + VisionIpcPacket packet = {0}; + packet.server_id = server_id; + packet.idx = buf->idx; + packet.extra = *extra; + + sockets[buf->type]->send((char*)&packet, sizeof(packet)); +} + +VisionIpcServer::~VisionIpcServer(){ + should_exit = true; + listener_thread.join(); + + // VisionBuf cleanup + for( auto const& [type, buf] : buffers ) { + for (VisionBuf* b : buf){ + b->free(); + delete b; + } + } + + // Messaging cleanup + for( auto const& [type, sock] : sockets ) { + delete sock; + } + delete msg_ctx; +} diff --git a/cereal/visionipc/visionipc_server.h b/cereal/visionipc/visionipc_server.h new file mode 100644 index 000000000..4472acf61 --- /dev/null +++ b/cereal/visionipc/visionipc_server.h @@ -0,0 +1,42 @@ +#pragma once +#include +#include +#include +#include +#include + +#include "messaging.hpp" +#include "visionipc.h" +#include "visionbuf.h" + +std::string get_endpoint_name(std::string name, VisionStreamType type); + +class VisionIpcServer { + private: + cl_device_id device_id = nullptr; + cl_context ctx = nullptr; + uint64_t server_id; + + std::atomic should_exit = false; + std::string name; + std::thread listener_thread; + + std::map > cur_idx; + std::map > buffers; + std::map > idxs; + + Context * msg_ctx; + std::map sockets; + + void listener(void); + + public: + VisionIpcServer(std::string name, cl_device_id device_id=nullptr, cl_context ctx=nullptr); + ~VisionIpcServer(); + + VisionBuf * get_buffer(VisionStreamType type); + + void create_buffers(VisionStreamType type, size_t num_buffers, bool rgb, size_t width, size_t height); + void send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync=true); + void start_listener(); +}; diff --git a/cereal/visionipc/visionipc_tests.cc b/cereal/visionipc/visionipc_tests.cc new file mode 100644 index 000000000..c2cd43da2 --- /dev/null +++ b/cereal/visionipc/visionipc_tests.cc @@ -0,0 +1,135 @@ +#include +#include + +#include "catch2/catch.hpp" +#include "visionipc_server.h" +#include "visionipc_client.h" + +static void zmq_sleep(int milliseconds=1000){ + if (messaging_use_zmq()){ + std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); + } +} + +TEST_CASE("Connecting"){ + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, 1, false, 100, 100); + server.start_listener(); + + VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false); + REQUIRE(client.connect()); + + REQUIRE(client.connected); +} + +TEST_CASE("Check buffers"){ + size_t width = 100, height = 200, num_buffers = 5; + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, num_buffers, false, width, height); + server.start_listener(); + + VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false); + REQUIRE(client.connect()); + + REQUIRE(client.buffers[0].width == width); + REQUIRE(client.buffers[0].height == height); + REQUIRE(client.buffers[0].len); + REQUIRE(client.num_buffers == num_buffers); +} + +TEST_CASE("Check yuv/rgb"){ + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, 1, false, 100, 100); + server.create_buffers(VISION_STREAM_RGB_BACK, 1, true, 100, 100); + server.start_listener(); + + VisionIpcClient client_yuv = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false); + VisionIpcClient client_rgb = VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, false); + client_yuv.connect(); + client_rgb.connect(); + + REQUIRE(client_rgb.buffers[0].rgb == true); + REQUIRE(client_yuv.buffers[0].rgb == false); +} + +TEST_CASE("Send single buffer"){ + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100); + server.start_listener(); + + VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false); + REQUIRE(client.connect()); + zmq_sleep(); + + VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK); + REQUIRE(buf != nullptr); + + *((uint64_t*)buf->addr) = 1234; + + VisionIpcBufExtra extra = {0}; + extra.frame_id = 1337; + + server.send(buf, &extra); + + VisionIpcBufExtra extra_recv = {0}; + VisionBuf * recv_buf = client.recv(&extra_recv); + REQUIRE(recv_buf != nullptr); + REQUIRE(*(uint64_t*)recv_buf->addr == 1234); + REQUIRE(extra_recv.frame_id == extra.frame_id); +} + + +TEST_CASE("Test no conflate"){ + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100); + server.start_listener(); + + VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, false); + REQUIRE(client.connect()); + zmq_sleep(); + + VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK); + REQUIRE(buf != nullptr); + + VisionIpcBufExtra extra = {0}; + extra.frame_id = 1; + server.send(buf, &extra); + extra.frame_id = 2; + server.send(buf, &extra); + + VisionIpcBufExtra extra_recv = {0}; + VisionBuf * recv_buf = client.recv(&extra_recv); + REQUIRE(recv_buf != nullptr); + REQUIRE(extra_recv.frame_id == 1); + + recv_buf = client.recv(&extra_recv); + REQUIRE(recv_buf != nullptr); + REQUIRE(extra_recv.frame_id == 2); +} + +TEST_CASE("Test conflate"){ + VisionIpcServer server("camerad"); + server.create_buffers(VISION_STREAM_YUV_BACK, 1, true, 100, 100); + server.start_listener(); + + VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true); + REQUIRE(client.connect()); + zmq_sleep(); + + VisionBuf * buf = server.get_buffer(VISION_STREAM_YUV_BACK); + REQUIRE(buf != nullptr); + + VisionIpcBufExtra extra = {0}; + extra.frame_id = 1; + server.send(buf, &extra); + extra.frame_id = 2; + server.send(buf, &extra); + + VisionIpcBufExtra extra_recv = {0}; + VisionBuf * recv_buf = client.recv(&extra_recv); + REQUIRE(recv_buf != nullptr); + REQUIRE(extra_recv.frame_id == 2); + + recv_buf = client.recv(&extra_recv); + REQUIRE(recv_buf == nullptr); +} diff --git a/common/api/__init__.py b/common/api/__init__.py index b030c3333..365e4f5ea 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -27,7 +27,11 @@ class Api(): 'iat': now, 'exp': now + timedelta(hours=1) } - return jwt.encode(payload, self.private_key, algorithm='RS256').decode('utf8') + token = jwt.encode(payload, self.private_key, algorithm='RS256') + if isinstance(token, bytes): + token = token.decode('utf8') + return token + def api_get(endpoint, method='GET', timeout=None, access_token=None, **params): backend = "https://api.commadotai.com/" diff --git a/common/basedir.py b/common/basedir.py index a728eaac1..8be1cf6af 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,8 +1,11 @@ import os -BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) +from pathlib import Path from selfdrive.hardware import PC + +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) + if PC: - PERSIST = os.path.join(BASEDIR, "persist") + PERSIST = os.path.join(str(Path.home()), ".comma", "persist") else: PERSIST = "/persist" diff --git a/common/params.py b/common/params.py index 5a7a0d410..0acb61fa6 100644 --- a/common/params.py +++ b/common/params.py @@ -2,3 +2,22 @@ from common.params_pyx import Params, UnknownKeyName, put_nonblocking # pylint: assert Params assert UnknownKeyName assert put_nonblocking + +if __name__ == "__main__": + import sys + from common.params_pyx import keys # pylint: disable=no-name-in-module, import-error + + params = Params() + if len(sys.argv) == 3: + name = sys.argv[1] + val = sys.argv[2] + assert name.encode("utf-8") in keys.keys(), f"unknown param: {name}" + print(f"SET: {name} = {val}") + params.put(name, val) + elif len(sys.argv) == 2: + name = sys.argv[1] + assert name.encode("utf-8") in keys.keys(), f"unknown param: {name}" + print(f"GET: {name} = {params.get(name)}") + else: + for k in keys.keys(): + print(f"GET: {k} = {params.get(k)}") diff --git a/common/params_pxd.pxd b/common/params_pxd.pxd index e437a09b0..9faa117cb 100644 --- a/common/params_pxd.pxd +++ b/common/params_pxd.pxd @@ -4,7 +4,7 @@ from libcpp cimport bool cdef extern from "selfdrive/common/params.cc": pass -cdef extern from "selfdrive/common/util.c": +cdef extern from "selfdrive/common/util.cc": pass cdef extern from "selfdrive/common/params.h": diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index b104011e2..cae66468a 100755 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -31,9 +31,11 @@ keys = { b"GitCommit": [TxType.PERSISTENT], b"GitRemote": [TxType.PERSISTENT], b"GithubSshKeys": [TxType.PERSISTENT], + b"HardwareSerial": [TxType.PERSISTENT], b"HasAcceptedTerms": [TxType.PERSISTENT], b"HasCompletedSetup": [TxType.PERSISTENT], b"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], + b"IMEI": [TxType.PERSISTENT], b"IsLdwEnabled": [TxType.PERSISTENT], b"IsMetric": [TxType.PERSISTENT], b"IsOffroad": [TxType.CLEAR_ON_MANAGER_START], @@ -42,8 +44,9 @@ keys = { b"IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], b"IsUploadRawEnabled": [TxType.PERSISTENT], b"LastAthenaPingTime": [TxType.PERSISTENT], - b"LastUpdateTime": [TxType.PERSISTENT], + b"LastGPSPosition": [TxType.PERSISTENT], b"LastUpdateException": [TxType.PERSISTENT], + b"LastUpdateTime": [TxType.PERSISTENT], b"LiveParameters": [TxType.PERSISTENT], b"OpenpilotEnabledToggle": [TxType.PERSISTENT], b"LaneChangeEnabled": [TxType.PERSISTENT], @@ -55,11 +58,14 @@ keys = { b"ReleaseNotes": [TxType.PERSISTENT], b"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], b"SubscriberInfo": [TxType.PERSISTENT], + b"SshEnabled": [TxType.PERSISTENT], b"TermsVersion": [TxType.PERSISTENT], + b"Timezone": [TxType.PERSISTENT], b"TrainingVersion": [TxType.PERSISTENT], b"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], b"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START], b"Version": [TxType.PERSISTENT], + b"VisionRadarToggle": [TxType.PERSISTENT], b"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], b"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START], b"Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START], @@ -70,6 +76,7 @@ keys = { b"Offroad_NeosUpdate": [TxType.CLEAR_ON_MANAGER_START], b"Offroad_UpdateFailed": [TxType.CLEAR_ON_MANAGER_START], b"Offroad_HardwareUnsupported": [TxType.CLEAR_ON_MANAGER_START], + b"ForcePowerDown": [TxType.CLEAR_ON_MANAGER_START], } def ensure_bytes(v): diff --git a/common/spinner.py b/common/spinner.py index 1232371d8..cde334203 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -16,7 +16,7 @@ class Spinner(): def __enter__(self): return self - def update(self, spinner_text): + def update(self, spinner_text: str): if self.spinner_proc is not None: self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n") try: @@ -24,6 +24,9 @@ class Spinner(): except BrokenPipeError: pass + def update_progress(self, cur: int, total: int): + self.update(str(int(100 * cur / total))) + def close(self): if self.spinner_proc is not None: try: diff --git a/common/transformations/SConscript b/common/transformations/SConscript index adc9642a4..ee9b9a2b7 100644 --- a/common/transformations/SConscript +++ b/common/transformations/SConscript @@ -1,3 +1,6 @@ -Import('envCython') +Import('env', 'envCython') + +transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc']) +Export('transformations') envCython.Program('transformations.so', 'transformations.pyx') diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc index 086219d23..8d1a304a2 100644 --- a/common/transformations/orientation.cc +++ b/common/transformations/orientation.cc @@ -39,7 +39,7 @@ Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat){ return quat.toRotationMatrix(); } -Eigen::Quaterniond rot2quat(Eigen::Matrix3d rot){ +Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot){ return ensure_unique(Eigen::Quaterniond(rot)); } @@ -47,7 +47,7 @@ Eigen::Matrix3d euler2rot(Eigen::Vector3d euler){ return quat2rot(euler2quat(euler)); } -Eigen::Vector3d rot2euler(Eigen::Matrix3d rot){ +Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot){ return quat2euler(rot2quat(rot)); } @@ -141,7 +141,3 @@ Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){ return {phi, theta, psi}; } - - -int main(void){ -} diff --git a/common/transformations/orientation.hpp b/common/transformations/orientation.hpp index da95f7099..ebd7da0ae 100644 --- a/common/transformations/orientation.hpp +++ b/common/transformations/orientation.hpp @@ -8,9 +8,9 @@ Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat); Eigen::Quaterniond euler2quat(Eigen::Vector3d euler); Eigen::Vector3d quat2euler(Eigen::Quaterniond quat); Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat); -Eigen::Quaterniond rot2quat(Eigen::Matrix3d rot); +Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot); Eigen::Matrix3d euler2rot(Eigen::Vector3d euler); -Eigen::Vector3d rot2euler(Eigen::Matrix3d rot); +Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot); Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw); Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle); Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose); diff --git a/installer/updater/updater.cc b/installer/updater/updater.cc index d737ea248..6dd8d0289 100644 --- a/installer/updater/updater.cc +++ b/installer/updater/updater.cc @@ -12,6 +12,7 @@ #include #include #include +#include #include #include @@ -30,7 +31,7 @@ #include "common/framebuffer.h" #include "common/touch.h" -#include "common/utilpp.h" +#include "common/util.h" #define USER_AGENT "NEOSUpdater-0.2" @@ -183,7 +184,7 @@ struct Updater { int fb_w, fb_h; - FramebufferState *fb = NULL; + std::unique_ptr fb; NVGcontext *vg = NULL; int font_regular; int font_semibold; @@ -227,11 +228,9 @@ struct Updater { void ui_init() { touch_init(&touch); - fb = framebuffer_init("updater", 0x00001000, false, - &fb_w, &fb_h); - assert(fb); + fb = std::make_unique("updater", 0x00001000, false, &fb_w, &fb_h); - framebuffer_set_power(fb, HWC_POWER_MODE_NORMAL); + fb->set_power(HWC_POWER_MODE_NORMAL); vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(vg); @@ -465,7 +464,7 @@ struct Updater { while(battery_cap < min_battery_cap) { battery_cap = battery_capacity(); battery_cap_text = std::to_string(battery_cap); - usleep(1000000); + util::sleep_for(1000); } set_running(); } @@ -483,7 +482,7 @@ struct Updater { while(battery_cap < min_battery_cap) { battery_cap = battery_capacity(); battery_cap_text = std::to_string(battery_cap); - usleep(1000000); + util::sleep_for(1000); } set_running(); } @@ -751,12 +750,12 @@ struct Updater { glDisable(GL_BLEND); - framebuffer_swap(fb); + fb->swap(); assert(glGetError() == GL_NO_ERROR); // no simple way to do 30fps vsync with surfaceflinger... - usleep(30000); + util::sleep_for(30); } if (update_thread_handle.joinable()) { diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 8fe3cbf61..a5298d9d0 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -8,11 +8,6 @@ source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -function tici_init { - sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor' - sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor' -} - function two_init { # Wifi scan wpa_cli IFNAME=wlan0 SCAN @@ -110,6 +105,74 @@ function two_init { fi } +function tici_init { + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor' + sudo su -c 'echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu4/governor' + + # set success flag for current boot slot + sudo abctl --set_success + + # Check if AGNOS update is required + if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then + # Get number of slot to switch to + CUR_SLOT=$(abctl --boot_slot) + if [[ "$CUR_SLOT" == "_a" ]]; then + OTHER_SLOT="_b" + OTHER_SLOT_NUMBER="1" + else + OTHER_SLOT="_a" + OTHER_SLOT_NUMBER="0" + fi + echo "Cur slot $CUR_SLOT, target $OTHER_SLOT" + + # Get expected hashes from manifest + MANIFEST="$DIR/selfdrive/hardware/tici/agnos.json" + SYSTEM_HASH_EXPECTED=$(jq -r ".[] | select(.name == \"system\") | .hash_raw" $MANIFEST) + SYSTEM_SIZE=$(jq -r ".[] | select(.name == \"system\") | .size" $MANIFEST) + BOOT_HASH_EXPECTED=$(jq -r ".[] | select(.name == \"boot\") | .hash_raw" $MANIFEST) + BOOT_SIZE=$(jq -r ".[] | select(.name == \"boot\") | .size" $MANIFEST) + echo "Expected hashes:" + echo "System: $SYSTEM_HASH_EXPECTED" + echo "Boot: $BOOT_HASH_EXPECTED" + + # Read hashes from alternate partitions, should already be flashed by updated + SYSTEM_HASH=$(dd if="/dev/disk/by-partlabel/system$OTHER_SLOT" bs=1 skip="$SYSTEM_SIZE" count=64 2>/dev/null) + BOOT_HASH=$(dd if="/dev/disk/by-partlabel/boot$OTHER_SLOT" bs=1 skip="$BOOT_SIZE" count=64 2>/dev/null) + echo "Found hashes:" + echo "System: $SYSTEM_HASH" + echo "Boot: $BOOT_HASH" + + if [[ "$SYSTEM_HASH" == "$SYSTEM_HASH_EXPECTED" && "$BOOT_HASH" == "$BOOT_HASH_EXPECTED" ]]; then + echo "Swapping active slot to $OTHER_SLOT_NUMBER" + + # Clean hashes before swapping to prevent looping + dd if=/dev/zero of="/dev/disk/by-partlabel/system$OTHER_SLOT" bs=1 seek="$SYSTEM_SIZE" count=64 + dd if=/dev/zero of="/dev/disk/by-partlabel/boot$OTHER_SLOT" bs=1 seek="$BOOT_SIZE" count=64 + sync + + abctl --set_active "$OTHER_SLOT_NUMBER" + + sleep 1 + sudo reboot + else + echo "Hash mismatch, downloading agnos" + if $DIR/selfdrive/hardware/tici/agnos.py $MANIFEST; then + echo "Download done, swapping active slot to $OTHER_SLOT_NUMBER" + + # Clean hashes before swapping to prevent looping + dd if=/dev/zero of="/dev/disk/by-partlabel/system$OTHER_SLOT" bs=1 seek="$SYSTEM_SIZE" count=64 + dd if=/dev/zero of="/dev/disk/by-partlabel/boot$OTHER_SLOT" bs=1 seek="$BOOT_SIZE" count=64 + sync + + abctl --set_active "$OTHER_SLOT_NUMBER" + fi + + sleep 1 + sudo reboot + fi + fi +} + function launch { # Remove orphaned git lock if it exists on boot [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock @@ -144,6 +207,7 @@ function launch { echo "Restarting launch script ${LAUNCHER_LOCATION}" unset REQUIRED_NEOS_VERSION + unset AGNOS_VERSION exec "${LAUNCHER_LOCATION}" else echo "openpilot backup found, not updating" @@ -153,17 +217,17 @@ function launch { fi fi - # comma two init + # handle pythonpath + ln -sfn $(pwd) /data/pythonpath + export PYTHONPATH="$PWD" + + # hardware specific init if [ -f /EON ]; then two_init elif [ -f /TICI ]; then tici_init fi - # handle pythonpath - ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD" - # write tmux scrollback to a file tmux capture-pane -pq -S-1000 > /tmp/launch_log diff --git a/launch_env.sh b/launch_env.sh index ca8f4e8a1..d75f8d0dd 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -10,6 +10,10 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then export REQUIRED_NEOS_VERSION="15-1" fi +if [ -z "$AGNOS_VERSION" ]; then + export AGNOS_VERSION="0.6" +fi + if [ -z "$PASSIVE" ]; then export PASSIVE="1" fi diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 9935584bf..b067c3e6d 100644 Binary files a/models/dmonitoring_model_q.dlc and b/models/dmonitoring_model_q.dlc differ diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc index d3c8f7c00..ebb51d1db 100644 --- a/opendbc/acura_rdx_2020_can_generated.dbc +++ b/opendbc/acura_rdx_2020_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/can/SConscript b/opendbc/can/SConscript index 4e67fc797..309404734 100644 --- a/opendbc/can/SConscript +++ b/opendbc/can/SConscript @@ -18,5 +18,8 @@ libdbc = env.SharedLibrary('libdbc', ["dbc.cc", "parser.cc", "packer.cc", "commo # Build packer and parser lenv = envCython.Clone() lenv["LINKFLAGS"] += [libdbc[0].get_labspath()] -lenv.Program('parser_pyx.so', 'parser_pyx.pyx') -lenv.Program('packer_pyx.so', 'packer_pyx.pyx') +parser = lenv.Program('parser_pyx.so', 'parser_pyx.pyx') +packer = lenv.Program('packer_pyx.so', 'packer_pyx.pyx') + +lenv.Depends(parser, libdbc) +lenv.Depends(packer, libdbc) diff --git a/opendbc/can/common.h b/opendbc/can/common.h index dd91a6ce7..4daf8164d 100644 --- a/opendbc/can/common.h +++ b/opendbc/can/common.h @@ -56,7 +56,7 @@ public: const std::vector &sigoptions); void UpdateCans(uint64_t sec, const capnp::List::Reader& cans); void UpdateValid(uint64_t sec); - void update_string(std::string data, bool sendcan); + void update_string(const std::string &data, bool sendcan); std::vector query_latest(); }; diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index c6d85d9ee..44baee5e9 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -208,7 +208,7 @@ void CANParser::UpdateValid(uint64_t sec) { } } -void CANParser::update_string(std::string data, bool sendcan) { +void CANParser::update_string(const std::string &data, bool sendcan) { // format for board, make copy due to alignment issues, will be freed on out of scope auto amsg = kj::heapArray((data.length() / sizeof(capnp::word)) + 1); memcpy(amsg.begin(), data.data(), data.length()); diff --git a/opendbc/chrysler_pacifica_2017_hybrid.dbc b/opendbc/chrysler_pacifica_2017_hybrid.dbc index 54e4819f2..245eefb8b 100644 --- a/opendbc/chrysler_pacifica_2017_hybrid.dbc +++ b/opendbc/chrysler_pacifica_2017_hybrid.dbc @@ -300,6 +300,7 @@ BO_ 501 DASHBOARD: 8 XXX SG_ ACC_DISTANCE_CONFIG_1 : 1|2@0+ (1,0) [0|3] "" XXX SG_ ACC_DISTANCE_CONFIG_2 : 41|2@0+ (1,0) [0|3] "" XXX SG_ SPEED_DIGITAL : 63|8@0+ (1,0) [0|255] "mph" XXX + SG_ CRUISE_STATE : 38|3@0+ (1,0) [0|7] "" XXX BO_ 639 NEW_MSG_27f: 8 XXX SG_ INCREASING : 47|8@0+ (1,0) [0|255] "" XXX @@ -425,9 +426,11 @@ CM_ SG_ 500 ACC_STATUS_1 "2 briefly (9 packets) when ACC goes to green, 1 help w CM_ SG_ 500 BRAKE_MAYBE "2046 in non-ACC and non-decel. Signal on deceleration. 818 for already stopped break."; CM_ SG_ 500 ACC_STATUS_2 "set to 1 in non-ACC, 3 when ACC enabled (white icon), and 7 when ACC in use (green icon)"; CM_ SG_ 500 BRAKE_BOOL_1 "set to 1 when ACC decel. 0 on non-ACC and accel."; +CM_ SG_ 501 CRUISE_STATE "may just be an icon, but seems to indicate different cruise modes: ACC and Non-ACC and engaged state for both."; CM_ SG_ 625 SPEED "zero on non-acc drives"; CM_ SG_ 625 ACCEL_PERHAPS "set to 7767 on non-ACC drives. ACC drive 40k is constant speed, 42k is accelerating"; CM_ SG_ 268 BRAKE_PERHAPS "triggers only on ACC braking"; CM_ SG_ 384 NEW_SIGNAL_1 "set in ACC gas driving. not set in electric human. not sure about gas human driving."; +VAL_ 501 CRUISE_STATE 0 "Off" 1 "CC On" 2 "CC Engaged" 3 "ACC On" 4 "ACC Engaged"; VAL_ 746 PRNDL 5 "L" 4 "D" 3 "N" 2 "R" 1 "P" ; VAL_ 792 TURN_SIGNALS 2 "Right" 1 "Left" ; diff --git a/opendbc/honda_accord_lx15t_2018_can_generated.dbc b/opendbc/honda_accord_lx15t_2018_can_generated.dbc index 25b4c15a3..938b47427 100644 --- a/opendbc/honda_accord_lx15t_2018_can_generated.dbc +++ b/opendbc/honda_accord_lx15t_2018_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc index 6407b49fe..970f62bd8 100644 --- a/opendbc/honda_accord_s2t_2018_can_generated.dbc +++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index 0443f2d5c..ddee5dc16 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc index fecfc5886..31e1ddfd7 100644 --- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc +++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc index 2e6fbf8bf..8637a426a 100644 --- a/opendbc/honda_crv_ex_2017_can_generated.dbc +++ b/opendbc/honda_crv_ex_2017_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_crv_hybrid_2019_can_generated.dbc b/opendbc/honda_crv_hybrid_2019_can_generated.dbc index e29d03201..59850d160 100644 --- a/opendbc/honda_crv_hybrid_2019_can_generated.dbc +++ b/opendbc/honda_crv_hybrid_2019_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc index 7287af23d..59c5735c1 100644 --- a/opendbc/honda_insight_ex_2019_can_generated.dbc +++ b/opendbc/honda_insight_ex_2019_can_generated.dbc @@ -48,6 +48,7 @@ BO_ 228 STEERING_CONTROL: 5 EON SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS diff --git a/opendbc/hyundai_kia_generic.dbc b/opendbc/hyundai_kia_generic.dbc index 13ee8f3a1..a56ceb708 100644 --- a/opendbc/hyundai_kia_generic.dbc +++ b/opendbc/hyundai_kia_generic.dbc @@ -996,7 +996,7 @@ BO_ 1342 LKAS12: 6 LDWS_LKAS SG_ CF_LkasTsrSpeed_Display_Navi : 24|8@1+ (1.0,0.0) [0.0|255.0] "" BCM,CLU SG_ CF_Lkas_TsrAddinfo_Display : 32|2@1+ (1.0,0.0) [0.0|3.0] "" CLU SG_ CF_LkasDawStatus : 40|3@1+ (1,0) [0|7] "" CLU - SG_ CF_Lkas_Daw_USM : 39|3@1+ (1,0) [0|7] "" CLU + SG_ CF_Lkas_Daw_USM : 37|3@1+ (1,0) [0|7] "" CLU BO_ 1338 TMU_GW_E_01: 8 CLU SG_ CF_Gway_TeleReqDrLock : 0|2@1+ (1.0,0.0) [0.0|3.0] "" BCM @@ -1470,19 +1470,24 @@ BO_ 1186 FRT_RADAR11: 2 FCA SG_ CF_FCA_Equip_Front_Radar : 0|3@1+ (1,0) [0|7] "" LDWS_LKAS,LDW_LKA,ESC BO_ 905 SCC14: 8 SCC - SG_ ComfortBandUpper : 0|6@1+ (0.0986,-4.14) [0|1.26] "m/s^2" ESC - SG_ ComfortBandLower : 6|6@1+ (0.0986,-4.14) [0|1.26] "m/s^2" ESC - SG_ JerkUpperLimit : 12|7@1+ (1,0) [0|12.7] "m/s^3" ESC + SG_ ComfortBandUpper : 0|6@1+ (0.02,0) [0|1.26] "m/s^2" ESC + SG_ ComfortBandLower : 6|6@1+ (0.02,0) [0|1.26] "m/s^2" ESC + SG_ JerkUpperLimit : 12|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC BO_ 1157 LFAHDA_MFC: 4 XXX SG_ HDA_USM : 0|2@1+ (1,0) [0|3] "" XXX - SG_ ACTIVE2 : 4|2@0+ (1,0) [0|3] "" XXX - SG_ LFA_SysWarning : 16|2@1+ (1,0) [0|3] "" XXX - SG_ ACTIVE : 25|1@1+ (1,0) [0|3] "" XXX - SG_ LFA_USM : 28|2@1+ (1,0) [0|3] "" XXX + SG_ HDA_Active : 2|1@1+ (1,0) [0|1] "" XXX + SG_ HDA_Icon_State : 3|2@1+ (1,0) [0|3] "" XXX + SG_ HDA_Chime : 7|1@1+ (1,0) [0|1] "" XXX + SG_ HDA_VSetReq : 8|8@1+ (1,0) [0|255] "km/h" XXX + SG_ LFA_SysWarning : 16|3@1+ (1,0) [0|7] "" XXX + SG_ NEW_SIGNAL_1 : 20|3@1+ (1,0) [0|7] "" XXX + SG_ LFA_Icon_State : 24|2@1+ (1,0) [0|3] "" XXX + SG_ LFA_USM : 27|2@1+ (1,0) [0|3] "" XXX + SG_ HDA_SysWarning : 29|2@1+ (1,0) [0|3] "" XXX BO_ 913 BCM_PO_11: 8 Vector__XXX SG_ BCM_Door_Dri_Status : 5|1@0+ (1,0) [0|1] "" PT_ESC_ABS @@ -1565,7 +1570,7 @@ BO_ 352 AHB1: 8 iBAU SG_ CF_Ahb_Bzzr : 26|1@1+ (1,0) [0|1] "" Vector__XXX SG_ CF_Ahb_ChkSum : 56|8@1+ (1,0) [0|255] "" Vector__XXX -BO_ 1191 4a7MFC: 8 XXX +BO_ 1191 MFC_4a7: 8 XXX SG_ PAINT1 : 0|1@0+ (1,0) [0|1] "" XXX BO_ 1162 BCA11: 8 BCW @@ -1575,9 +1580,11 @@ BO_ 1162 BCA11: 8 BCW SG_ RCCA_Brake_Command : 29|1@1+ (1,0) [0|1] "" iBAU SG_ Check_Sum : 56|8@1+ (1,0) [0|16] "" iBAU -BO_ 1136 P_STS: 6 CGW +BO_ 1136 P_STS: 8 CGW SG_ HCU1_STS : 6|2@1+ (1,0) [0|3] "" BCW,EPB,FCA,MDPS,SCC,iBAU SG_ HCU5_STS : 8|2@1+ (1,0) [0|3] "" EPB,FCA,MDPS,iBAU + SG_ Counter : 58|4@1+ (1,0) [0|15] "" MDPS + SG_ Checksum : 62|2@1+ (1,0) [0|3] "" MDPS BO_ 304 YRS11: 8 ACU SG_ CR_Yrs_Yr : 0|16@1+ (0.005,-163.84) [-163.84|163.83] "deg/s" CGW,iBAU @@ -1601,14 +1608,14 @@ BO_ 320 YRS12: 8 ACU BO_ 1173 YRS13: 8 ACU SG_ YRS_SeralNo : 16|48@1+ (1,0) [0|281474976710655] "" iBAU -BO_ 870 366_EMS: 8 EMS - SG_ N : 7|16@0+ (1,0.25) [0|16383.75] "rpm" XXX - SG_ EMS_Related : 23|16@0+ (1,0) [0|65535] "" XXX - SG_ TQFR : 39|8@0+ (0.390625,0) [0|99.6094] "%" XXX +BO_ 870 EMS_366: 8 EMS + SG_ TQI_1 : 0|8@1+ (0.390625,0) [0|99.6094] "%" MDPS + SG_ N : 8|16@1+ (0.25,0.0) [0.0|16383.75] "rpm" MDPS + SG_ TQI_2 : 24|8@1+ (0.390625,0) [0|99.6094] "%" MDPS SG_ VS : 40|8@1+ (1,0) [0|255] "km/h" MDPS SG_ SWI_IGK : 48|1@0+ (1,0) [0|1] "" XXX -BO_ 854 356: 8 XXX +BO_ 854 M_356: 8 XXX SG_ PAINT1 : 32|1@0+ (1,0) [0|1] "" XXX SG_ PAINT2 : 34|2@0+ (1,0) [0|1] "" XXX SG_ PAINT3 : 36|2@0+ (1,0) [0|3] "" XXX @@ -1633,3 +1640,7 @@ BO_ 1042 ICM_412h: 8 ICM SG_ PopupMessageOutput_8Level : 55|1@0+ (1,0) [0|1] "" Vector__XXX VAL_ 909 CF_VSM_Warn 2 "FCW" 3 "AEB"; +VAL_ 1157 LFA_Icon_State 0 "no_wheel" 1 "white_wheel" 2 "green_wheel" 3 "green_wheel_blink"; +VAL_ 1157 LFA_SysWarning 0 "no_message" 1 "switching_to_hda" 2 "switching_to_scc" 3 "lfa_error" 4 "check_hda" 5 "keep_hands_on_wheel_orange" 6 "keep_hands_on_wheel_red"; +VAL_ 1157 HDA_Icon_State 0 "no_hda" 1 "white_hda" 2 "green_hda"; +VAL_ 1157 HDA_SysWarning 0 "no_message" 1 "driving_convenience_systems_cancelled" 2 "highway_drive_assist_system_cancelled"; diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc index 54c812780..30646e552 100644 --- a/opendbc/lexus_ct200h_2018_pt_generated.dbc +++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc index 24aa11136..9f02ded0f 100644 --- a/opendbc/lexus_is_2018_pt_generated.dbc +++ b/opendbc/lexus_is_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc index 824d301d0..0f4ddab95 100644 --- a/opendbc/lexus_nx300_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc index 94e6818c8..1f232541f 100644 --- a/opendbc/lexus_nx300h_2018_pt_generated.dbc +++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc index 58c21c595..d96711e42 100644 --- a/opendbc/lexus_rx_350_2016_pt_generated.dbc +++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc index e8a358f46..3d1252dc2 100644 --- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc index fb6a569d4..f33f079f8 100644 --- a/opendbc/toyota_avalon_2017_pt_generated.dbc +++ b/opendbc/toyota_avalon_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc index 72f3d8bde..17ef5922d 100644 --- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc index d76e3cd71..2f2dc8bdb 100644 --- a/opendbc/toyota_corolla_2017_pt_generated.dbc +++ b/opendbc/toyota_corolla_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc index b81779fe7..6cf6d27cb 100644 --- a/opendbc/toyota_highlander_2017_pt_generated.dbc +++ b/opendbc/toyota_highlander_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc index 6e22e0e22..2d498cf1b 100644 --- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc +++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc index 6d77072b0..09ed0690f 100644 --- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc +++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc @@ -271,6 +271,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -333,6 +334,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -362,8 +364,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index 068766894..bf2f02e4f 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -271,6 +271,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -333,6 +334,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -362,8 +364,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc index 868381e5e..e44e776f5 100644 --- a/opendbc/toyota_prius_2017_pt_generated.dbc +++ b/opendbc/toyota_prius_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc index 19c5271d8..703726026 100644 --- a/opendbc/toyota_rav4_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc index bb98e0d30..99f724ea9 100644 --- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc +++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc index ad0d1f31d..06b9388bc 100644 --- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc +++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc @@ -254,6 +254,7 @@ BO_ 1410 VIN_PART_3: 8 CGW BO_ 1553 UI_SETTING: 8 XXX SG_ UNITS : 26|2@0+ (1,0) [0|3] "" XXX + SG_ ODOMETER : 43|20@0+ (1,0) [0|1048575] "" XXX BO_ 1556 STEERING_LEVERS: 8 XXX SG_ TURN_SIGNALS : 29|2@0+ (1,0) [0|3] "" XXX @@ -316,6 +317,7 @@ CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd"; CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set"; CM_ SG_ 37 STEER_RATE "factor is tbd"; CM_ SG_ 466 ACCEL_NET "net acceleration produced by the system, given ACCEL_CMD, road grade and other factors"; +CM_ SG_ 466 CRUISE_STATE "Active state is 8, if standstill is requested will switch to state 11(3 sec timer), after timer is elapsed will switch into state 7(standstill). If plus button was pressed - status 9, minus button pressed - status 10"; CM_ SG_ 467 SET_SPEED "43 kph are shown as 28mph, so conversion isn't perfect"; CM_ SG_ 467 LOW_SPEED_LOCKOUT "in low speed lockout, system would always disengage below 28mph"; CM_ SG_ 560 BRAKE_PRESSED "another brake pressed?"; @@ -345,8 +347,9 @@ CM_ SG_ 1163 OVSPVALL "-5 at start then 2 after 2 seconds"; CM_ SG_ 1163 OVSPVALM "-5 at start then 5 after 2 seconds"; CM_ SG_ 1163 OVSPVALH "-5 at start then 10 after 2 seconds"; CM_ SG_ 1163 TSRSPU "always 1"; +CM_ SG_ 1553 ODOMETER "Unit is dependent upon units signal"; -VAL_ 466 CRUISE_STATE 8 "active" 7 "standstill" 1 "off"; +VAL_ 466 CRUISE_STATE 11 "timer_3sec" 10 "adaptive click down" 9 "adaptive click up" 8 "adaptive engaged" 7 "standstill" 6 "non-adaptive click up" 5 "non-adaptive click down" 4 "non-adaptive hold down" 3 "non-adaptive hold up" 2 "non-adaptive being engaged" 1 "non-adaptive engaged" 0 "off"; VAL_ 467 LOW_SPEED_LOCKOUT 2 "low speed locked" 1 "ok"; VAL_ 614 STATE 3 "enabled" 1 "disabled"; VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left"; diff --git a/panda/.gitignore b/panda/.gitignore index 503ce91c7..2c453216a 100644 --- a/panda/.gitignore +++ b/panda/.gitignore @@ -13,6 +13,6 @@ pandacan.egg-info/ board/obj/ examples/output.csv .DS_Store -.vscode +.vscode* nosetests.xml .mypy_cache/ diff --git a/panda/board/boards/common.h b/panda/board/boards/common.h index 2f35c34fe..d09d5a48f 100644 --- a/panda/board/boards/common.h +++ b/panda/board/boards/common.h @@ -59,9 +59,10 @@ void peripherals_init(void){ RCC->APB1ENR |= RCC_APB1ENR_DACEN; RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // main counter RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // pedal and fan PWM - RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // gmlan_alt and IR PWM + RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // IR PWM RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; // k-line init RCC->APB1ENR |= RCC_APB1ENR_TIM6EN; // interrupt timer + RCC->APB1ENR |= RCC_APB1ENR_TIM12EN; // gmlan_alt RCC->APB1ENR |= RCC_APB1ENR_PWREN; // for RTC config RCC->APB2ENR |= RCC_APB2ENR_USART1EN; RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN; diff --git a/panda/board/boards/dos.h b/panda/board/boards/dos.h index 307ae7247..fd952cd21 100644 --- a/panda/board/boards/dos.h +++ b/panda/board/boards/dos.h @@ -62,7 +62,23 @@ void dos_set_phone_power(bool enabled){ } void dos_set_usb_power_mode(uint8_t mode) { - dos_set_bootkick(mode == USB_POWER_CDP); + bool valid = false; + switch (mode) { + case USB_POWER_CLIENT: + dos_set_bootkick(false); + valid = true; + break; + case USB_POWER_CDP: + dos_set_bootkick(true); + valid = true; + break; + default: + puts("Invalid USB power mode\n"); + break; + } + if (valid) { + usb_power_mode = mode; + } } void dos_set_gps_mode(uint8_t mode) { diff --git a/panda/board/build.mk b/panda/board/build.mk index 534d42339..c53f88600 100644 --- a/panda/board/build.mk +++ b/panda/board/build.mk @@ -4,13 +4,19 @@ CFLAGS += -Tstm32_flash.ld DFU_UTIL = "dfu-util" -# Compile fast charge (DCP) only not on EON +PC = 0 + ifeq (,$(wildcard /EON)) + ifeq (,$(wildcard /TICI)) + PC = 1 + endif +endif + +ifeq (1, $(PC)) BUILDER = DEV else CFLAGS += "-DEON" BUILDER = EON - DFU_UTIL = "tools/dfu-util-aarch64" endif #COMPILER_PATH = /home/batman/Downloads/gcc-arm-none-eabi-9-2020-q2-update/bin/ diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h index 2f2e56805..c5d111fe6 100644 --- a/panda/board/drivers/gmlan_alt.h +++ b/panda/board/drivers/gmlan_alt.h @@ -122,23 +122,23 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) { return len; } -void TIM4_IRQ_Handler(void); +void TIM12_IRQ_Handler(void); -void setup_timer4(void) { +void setup_timer(void) { // register interrupt - REGISTER_INTERRUPT(TIM4_IRQn, TIM4_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN) + REGISTER_INTERRUPT(TIM8_BRK_TIM12_IRQn, TIM12_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN) // setup - register_set(&(TIM4->PSC), (48-1), 0xFFFFU); // Tick on 1 us - register_set(&(TIM4->CR1), TIM_CR1_CEN, 0x3FU); // Enable - register_set(&(TIM4->ARR), (30-1), 0xFFFFU); // 33.3 kbps + register_set(&(TIM12->PSC), (48-1), 0xFFFFU); // Tick on 1 us + register_set(&(TIM12->CR1), TIM_CR1_CEN, 0x3FU); // Enable + register_set(&(TIM12->ARR), (30-1), 0xFFFFU); // 33.3 kbps // in case it's disabled - NVIC_EnableIRQ(TIM4_IRQn); + NVIC_EnableIRQ(TIM8_BRK_TIM12_IRQn); // run the interrupt - register_set(&(TIM4->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt - TIM4->SR = 0; + register_set(&(TIM12->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt + TIM12->SR = 0; } int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms @@ -154,7 +154,7 @@ void gmlan_switch_init(int timeout_enable) { gmlan_switch_below_timeout = 1; set_gpio_mode(GPIOB, 13, MODE_OUTPUT); - setup_timer4(); + setup_timer(); inverted_bit_to_send = GMLAN_LOW; //We got initialized, set the output low } @@ -192,9 +192,9 @@ int gmlan_fail_count = 0; #define REQUIRED_SILENT_TIME 10 #define MAX_FAIL_COUNT 10 -void TIM4_IRQ_Handler(void) { +void TIM12_IRQ_Handler(void) { if (gmlan_alt_mode == BITBANG) { - if ((TIM4->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) { + if ((TIM12->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) { int read = get_gpio_input(GPIOB, 12); if (gmlan_silent_count < REQUIRED_SILENT_TIME) { if (read == 0) { @@ -236,13 +236,13 @@ void TIM4_IRQ_Handler(void) { if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) { set_bitbanged_gmlan(1); // recessive set_gpio_mode(GPIOB, 13, MODE_INPUT); - register_clear_bits(&(TIM4->DIER), TIM_DIER_UIE); // No update interrupt - register_set(&(TIM4->CR1), 0U, 0x3FU); // Disable timer + register_clear_bits(&(TIM12->DIER), TIM_DIER_UIE); // No update interrupt + register_set(&(TIM12->CR1), 0U, 0x3FU); // Disable timer gmlan_sendmax = -1; // exit } } } else if (gmlan_alt_mode == GPIO_SWITCH) { - if ((TIM4->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) { + if ((TIM12->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) { if ((can_timeout_counter == 0) && gmlan_switch_timeout_enable) { //it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output set_gpio_output(GPIOB, 13, GMLAN_LOW); @@ -266,7 +266,7 @@ void TIM4_IRQ_Handler(void) { } else { // Invalid GMLAN mode. Do not put a print statement here, way too fast to keep up with } - TIM4->SR = 0; + TIM12->SR = 0; } bool bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) { @@ -284,7 +284,7 @@ bool bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) { set_gpio_mode(GPIOB, 13, MODE_OUTPUT); // 33kbps - setup_timer4(); + setup_timer(); } return gmlan_send_ok; } diff --git a/panda/board/main.c b/panda/board/main.c index c6e4f0fce..0ce54f835 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -614,6 +614,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) case 0xf6: siren_enabled = (setup->b.wValue.w != 0U); break; + // **** 0xf7: set green led enabled + case 0xf7: + green_led_enabled = (setup->b.wValue.w != 0U); + break; default: puts("NO HANDLER "); puth(setup->b.bRequest); @@ -701,7 +705,7 @@ void TIM1_BRK_TIM9_IRQ_Handler(void) { fan_tick(); // set green LED to be controls allowed - current_board->set_led(LED_GREEN, controls_allowed); + current_board->set_led(LED_GREEN, controls_allowed | green_led_enabled); // turn off the blue LED, turned on by CAN // unless we are in power saving mode diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h index 7fa2c4bbb..ade4acac3 100644 --- a/panda/board/main_declarations.h +++ b/panda/board/main_declarations.h @@ -14,3 +14,4 @@ bool is_enumerated = 0; uint32_t heartbeat_counter = 0; uint32_t uptime_cnt = 0; bool siren_enabled = false; +bool green_led_enabled = false; diff --git a/panda/board/tools/dfu-util-aarch64 b/panda/board/tools/dfu-util-aarch64 deleted file mode 100755 index 250c592ad..000000000 Binary files a/panda/board/tools/dfu-util-aarch64 and /dev/null differ diff --git a/panda/board/tools/dfu-util-aarch64-linux b/panda/board/tools/dfu-util-aarch64-linux deleted file mode 100755 index b366f869a..000000000 Binary files a/panda/board/tools/dfu-util-aarch64-linux and /dev/null differ diff --git a/panda/board/tools/dfu-util-x86_64-linux b/panda/board/tools/dfu-util-x86_64-linux deleted file mode 100755 index 7be3dc3a7..000000000 Binary files a/panda/board/tools/dfu-util-x86_64-linux and /dev/null differ diff --git a/panda/board/tools/enter_download_mode.py b/panda/board/tools/enter_download_mode.py deleted file mode 100755 index 4e6a2b4cc..000000000 --- a/panda/board/tools/enter_download_mode.py +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python3 - - -import sys -import time -import usb1 - -def enter_download_mode(device): - handle = device.open() - handle.claimInterface(0) - - try: - handle.controlWrite(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd1, 0, 0, b'') - except (usb1.USBErrorIO, usb1.USBErrorPipe): - print("Device download mode enabled.") - time.sleep(1) - else: - print("Device failed to enter download mode.") - sys.exit(1) - -def find_first_panda(context=None): - context = context or usb1.USBContext() - for device in context.getDeviceList(skip_on_error=True): - if device.getVendorID() == 0xbbaa and device.getProductID() & 0xFF00 == 0xdd00: - return device - -if __name__ == "__main__": - panda_dev = find_first_panda() - if panda_dev is None: - print("no device found") - sys.exit(0) - print("found device") - enter_download_mode(panda_dev) diff --git a/panda/python/__init__.py b/panda/python/__init__.py index 936c975f0..b22b01027 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -676,3 +676,7 @@ class Panda(object): # ****************** Siren ***************** def set_siren(self, enabled): self._handle.controlWrite(Panda.REQUEST_OUT, 0xf6, int(enabled), 0, b'') + + # ****************** Debug ***************** + def set_green_led(self, enabled): + self._handle.controlWrite(Panda.REQUEST_OUT, 0xf7, int(enabled), 0, b'') diff --git a/phonelibs/android_frameworks_native/include/batteryservice/BatteryService.h b/phonelibs/android_frameworks_native/include/batteryservice/BatteryService.h deleted file mode 100644 index 3e6bfb820..000000000 --- a/phonelibs/android_frameworks_native/include/batteryservice/BatteryService.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * Copyright (C) 2015 The CyanogenMod Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_BATTERYSERVICE_H -#define ANDROID_BATTERYSERVICE_H - -#include -#include -#include -#include - -namespace android { - -// must be kept in sync with definitions in BatteryManager.java -enum { - BATTERY_STATUS_UNKNOWN = 1, // equals BatteryManager.BATTERY_STATUS_UNKNOWN constant - BATTERY_STATUS_CHARGING = 2, // equals BatteryManager.BATTERY_STATUS_CHARGING constant - BATTERY_STATUS_DISCHARGING = 3, // equals BatteryManager.BATTERY_STATUS_DISCHARGING constant - BATTERY_STATUS_NOT_CHARGING = 4, // equals BatteryManager.BATTERY_STATUS_NOT_CHARGING constant - BATTERY_STATUS_FULL = 5, // equals BatteryManager.BATTERY_STATUS_FULL constant -}; - -// must be kept in sync with definitions in BatteryManager.java -enum { - BATTERY_HEALTH_UNKNOWN = 1, // equals BatteryManager.BATTERY_HEALTH_UNKNOWN constant - BATTERY_HEALTH_GOOD = 2, // equals BatteryManager.BATTERY_HEALTH_GOOD constant - BATTERY_HEALTH_OVERHEAT = 3, // equals BatteryManager.BATTERY_HEALTH_OVERHEAT constant - BATTERY_HEALTH_DEAD = 4, // equals BatteryManager.BATTERY_HEALTH_DEAD constant - BATTERY_HEALTH_OVER_VOLTAGE = 5, // equals BatteryManager.BATTERY_HEALTH_OVER_VOLTAGE constant - BATTERY_HEALTH_UNSPECIFIED_FAILURE = 6, // equals BatteryManager.BATTERY_HEALTH_UNSPECIFIED_FAILURE constant - BATTERY_HEALTH_COLD = 7, // equals BatteryManager.BATTERY_HEALTH_COLD constant -}; - -// must be kept in sync with definitions in BatteryProperty.java -enum { - BATTERY_PROP_CHARGE_COUNTER = 1, // equals BatteryProperty.CHARGE_COUNTER constant - BATTERY_PROP_CURRENT_NOW = 2, // equals BatteryProperty.CURRENT_NOW constant - BATTERY_PROP_CURRENT_AVG = 3, // equals BatteryProperty.CURRENT_AVG constant - BATTERY_PROP_CAPACITY = 4, // equals BatteryProperty.CAPACITY constant - BATTERY_PROP_ENERGY_COUNTER = 5, // equals BatteryProperty.ENERGY_COUNTER constant -}; - -struct BatteryProperties { - bool chargerAcOnline; - bool chargerUsbOnline; - bool chargerWirelessOnline; - int maxChargingCurrent; - int batteryStatus; - int batteryHealth; - bool batteryPresent; - int batteryLevel; - int batteryVoltage; - int batteryTemperature; - String8 batteryTechnology; - - bool dockBatterySupported; - bool chargerDockAcOnline; - int dockBatteryStatus; - int dockBatteryHealth; - bool dockBatteryPresent; - int dockBatteryLevel; - int dockBatteryVoltage; - int dockBatteryTemperature; - String8 dockBatteryTechnology; - - status_t writeToParcel(Parcel* parcel) const; - status_t readFromParcel(Parcel* parcel); -}; - -struct BatteryProperty { - int64_t valueInt64; - - status_t writeToParcel(Parcel* parcel) const; - status_t readFromParcel(Parcel* parcel); -}; - -}; // namespace android - -#endif // ANDROID_BATTERYSERVICE_H diff --git a/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesListener.h b/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesListener.h deleted file mode 100644 index b02d8e907..000000000 --- a/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesListener.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_IBATTERYPROPERTIESLISTENER_H -#define ANDROID_IBATTERYPROPERTIESLISTENER_H - -#include -#include - -#include - -namespace android { - -// must be kept in sync with interface defined in IBatteryPropertiesListener.aidl -enum { - TRANSACT_BATTERYPROPERTIESCHANGED = IBinder::FIRST_CALL_TRANSACTION, -}; - -// ---------------------------------------------------------------------------- - -class IBatteryPropertiesListener : public IInterface { -public: - DECLARE_META_INTERFACE(BatteryPropertiesListener); - - virtual void batteryPropertiesChanged(struct BatteryProperties props) = 0; -}; - -// ---------------------------------------------------------------------------- - -}; // namespace android - -#endif // ANDROID_IBATTERYPROPERTIESLISTENER_H diff --git a/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesRegistrar.h b/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesRegistrar.h deleted file mode 100644 index f6a7981d5..000000000 --- a/phonelibs/android_frameworks_native/include/batteryservice/IBatteryPropertiesRegistrar.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * Copyright (C) 2015 The CyanogenMod Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_IBATTERYPROPERTIESREGISTRAR_H -#define ANDROID_IBATTERYPROPERTIESREGISTRAR_H - -#include -#include - -namespace android { - -// must be kept in sync with interface defined in IBatteryPropertiesRegistrar.aidl -enum { - REGISTER_LISTENER = IBinder::FIRST_CALL_TRANSACTION, - UNREGISTER_LISTENER, - GET_PROPERTY, - GET_DOCK_PROPERTY, -}; - -class IBatteryPropertiesRegistrar : public IInterface { -public: - DECLARE_META_INTERFACE(BatteryPropertiesRegistrar); - - virtual void registerListener(const sp& listener) = 0; - virtual void unregisterListener(const sp& listener) = 0; - virtual status_t getProperty(int id, struct BatteryProperty *val) = 0; - virtual status_t getDockProperty(int id, struct BatteryProperty *val) = 0; -}; - -class BnBatteryPropertiesRegistrar : public BnInterface { -public: - virtual status_t onTransact(uint32_t code, const Parcel& data, - Parcel* reply, uint32_t flags = 0); -}; - -}; // namespace android - -#endif // ANDROID_IBATTERYPROPERTIESREGISTRAR_H diff --git a/phonelibs/android_frameworks_native/include/diskusage/dirsize.h b/phonelibs/android_frameworks_native/include/diskusage/dirsize.h deleted file mode 100644 index 34236c0e6..000000000 --- a/phonelibs/android_frameworks_native/include/diskusage/dirsize.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef __LIBDISKUSAGE_DIRSIZE_H -#define __LIBDISKUSAGE_DIRSIZE_H - -#include - -__BEGIN_DECLS - -int64_t stat_size(struct stat *s); -int64_t calculate_dir_size(int dfd); - -__END_DECLS - -#endif /* __LIBDISKUSAGE_DIRSIZE_H */ diff --git a/phonelibs/android_frameworks_native/include/input/IInputFlinger.h b/phonelibs/android_frameworks_native/include/input/IInputFlinger.h deleted file mode 100644 index 629310ff2..000000000 --- a/phonelibs/android_frameworks_native/include/input/IInputFlinger.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_IINPUT_FLINGER_H -#define _LIBINPUT_IINPUT_FLINGER_H - -#include -#include - -#include - -namespace android { - -/* - * This class defines the Binder IPC interface for accessing various - * InputFlinger features. - */ -class IInputFlinger : public IInterface { -public: - DECLARE_META_INTERFACE(InputFlinger); -}; - - -/** - * Binder implementation. - */ -class BnInputFlinger : public BnInterface { -public: - enum { - DO_SOMETHING_TRANSACTION = IBinder::FIRST_CALL_TRANSACTION, - }; - - virtual status_t onTransact(uint32_t code, const Parcel& data, - Parcel* reply, uint32_t flags = 0); -}; - -} // namespace android - -#endif // _LIBINPUT_IINPUT_FLINGER_H diff --git a/phonelibs/android_frameworks_native/include/input/Input.h b/phonelibs/android_frameworks_native/include/input/Input.h deleted file mode 100644 index 82fc6599a..000000000 --- a/phonelibs/android_frameworks_native/include/input/Input.h +++ /dev/null @@ -1,681 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_INPUT_H -#define _LIBINPUT_INPUT_H - -/** - * Native input event structures. - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * Additional private constants not defined in ndk/ui/input.h. - */ -enum { - /* Signifies that the key is being predispatched */ - AKEY_EVENT_FLAG_PREDISPATCH = 0x20000000, - - /* Private control to determine when an app is tracking a key sequence. */ - AKEY_EVENT_FLAG_START_TRACKING = 0x40000000, - - /* Key event is inconsistent with previously sent key events. */ - AKEY_EVENT_FLAG_TAINTED = 0x80000000, -}; - -enum { - - /** - * This flag indicates that the window that received this motion event is partly - * or wholly obscured by another visible window above it. This flag is set to true - * even if the event did not directly pass through the obscured area. - * A security sensitive application can check this flag to identify situations in which - * a malicious application may have covered up part of its content for the purpose - * of misleading the user or hijacking touches. An appropriate response might be - * to drop the suspect touches or to take additional precautions to confirm the user's - * actual intent. - */ - AMOTION_EVENT_FLAG_WINDOW_IS_PARTIALLY_OBSCURED = 0x2, - - /* Motion event is inconsistent with previously sent motion events. */ - AMOTION_EVENT_FLAG_TAINTED = 0x80000000, -}; - -enum { - /* Used when a motion event is not associated with any display. - * Typically used for non-pointer events. */ - ADISPLAY_ID_NONE = -1, - - /* The default display id. */ - ADISPLAY_ID_DEFAULT = 0, -}; - -enum { - /* - * Indicates that an input device has switches. - * This input source flag is hidden from the API because switches are only used by the system - * and applications have no way to interact with them. - */ - AINPUT_SOURCE_SWITCH = 0x80000000, -}; - -enum { - /** - * Constants for LEDs. Hidden from the API since we don't actually expose a way to interact - * with LEDs to developers - * - * NOTE: If you add LEDs here, you must also add them to InputEventLabels.h - */ - - ALED_NUM_LOCK = 0x00, - ALED_CAPS_LOCK = 0x01, - ALED_SCROLL_LOCK = 0x02, - ALED_COMPOSE = 0x03, - ALED_KANA = 0x04, - ALED_SLEEP = 0x05, - ALED_SUSPEND = 0x06, - ALED_MUTE = 0x07, - ALED_MISC = 0x08, - ALED_MAIL = 0x09, - ALED_CHARGING = 0x0a, - ALED_CONTROLLER_1 = 0x10, - ALED_CONTROLLER_2 = 0x11, - ALED_CONTROLLER_3 = 0x12, - ALED_CONTROLLER_4 = 0x13, -}; - -/* Maximum number of controller LEDs we support */ -#define MAX_CONTROLLER_LEDS 4 - -/* - * SystemUiVisibility constants from View. - */ -enum { - ASYSTEM_UI_VISIBILITY_STATUS_BAR_VISIBLE = 0, - ASYSTEM_UI_VISIBILITY_STATUS_BAR_HIDDEN = 0x00000001, -}; - -/* - * Maximum number of pointers supported per motion event. - * Smallest number of pointers is 1. - * (We want at least 10 but some touch controllers obstensibly configured for 10 pointers - * will occasionally emit 11. There is not much harm making this constant bigger.) - */ -#define MAX_POINTERS 16 - -/* - * Maximum number of samples supported per motion event. - */ -#define MAX_SAMPLES UINT16_MAX - -/* - * Maximum pointer id value supported in a motion event. - * Smallest pointer id is 0. - * (This is limited by our use of BitSet32 to track pointer assignments.) - */ -#define MAX_POINTER_ID 31 - -/* - * Declare a concrete type for the NDK's input event forward declaration. - */ -struct AInputEvent { - virtual ~AInputEvent() { } -}; - -/* - * Declare a concrete type for the NDK's input device forward declaration. - */ -struct AInputDevice { - virtual ~AInputDevice() { } -}; - - -namespace android { - -#ifdef HAVE_ANDROID_OS -class Parcel; -#endif - -/* - * Flags that flow alongside events in the input dispatch system to help with certain - * policy decisions such as waking from device sleep. - * - * These flags are also defined in frameworks/base/core/java/android/view/WindowManagerPolicy.java. - */ -enum { - /* These flags originate in RawEvents and are generally set in the key map. - * NOTE: If you want a flag to be able to set in a keylayout file, then you must add it to - * InputEventLabels.h as well. */ - - // Indicates that the event should wake the device. - POLICY_FLAG_WAKE = 0x00000001, - - // Indicates that the key is virtual, such as a capacitive button, and should - // generate haptic feedback. Virtual keys may be suppressed for some time - // after a recent touch to prevent accidental activation of virtual keys adjacent - // to the touch screen during an edge swipe. - POLICY_FLAG_VIRTUAL = 0x00000002, - - // Indicates that the key is the special function modifier. - POLICY_FLAG_FUNCTION = 0x00000004, - - // Indicates that the key represents a special gesture that has been detected by - // the touch firmware or driver. Causes touch events from the same device to be canceled. - POLICY_FLAG_GESTURE = 0x00000008, - - POLICY_FLAG_RAW_MASK = 0x0000ffff, - - /* These flags are set by the input dispatcher. */ - - // Indicates that the input event was injected. - POLICY_FLAG_INJECTED = 0x01000000, - - // Indicates that the input event is from a trusted source such as a directly attached - // input device or an application with system-wide event injection permission. - POLICY_FLAG_TRUSTED = 0x02000000, - - // Indicates that the input event has passed through an input filter. - POLICY_FLAG_FILTERED = 0x04000000, - - // Disables automatic key repeating behavior. - POLICY_FLAG_DISABLE_KEY_REPEAT = 0x08000000, - - /* These flags are set by the input reader policy as it intercepts each event. */ - - // Indicates that the device was in an interactive state when the - // event was intercepted. - POLICY_FLAG_INTERACTIVE = 0x20000000, - - // Indicates that the event should be dispatched to applications. - // The input event should still be sent to the InputDispatcher so that it can see all - // input events received include those that it will not deliver. - POLICY_FLAG_PASS_TO_USER = 0x40000000, -}; - -/* - * Pointer coordinate data. - */ -struct PointerCoords { - enum { MAX_AXES = 30 }; // 30 so that sizeof(PointerCoords) == 128 - - // Bitfield of axes that are present in this structure. - uint64_t bits __attribute__((aligned(8))); - - // Values of axes that are stored in this structure packed in order by axis id - // for each axis that is present in the structure according to 'bits'. - float values[MAX_AXES]; - - inline void clear() { - BitSet64::clear(bits); - } - - bool isEmpty() const { - return BitSet64::isEmpty(bits); - } - - float getAxisValue(int32_t axis) const; - status_t setAxisValue(int32_t axis, float value); - - void scale(float scale); - void applyOffset(float xOffset, float yOffset); - - inline float getX() const { - return getAxisValue(AMOTION_EVENT_AXIS_X); - } - - inline float getY() const { - return getAxisValue(AMOTION_EVENT_AXIS_Y); - } - -#ifdef HAVE_ANDROID_OS - status_t readFromParcel(Parcel* parcel); - status_t writeToParcel(Parcel* parcel) const; -#endif - - bool operator==(const PointerCoords& other) const; - inline bool operator!=(const PointerCoords& other) const { - return !(*this == other); - } - - void copyFrom(const PointerCoords& other); - -private: - void tooManyAxes(int axis); -}; - -/* - * Pointer property data. - */ -struct PointerProperties { - // The id of the pointer. - int32_t id; - - // The pointer tool type. - int32_t toolType; - - inline void clear() { - id = -1; - toolType = 0; - } - - bool operator==(const PointerProperties& other) const; - inline bool operator!=(const PointerProperties& other) const { - return !(*this == other); - } - - void copyFrom(const PointerProperties& other); -}; - -/* - * Input events. - */ -class InputEvent : public AInputEvent { -public: - virtual ~InputEvent() { } - - virtual int32_t getType() const = 0; - - inline int32_t getDeviceId() const { return mDeviceId; } - - inline int32_t getSource() const { return mSource; } - - inline void setSource(int32_t source) { mSource = source; } - -protected: - void initialize(int32_t deviceId, int32_t source); - void initialize(const InputEvent& from); - - int32_t mDeviceId; - int32_t mSource; -}; - -/* - * Key events. - */ -class KeyEvent : public InputEvent { -public: - virtual ~KeyEvent() { } - - virtual int32_t getType() const { return AINPUT_EVENT_TYPE_KEY; } - - inline int32_t getAction() const { return mAction; } - - inline int32_t getFlags() const { return mFlags; } - - inline void setFlags(int32_t flags) { mFlags = flags; } - - inline int32_t getKeyCode() const { return mKeyCode; } - - inline int32_t getScanCode() const { return mScanCode; } - - inline int32_t getMetaState() const { return mMetaState; } - - inline int32_t getRepeatCount() const { return mRepeatCount; } - - inline nsecs_t getDownTime() const { return mDownTime; } - - inline nsecs_t getEventTime() const { return mEventTime; } - - static const char* getLabel(int32_t keyCode); - static int32_t getKeyCodeFromLabel(const char* label); - - void initialize( - int32_t deviceId, - int32_t source, - int32_t action, - int32_t flags, - int32_t keyCode, - int32_t scanCode, - int32_t metaState, - int32_t repeatCount, - nsecs_t downTime, - nsecs_t eventTime); - void initialize(const KeyEvent& from); - -protected: - int32_t mAction; - int32_t mFlags; - int32_t mKeyCode; - int32_t mScanCode; - int32_t mMetaState; - int32_t mRepeatCount; - nsecs_t mDownTime; - nsecs_t mEventTime; -}; - -/* - * Motion events. - */ -class MotionEvent : public InputEvent { -public: - virtual ~MotionEvent() { } - - virtual int32_t getType() const { return AINPUT_EVENT_TYPE_MOTION; } - - inline int32_t getAction() const { return mAction; } - - inline int32_t getActionMasked() const { return mAction & AMOTION_EVENT_ACTION_MASK; } - - inline int32_t getActionIndex() const { - return (mAction & AMOTION_EVENT_ACTION_POINTER_INDEX_MASK) - >> AMOTION_EVENT_ACTION_POINTER_INDEX_SHIFT; - } - - inline void setAction(int32_t action) { mAction = action; } - - inline int32_t getFlags() const { return mFlags; } - - inline void setFlags(int32_t flags) { mFlags = flags; } - - inline int32_t getEdgeFlags() const { return mEdgeFlags; } - - inline void setEdgeFlags(int32_t edgeFlags) { mEdgeFlags = edgeFlags; } - - inline int32_t getMetaState() const { return mMetaState; } - - inline void setMetaState(int32_t metaState) { mMetaState = metaState; } - - inline int32_t getButtonState() const { return mButtonState; } - - inline int32_t setButtonState(int32_t buttonState) { mButtonState = buttonState; } - - inline int32_t getActionButton() const { return mActionButton; } - - inline void setActionButton(int32_t button) { mActionButton = button; } - - inline float getXOffset() const { return mXOffset; } - - inline float getYOffset() const { return mYOffset; } - - inline float getXPrecision() const { return mXPrecision; } - - inline float getYPrecision() const { return mYPrecision; } - - inline nsecs_t getDownTime() const { return mDownTime; } - - inline void setDownTime(nsecs_t downTime) { mDownTime = downTime; } - - inline size_t getPointerCount() const { return mPointerProperties.size(); } - - inline const PointerProperties* getPointerProperties(size_t pointerIndex) const { - return &mPointerProperties[pointerIndex]; - } - - inline int32_t getPointerId(size_t pointerIndex) const { - return mPointerProperties[pointerIndex].id; - } - - inline int32_t getToolType(size_t pointerIndex) const { - return mPointerProperties[pointerIndex].toolType; - } - - inline nsecs_t getEventTime() const { return mSampleEventTimes[getHistorySize()]; } - - const PointerCoords* getRawPointerCoords(size_t pointerIndex) const; - - float getRawAxisValue(int32_t axis, size_t pointerIndex) const; - - inline float getRawX(size_t pointerIndex) const { - return getRawAxisValue(AMOTION_EVENT_AXIS_X, pointerIndex); - } - - inline float getRawY(size_t pointerIndex) const { - return getRawAxisValue(AMOTION_EVENT_AXIS_Y, pointerIndex); - } - - float getAxisValue(int32_t axis, size_t pointerIndex) const; - - inline float getX(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_X, pointerIndex); - } - - inline float getY(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_Y, pointerIndex); - } - - inline float getPressure(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_PRESSURE, pointerIndex); - } - - inline float getSize(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_SIZE, pointerIndex); - } - - inline float getTouchMajor(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_TOUCH_MAJOR, pointerIndex); - } - - inline float getTouchMinor(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_TOUCH_MINOR, pointerIndex); - } - - inline float getToolMajor(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_TOOL_MAJOR, pointerIndex); - } - - inline float getToolMinor(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_TOOL_MINOR, pointerIndex); - } - - inline float getOrientation(size_t pointerIndex) const { - return getAxisValue(AMOTION_EVENT_AXIS_ORIENTATION, pointerIndex); - } - - inline size_t getHistorySize() const { return mSampleEventTimes.size() - 1; } - - inline nsecs_t getHistoricalEventTime(size_t historicalIndex) const { - return mSampleEventTimes[historicalIndex]; - } - - const PointerCoords* getHistoricalRawPointerCoords( - size_t pointerIndex, size_t historicalIndex) const; - - float getHistoricalRawAxisValue(int32_t axis, size_t pointerIndex, - size_t historicalIndex) const; - - inline float getHistoricalRawX(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalRawAxisValue( - AMOTION_EVENT_AXIS_X, pointerIndex, historicalIndex); - } - - inline float getHistoricalRawY(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalRawAxisValue( - AMOTION_EVENT_AXIS_Y, pointerIndex, historicalIndex); - } - - float getHistoricalAxisValue(int32_t axis, size_t pointerIndex, size_t historicalIndex) const; - - inline float getHistoricalX(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_X, pointerIndex, historicalIndex); - } - - inline float getHistoricalY(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_Y, pointerIndex, historicalIndex); - } - - inline float getHistoricalPressure(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_PRESSURE, pointerIndex, historicalIndex); - } - - inline float getHistoricalSize(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_SIZE, pointerIndex, historicalIndex); - } - - inline float getHistoricalTouchMajor(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_TOUCH_MAJOR, pointerIndex, historicalIndex); - } - - inline float getHistoricalTouchMinor(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_TOUCH_MINOR, pointerIndex, historicalIndex); - } - - inline float getHistoricalToolMajor(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_TOOL_MAJOR, pointerIndex, historicalIndex); - } - - inline float getHistoricalToolMinor(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_TOOL_MINOR, pointerIndex, historicalIndex); - } - - inline float getHistoricalOrientation(size_t pointerIndex, size_t historicalIndex) const { - return getHistoricalAxisValue( - AMOTION_EVENT_AXIS_ORIENTATION, pointerIndex, historicalIndex); - } - - ssize_t findPointerIndex(int32_t pointerId) const; - - void initialize( - int32_t deviceId, - int32_t source, - int32_t action, - int32_t actionButton, - int32_t flags, - int32_t edgeFlags, - int32_t metaState, - int32_t buttonState, - float xOffset, - float yOffset, - float xPrecision, - float yPrecision, - nsecs_t downTime, - nsecs_t eventTime, - size_t pointerCount, - const PointerProperties* pointerProperties, - const PointerCoords* pointerCoords); - - void copyFrom(const MotionEvent* other, bool keepHistory); - - void addSample( - nsecs_t eventTime, - const PointerCoords* pointerCoords); - - void offsetLocation(float xOffset, float yOffset); - - void scale(float scaleFactor); - - // Apply 3x3 perspective matrix transformation. - // Matrix is in row-major form and compatible with SkMatrix. - void transform(const float matrix[9]); - -#ifdef HAVE_ANDROID_OS - status_t readFromParcel(Parcel* parcel); - status_t writeToParcel(Parcel* parcel) const; -#endif - - static bool isTouchEvent(int32_t source, int32_t action); - inline bool isTouchEvent() const { - return isTouchEvent(mSource, mAction); - } - - // Low-level accessors. - inline const PointerProperties* getPointerProperties() const { - return mPointerProperties.array(); - } - inline const nsecs_t* getSampleEventTimes() const { return mSampleEventTimes.array(); } - inline const PointerCoords* getSamplePointerCoords() const { - return mSamplePointerCoords.array(); - } - - static const char* getLabel(int32_t axis); - static int32_t getAxisFromLabel(const char* label); - -protected: - int32_t mAction; - int32_t mActionButton; - int32_t mFlags; - int32_t mEdgeFlags; - int32_t mMetaState; - int32_t mButtonState; - float mXOffset; - float mYOffset; - float mXPrecision; - float mYPrecision; - nsecs_t mDownTime; - Vector mPointerProperties; - Vector mSampleEventTimes; - Vector mSamplePointerCoords; -}; - -/* - * Input event factory. - */ -class InputEventFactoryInterface { -protected: - virtual ~InputEventFactoryInterface() { } - -public: - InputEventFactoryInterface() { } - - virtual KeyEvent* createKeyEvent() = 0; - virtual MotionEvent* createMotionEvent() = 0; -}; - -/* - * A simple input event factory implementation that uses a single preallocated instance - * of each type of input event that are reused for each request. - */ -class PreallocatedInputEventFactory : public InputEventFactoryInterface { -public: - PreallocatedInputEventFactory() { } - virtual ~PreallocatedInputEventFactory() { } - - virtual KeyEvent* createKeyEvent() { return & mKeyEvent; } - virtual MotionEvent* createMotionEvent() { return & mMotionEvent; } - -private: - KeyEvent mKeyEvent; - MotionEvent mMotionEvent; -}; - -/* - * An input event factory implementation that maintains a pool of input events. - */ -class PooledInputEventFactory : public InputEventFactoryInterface { -public: - PooledInputEventFactory(size_t maxPoolSize = 20); - virtual ~PooledInputEventFactory(); - - virtual KeyEvent* createKeyEvent(); - virtual MotionEvent* createMotionEvent(); - - void recycle(InputEvent* event); - -private: - const size_t mMaxPoolSize; - - Vector mKeyEventPool; - Vector mMotionEventPool; -}; - -} // namespace android - -#endif // _LIBINPUT_INPUT_H diff --git a/phonelibs/android_frameworks_native/include/input/InputDevice.h b/phonelibs/android_frameworks_native/include/input/InputDevice.h deleted file mode 100644 index 1ea69d352..000000000 --- a/phonelibs/android_frameworks_native/include/input/InputDevice.h +++ /dev/null @@ -1,170 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_INPUT_DEVICE_H -#define _LIBINPUT_INPUT_DEVICE_H - -#include -#include - -namespace android { - -/* - * Identifies a device. - */ -struct InputDeviceIdentifier { - inline InputDeviceIdentifier() : - bus(0), vendor(0), product(0), version(0) { - } - - // Information provided by the kernel. - String8 name; - String8 location; - String8 uniqueId; - uint16_t bus; - uint16_t vendor; - uint16_t product; - uint16_t version; - - // A composite input device descriptor string that uniquely identifies the device - // even across reboots or reconnections. The value of this field is used by - // upper layers of the input system to associate settings with individual devices. - // It is hashed from whatever kernel provided information is available. - // Ideally, the way this value is computed should not change between Android releases - // because that would invalidate persistent settings that rely on it. - String8 descriptor; - - // A value added to uniquely identify a device in the absence of a unique id. This - // is intended to be a minimum way to distinguish from other active devices and may - // reuse values that are not associated with an input anymore. - uint16_t nonce; -}; - -/* - * Describes the characteristics and capabilities of an input device. - */ -class InputDeviceInfo { -public: - InputDeviceInfo(); - InputDeviceInfo(const InputDeviceInfo& other); - ~InputDeviceInfo(); - - struct MotionRange { - int32_t axis; - uint32_t source; - float min; - float max; - float flat; - float fuzz; - float resolution; - }; - - void initialize(int32_t id, int32_t generation, int32_t controllerNumber, - const InputDeviceIdentifier& identifier, const String8& alias, bool isExternal, - bool hasMic); - - inline int32_t getId() const { return mId; } - inline int32_t getControllerNumber() const { return mControllerNumber; } - inline int32_t getGeneration() const { return mGeneration; } - inline const InputDeviceIdentifier& getIdentifier() const { return mIdentifier; } - inline const String8& getAlias() const { return mAlias; } - inline const String8& getDisplayName() const { - return mAlias.isEmpty() ? mIdentifier.name : mAlias; - } - inline bool isExternal() const { return mIsExternal; } - inline bool hasMic() const { return mHasMic; } - inline uint32_t getSources() const { return mSources; } - - const MotionRange* getMotionRange(int32_t axis, uint32_t source) const; - - void addSource(uint32_t source); - void addMotionRange(int32_t axis, uint32_t source, - float min, float max, float flat, float fuzz, float resolution); - void addMotionRange(const MotionRange& range); - - inline void setKeyboardType(int32_t keyboardType) { mKeyboardType = keyboardType; } - inline int32_t getKeyboardType() const { return mKeyboardType; } - - inline void setKeyCharacterMap(const sp& value) { - mKeyCharacterMap = value; - } - - inline sp getKeyCharacterMap() const { - return mKeyCharacterMap; - } - - inline void setVibrator(bool hasVibrator) { mHasVibrator = hasVibrator; } - inline bool hasVibrator() const { return mHasVibrator; } - - inline void setButtonUnderPad(bool hasButton) { mHasButtonUnderPad = hasButton; } - inline bool hasButtonUnderPad() const { return mHasButtonUnderPad; } - - inline const Vector& getMotionRanges() const { - return mMotionRanges; - } - -private: - int32_t mId; - int32_t mGeneration; - int32_t mControllerNumber; - InputDeviceIdentifier mIdentifier; - String8 mAlias; - bool mIsExternal; - bool mHasMic; - uint32_t mSources; - int32_t mKeyboardType; - sp mKeyCharacterMap; - bool mHasVibrator; - bool mHasButtonUnderPad; - - Vector mMotionRanges; -}; - -/* Types of input device configuration files. */ -enum InputDeviceConfigurationFileType { - INPUT_DEVICE_CONFIGURATION_FILE_TYPE_CONFIGURATION = 0, /* .idc file */ - INPUT_DEVICE_CONFIGURATION_FILE_TYPE_KEY_LAYOUT = 1, /* .kl file */ - INPUT_DEVICE_CONFIGURATION_FILE_TYPE_KEY_CHARACTER_MAP = 2, /* .kcm file */ -}; - -/* - * Gets the path of an input device configuration file, if one is available. - * Considers both system provided and user installed configuration files. - * - * The device identifier is used to construct several default configuration file - * names to try based on the device name, vendor, product, and version. - * - * Returns an empty string if not found. - */ -extern String8 getInputDeviceConfigurationFilePathByDeviceIdentifier( - const InputDeviceIdentifier& deviceIdentifier, - InputDeviceConfigurationFileType type); - -/* - * Gets the path of an input device configuration file, if one is available. - * Considers both system provided and user installed configuration files. - * - * The name is case-sensitive and is used to construct the filename to resolve. - * All characters except 'a'-'z', 'A'-'Z', '0'-'9', '-', and '_' are replaced by underscores. - * - * Returns an empty string if not found. - */ -extern String8 getInputDeviceConfigurationFilePathByName( - const String8& name, InputDeviceConfigurationFileType type); - -} // namespace android - -#endif // _LIBINPUT_INPUT_DEVICE_H diff --git a/phonelibs/android_frameworks_native/include/input/InputEventLabels.h b/phonelibs/android_frameworks_native/include/input/InputEventLabels.h deleted file mode 100644 index c03c0f2d5..000000000 --- a/phonelibs/android_frameworks_native/include/input/InputEventLabels.h +++ /dev/null @@ -1,447 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_INPUT_EVENT_LABELS_H -#define _LIBINPUT_INPUT_EVENT_LABELS_H - -#include -#include - -#define DEFINE_KEYCODE(key) { #key, AKEYCODE_##key } -#define DEFINE_AXIS(axis) { #axis, AMOTION_EVENT_AXIS_##axis } -#define DEFINE_LED(led) { #led, ALED_##led } -#define DEFINE_FLAG(flag) { #flag, POLICY_FLAG_##flag } - -namespace android { - -template -size_t size(T (&)[N]) { return N; } - -struct InputEventLabel { - const char *literal; - int value; -}; - - -static const InputEventLabel KEYCODES[] = { - // NOTE: If you add a new keycode here you must also add it to several other files. - // Refer to frameworks/base/core/java/android/view/KeyEvent.java for the full list. - DEFINE_KEYCODE(UNKNOWN), - DEFINE_KEYCODE(SOFT_LEFT), - DEFINE_KEYCODE(SOFT_RIGHT), - DEFINE_KEYCODE(HOME), - DEFINE_KEYCODE(BACK), - DEFINE_KEYCODE(CALL), - DEFINE_KEYCODE(ENDCALL), - DEFINE_KEYCODE(0), - DEFINE_KEYCODE(1), - DEFINE_KEYCODE(2), - DEFINE_KEYCODE(3), - DEFINE_KEYCODE(4), - DEFINE_KEYCODE(5), - DEFINE_KEYCODE(6), - DEFINE_KEYCODE(7), - DEFINE_KEYCODE(8), - DEFINE_KEYCODE(9), - DEFINE_KEYCODE(STAR), - DEFINE_KEYCODE(POUND), - DEFINE_KEYCODE(DPAD_UP), - DEFINE_KEYCODE(DPAD_DOWN), - DEFINE_KEYCODE(DPAD_LEFT), - DEFINE_KEYCODE(DPAD_RIGHT), - DEFINE_KEYCODE(DPAD_CENTER), - DEFINE_KEYCODE(VOLUME_UP), - DEFINE_KEYCODE(VOLUME_DOWN), - DEFINE_KEYCODE(POWER), - DEFINE_KEYCODE(CAMERA), - DEFINE_KEYCODE(CLEAR), - DEFINE_KEYCODE(A), - DEFINE_KEYCODE(B), - DEFINE_KEYCODE(C), - DEFINE_KEYCODE(D), - DEFINE_KEYCODE(E), - DEFINE_KEYCODE(F), - DEFINE_KEYCODE(G), - DEFINE_KEYCODE(H), - DEFINE_KEYCODE(I), - DEFINE_KEYCODE(J), - DEFINE_KEYCODE(K), - DEFINE_KEYCODE(L), - DEFINE_KEYCODE(M), - DEFINE_KEYCODE(N), - DEFINE_KEYCODE(O), - DEFINE_KEYCODE(P), - DEFINE_KEYCODE(Q), - DEFINE_KEYCODE(R), - DEFINE_KEYCODE(S), - DEFINE_KEYCODE(T), - DEFINE_KEYCODE(U), - DEFINE_KEYCODE(V), - DEFINE_KEYCODE(W), - DEFINE_KEYCODE(X), - DEFINE_KEYCODE(Y), - DEFINE_KEYCODE(Z), - DEFINE_KEYCODE(COMMA), - DEFINE_KEYCODE(PERIOD), - DEFINE_KEYCODE(ALT_LEFT), - DEFINE_KEYCODE(ALT_RIGHT), - DEFINE_KEYCODE(SHIFT_LEFT), - DEFINE_KEYCODE(SHIFT_RIGHT), - DEFINE_KEYCODE(TAB), - DEFINE_KEYCODE(SPACE), - DEFINE_KEYCODE(SYM), - DEFINE_KEYCODE(EXPLORER), - DEFINE_KEYCODE(ENVELOPE), - DEFINE_KEYCODE(ENTER), - DEFINE_KEYCODE(DEL), - DEFINE_KEYCODE(GRAVE), - DEFINE_KEYCODE(MINUS), - DEFINE_KEYCODE(EQUALS), - DEFINE_KEYCODE(LEFT_BRACKET), - DEFINE_KEYCODE(RIGHT_BRACKET), - DEFINE_KEYCODE(BACKSLASH), - DEFINE_KEYCODE(SEMICOLON), - DEFINE_KEYCODE(APOSTROPHE), - DEFINE_KEYCODE(SLASH), - DEFINE_KEYCODE(AT), - DEFINE_KEYCODE(NUM), - DEFINE_KEYCODE(HEADSETHOOK), - DEFINE_KEYCODE(FOCUS), // *Camera* focus - DEFINE_KEYCODE(PLUS), - DEFINE_KEYCODE(MENU), - DEFINE_KEYCODE(NOTIFICATION), - DEFINE_KEYCODE(SEARCH), - DEFINE_KEYCODE(MEDIA_PLAY_PAUSE), - DEFINE_KEYCODE(MEDIA_STOP), - DEFINE_KEYCODE(MEDIA_NEXT), - DEFINE_KEYCODE(MEDIA_PREVIOUS), - DEFINE_KEYCODE(MEDIA_REWIND), - DEFINE_KEYCODE(MEDIA_FAST_FORWARD), - DEFINE_KEYCODE(MUTE), - DEFINE_KEYCODE(PAGE_UP), - DEFINE_KEYCODE(PAGE_DOWN), - DEFINE_KEYCODE(PICTSYMBOLS), - DEFINE_KEYCODE(SWITCH_CHARSET), - DEFINE_KEYCODE(BUTTON_A), - DEFINE_KEYCODE(BUTTON_B), - DEFINE_KEYCODE(BUTTON_C), - DEFINE_KEYCODE(BUTTON_X), - DEFINE_KEYCODE(BUTTON_Y), - DEFINE_KEYCODE(BUTTON_Z), - DEFINE_KEYCODE(BUTTON_L1), - DEFINE_KEYCODE(BUTTON_R1), - DEFINE_KEYCODE(BUTTON_L2), - DEFINE_KEYCODE(BUTTON_R2), - DEFINE_KEYCODE(BUTTON_THUMBL), - DEFINE_KEYCODE(BUTTON_THUMBR), - DEFINE_KEYCODE(BUTTON_START), - DEFINE_KEYCODE(BUTTON_SELECT), - DEFINE_KEYCODE(BUTTON_MODE), - DEFINE_KEYCODE(ESCAPE), - DEFINE_KEYCODE(FORWARD_DEL), - DEFINE_KEYCODE(CTRL_LEFT), - DEFINE_KEYCODE(CTRL_RIGHT), - DEFINE_KEYCODE(CAPS_LOCK), - DEFINE_KEYCODE(SCROLL_LOCK), - DEFINE_KEYCODE(META_LEFT), - DEFINE_KEYCODE(META_RIGHT), - DEFINE_KEYCODE(FUNCTION), - DEFINE_KEYCODE(SYSRQ), - DEFINE_KEYCODE(BREAK), - DEFINE_KEYCODE(MOVE_HOME), - DEFINE_KEYCODE(MOVE_END), - DEFINE_KEYCODE(INSERT), - DEFINE_KEYCODE(FORWARD), - DEFINE_KEYCODE(MEDIA_PLAY), - DEFINE_KEYCODE(MEDIA_PAUSE), - DEFINE_KEYCODE(MEDIA_CLOSE), - DEFINE_KEYCODE(MEDIA_EJECT), - DEFINE_KEYCODE(MEDIA_RECORD), - DEFINE_KEYCODE(F1), - DEFINE_KEYCODE(F2), - DEFINE_KEYCODE(F3), - DEFINE_KEYCODE(F4), - DEFINE_KEYCODE(F5), - DEFINE_KEYCODE(F6), - DEFINE_KEYCODE(F7), - DEFINE_KEYCODE(F8), - DEFINE_KEYCODE(F9), - DEFINE_KEYCODE(F10), - DEFINE_KEYCODE(F11), - DEFINE_KEYCODE(F12), - DEFINE_KEYCODE(NUM_LOCK), - DEFINE_KEYCODE(NUMPAD_0), - DEFINE_KEYCODE(NUMPAD_1), - DEFINE_KEYCODE(NUMPAD_2), - DEFINE_KEYCODE(NUMPAD_3), - DEFINE_KEYCODE(NUMPAD_4), - DEFINE_KEYCODE(NUMPAD_5), - DEFINE_KEYCODE(NUMPAD_6), - DEFINE_KEYCODE(NUMPAD_7), - DEFINE_KEYCODE(NUMPAD_8), - DEFINE_KEYCODE(NUMPAD_9), - DEFINE_KEYCODE(NUMPAD_DIVIDE), - DEFINE_KEYCODE(NUMPAD_MULTIPLY), - DEFINE_KEYCODE(NUMPAD_SUBTRACT), - DEFINE_KEYCODE(NUMPAD_ADD), - DEFINE_KEYCODE(NUMPAD_DOT), - DEFINE_KEYCODE(NUMPAD_COMMA), - DEFINE_KEYCODE(NUMPAD_ENTER), - DEFINE_KEYCODE(NUMPAD_EQUALS), - DEFINE_KEYCODE(NUMPAD_LEFT_PAREN), - DEFINE_KEYCODE(NUMPAD_RIGHT_PAREN), - DEFINE_KEYCODE(VOLUME_MUTE), - DEFINE_KEYCODE(INFO), - DEFINE_KEYCODE(CHANNEL_UP), - DEFINE_KEYCODE(CHANNEL_DOWN), - DEFINE_KEYCODE(ZOOM_IN), - DEFINE_KEYCODE(ZOOM_OUT), - DEFINE_KEYCODE(TV), - DEFINE_KEYCODE(WINDOW), - DEFINE_KEYCODE(GUIDE), - DEFINE_KEYCODE(DVR), - DEFINE_KEYCODE(BOOKMARK), - DEFINE_KEYCODE(CAPTIONS), - DEFINE_KEYCODE(SETTINGS), - DEFINE_KEYCODE(TV_POWER), - DEFINE_KEYCODE(TV_INPUT), - DEFINE_KEYCODE(STB_POWER), - DEFINE_KEYCODE(STB_INPUT), - DEFINE_KEYCODE(AVR_POWER), - DEFINE_KEYCODE(AVR_INPUT), - DEFINE_KEYCODE(PROG_RED), - DEFINE_KEYCODE(PROG_GREEN), - DEFINE_KEYCODE(PROG_YELLOW), - DEFINE_KEYCODE(PROG_BLUE), - DEFINE_KEYCODE(APP_SWITCH), - DEFINE_KEYCODE(BUTTON_1), - DEFINE_KEYCODE(BUTTON_2), - DEFINE_KEYCODE(BUTTON_3), - DEFINE_KEYCODE(BUTTON_4), - DEFINE_KEYCODE(BUTTON_5), - DEFINE_KEYCODE(BUTTON_6), - DEFINE_KEYCODE(BUTTON_7), - DEFINE_KEYCODE(BUTTON_8), - DEFINE_KEYCODE(BUTTON_9), - DEFINE_KEYCODE(BUTTON_10), - DEFINE_KEYCODE(BUTTON_11), - DEFINE_KEYCODE(BUTTON_12), - DEFINE_KEYCODE(BUTTON_13), - DEFINE_KEYCODE(BUTTON_14), - DEFINE_KEYCODE(BUTTON_15), - DEFINE_KEYCODE(BUTTON_16), - DEFINE_KEYCODE(LANGUAGE_SWITCH), - DEFINE_KEYCODE(MANNER_MODE), - DEFINE_KEYCODE(3D_MODE), - DEFINE_KEYCODE(CONTACTS), - DEFINE_KEYCODE(CALENDAR), - DEFINE_KEYCODE(MUSIC), - DEFINE_KEYCODE(CALCULATOR), - DEFINE_KEYCODE(ZENKAKU_HANKAKU), - DEFINE_KEYCODE(EISU), - DEFINE_KEYCODE(MUHENKAN), - DEFINE_KEYCODE(HENKAN), - DEFINE_KEYCODE(KATAKANA_HIRAGANA), - DEFINE_KEYCODE(YEN), - DEFINE_KEYCODE(RO), - DEFINE_KEYCODE(KANA), - DEFINE_KEYCODE(ASSIST), - DEFINE_KEYCODE(BRIGHTNESS_DOWN), - DEFINE_KEYCODE(BRIGHTNESS_UP), - DEFINE_KEYCODE(MEDIA_AUDIO_TRACK), - DEFINE_KEYCODE(SLEEP), - DEFINE_KEYCODE(WAKEUP), - DEFINE_KEYCODE(PAIRING), - DEFINE_KEYCODE(MEDIA_TOP_MENU), - DEFINE_KEYCODE(11), - DEFINE_KEYCODE(12), - DEFINE_KEYCODE(LAST_CHANNEL), - DEFINE_KEYCODE(TV_DATA_SERVICE), - DEFINE_KEYCODE(VOICE_ASSIST), - DEFINE_KEYCODE(TV_RADIO_SERVICE), - DEFINE_KEYCODE(TV_TELETEXT), - DEFINE_KEYCODE(TV_NUMBER_ENTRY), - DEFINE_KEYCODE(TV_TERRESTRIAL_ANALOG), - DEFINE_KEYCODE(TV_TERRESTRIAL_DIGITAL), - DEFINE_KEYCODE(TV_SATELLITE), - DEFINE_KEYCODE(TV_SATELLITE_BS), - DEFINE_KEYCODE(TV_SATELLITE_CS), - DEFINE_KEYCODE(TV_SATELLITE_SERVICE), - DEFINE_KEYCODE(TV_NETWORK), - DEFINE_KEYCODE(TV_ANTENNA_CABLE), - DEFINE_KEYCODE(TV_INPUT_HDMI_1), - DEFINE_KEYCODE(TV_INPUT_HDMI_2), - DEFINE_KEYCODE(TV_INPUT_HDMI_3), - DEFINE_KEYCODE(TV_INPUT_HDMI_4), - DEFINE_KEYCODE(TV_INPUT_COMPOSITE_1), - DEFINE_KEYCODE(TV_INPUT_COMPOSITE_2), - DEFINE_KEYCODE(TV_INPUT_COMPONENT_1), - DEFINE_KEYCODE(TV_INPUT_COMPONENT_2), - DEFINE_KEYCODE(TV_INPUT_VGA_1), - DEFINE_KEYCODE(TV_AUDIO_DESCRIPTION), - DEFINE_KEYCODE(TV_AUDIO_DESCRIPTION_MIX_UP), - DEFINE_KEYCODE(TV_AUDIO_DESCRIPTION_MIX_DOWN), - DEFINE_KEYCODE(TV_ZOOM_MODE), - DEFINE_KEYCODE(TV_CONTENTS_MENU), - DEFINE_KEYCODE(TV_MEDIA_CONTEXT_MENU), - DEFINE_KEYCODE(TV_TIMER_PROGRAMMING), - DEFINE_KEYCODE(HELP), - DEFINE_KEYCODE(NAVIGATE_PREVIOUS), - DEFINE_KEYCODE(NAVIGATE_NEXT), - DEFINE_KEYCODE(NAVIGATE_IN), - DEFINE_KEYCODE(NAVIGATE_OUT), - DEFINE_KEYCODE(STEM_PRIMARY), - DEFINE_KEYCODE(STEM_1), - DEFINE_KEYCODE(STEM_2), - DEFINE_KEYCODE(STEM_3), - DEFINE_KEYCODE(MEDIA_SKIP_FORWARD), - DEFINE_KEYCODE(MEDIA_SKIP_BACKWARD), - DEFINE_KEYCODE(MEDIA_STEP_FORWARD), - DEFINE_KEYCODE(MEDIA_STEP_BACKWARD), - DEFINE_KEYCODE(SOFT_SLEEP), - - { NULL, 0 } -}; - -static const InputEventLabel AXES[] = { - DEFINE_AXIS(X), - DEFINE_AXIS(Y), - DEFINE_AXIS(PRESSURE), - DEFINE_AXIS(SIZE), - DEFINE_AXIS(TOUCH_MAJOR), - DEFINE_AXIS(TOUCH_MINOR), - DEFINE_AXIS(TOOL_MAJOR), - DEFINE_AXIS(TOOL_MINOR), - DEFINE_AXIS(ORIENTATION), - DEFINE_AXIS(VSCROLL), - DEFINE_AXIS(HSCROLL), - DEFINE_AXIS(Z), - DEFINE_AXIS(RX), - DEFINE_AXIS(RY), - DEFINE_AXIS(RZ), - DEFINE_AXIS(HAT_X), - DEFINE_AXIS(HAT_Y), - DEFINE_AXIS(LTRIGGER), - DEFINE_AXIS(RTRIGGER), - DEFINE_AXIS(THROTTLE), - DEFINE_AXIS(RUDDER), - DEFINE_AXIS(WHEEL), - DEFINE_AXIS(GAS), - DEFINE_AXIS(BRAKE), - DEFINE_AXIS(DISTANCE), - DEFINE_AXIS(TILT), - DEFINE_AXIS(GENERIC_1), - DEFINE_AXIS(GENERIC_2), - DEFINE_AXIS(GENERIC_3), - DEFINE_AXIS(GENERIC_4), - DEFINE_AXIS(GENERIC_5), - DEFINE_AXIS(GENERIC_6), - DEFINE_AXIS(GENERIC_7), - DEFINE_AXIS(GENERIC_8), - DEFINE_AXIS(GENERIC_9), - DEFINE_AXIS(GENERIC_10), - DEFINE_AXIS(GENERIC_11), - DEFINE_AXIS(GENERIC_12), - DEFINE_AXIS(GENERIC_13), - DEFINE_AXIS(GENERIC_14), - DEFINE_AXIS(GENERIC_15), - DEFINE_AXIS(GENERIC_16), - - // NOTE: If you add a new axis here you must also add it to several other files. - // Refer to frameworks/base/core/java/android/view/MotionEvent.java for the full list. - { NULL, 0 } -}; - -static const InputEventLabel LEDS[] = { - DEFINE_LED(NUM_LOCK), - DEFINE_LED(CAPS_LOCK), - DEFINE_LED(SCROLL_LOCK), - DEFINE_LED(COMPOSE), - DEFINE_LED(KANA), - DEFINE_LED(SLEEP), - DEFINE_LED(SUSPEND), - DEFINE_LED(MUTE), - DEFINE_LED(MISC), - DEFINE_LED(MAIL), - DEFINE_LED(CHARGING), - DEFINE_LED(CONTROLLER_1), - DEFINE_LED(CONTROLLER_2), - DEFINE_LED(CONTROLLER_3), - DEFINE_LED(CONTROLLER_4), - - // NOTE: If you add new LEDs here, you must also add them to Input.h - { NULL, 0 } -}; - -static const InputEventLabel FLAGS[] = { - DEFINE_FLAG(WAKE), - DEFINE_FLAG(VIRTUAL), - DEFINE_FLAG(FUNCTION), - DEFINE_FLAG(GESTURE), - - { NULL, 0 } -}; - -static int lookupValueByLabel(const char* literal, const InputEventLabel *list) { - while (list->literal) { - if (strcmp(literal, list->literal) == 0) { - return list->value; - } - list++; - } - return list->value; -} - -static const char* lookupLabelByValue(int value, const InputEventLabel* list) { - while (list->literal) { - if (list->value == value) { - return list->literal; - } - list++; - } - return NULL; -} - -static int32_t getKeyCodeByLabel(const char* label) { - return int32_t(lookupValueByLabel(label, KEYCODES)); -} - -static const char* getLabelByKeyCode(int32_t keyCode) { - if (keyCode >= 0 && keyCode < size(KEYCODES)) { - return KEYCODES[keyCode].literal; - } - return NULL; -} - -static uint32_t getKeyFlagByLabel(const char* label) { - return uint32_t(lookupValueByLabel(label, FLAGS)); -} - -static int32_t getAxisByLabel(const char* label) { - return int32_t(lookupValueByLabel(label, AXES)); -} - -static const char* getAxisLabel(int32_t axisId) { - return lookupLabelByValue(axisId, AXES); -} - -static int32_t getLedByLabel(const char* label) { - return int32_t(lookupValueByLabel(label, LEDS)); -} - - -} // namespace android -#endif // _LIBINPUT_INPUT_EVENT_LABELS_H diff --git a/phonelibs/android_frameworks_native/include/input/InputTransport.h b/phonelibs/android_frameworks_native/include/input/InputTransport.h deleted file mode 100644 index f31bceabd..000000000 --- a/phonelibs/android_frameworks_native/include/input/InputTransport.h +++ /dev/null @@ -1,453 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_INPUT_TRANSPORT_H -#define _LIBINPUT_INPUT_TRANSPORT_H - -/** - * Native input transport. - * - * The InputChannel provides a mechanism for exchanging InputMessage structures across processes. - * - * The InputPublisher and InputConsumer each handle one end-point of an input channel. - * The InputPublisher is used by the input dispatcher to send events to the application. - * The InputConsumer is used by the application to receive events from the input dispatcher. - */ - -#include -#include -#include -#include -#include -#include -#include - -namespace android { - -/* - * Intermediate representation used to send input events and related signals. - * - * Note that this structure is used for IPCs so its layout must be identical - * on 64 and 32 bit processes. This is tested in StructLayout_test.cpp. - */ -struct InputMessage { - enum { - TYPE_KEY = 1, - TYPE_MOTION = 2, - TYPE_FINISHED = 3, - }; - - struct Header { - uint32_t type; - // We don't need this field in order to align the body below but we - // leave it here because InputMessage::size() and other functions - // compute the size of this structure as sizeof(Header) + sizeof(Body). - uint32_t padding; - } header; - - // Body *must* be 8 byte aligned. - union Body { - struct Key { - uint32_t seq; - nsecs_t eventTime __attribute__((aligned(8))); - int32_t deviceId; - int32_t source; - int32_t action; - int32_t flags; - int32_t keyCode; - int32_t scanCode; - int32_t metaState; - int32_t repeatCount; - nsecs_t downTime __attribute__((aligned(8))); - - inline size_t size() const { - return sizeof(Key); - } - } key; - - struct Motion { - uint32_t seq; - nsecs_t eventTime __attribute__((aligned(8))); - int32_t deviceId; - int32_t source; - int32_t action; - int32_t actionButton; - int32_t flags; - int32_t metaState; - int32_t buttonState; - int32_t edgeFlags; - nsecs_t downTime __attribute__((aligned(8))); - float xOffset; - float yOffset; - float xPrecision; - float yPrecision; - uint32_t pointerCount; - // Note that PointerCoords requires 8 byte alignment. - struct Pointer { - PointerProperties properties; - PointerCoords coords; - } pointers[MAX_POINTERS]; - - int32_t getActionId() const { - uint32_t index = (action & AMOTION_EVENT_ACTION_POINTER_INDEX_MASK) - >> AMOTION_EVENT_ACTION_POINTER_INDEX_SHIFT; - return pointers[index].properties.id; - } - - inline size_t size() const { - return sizeof(Motion) - sizeof(Pointer) * MAX_POINTERS - + sizeof(Pointer) * pointerCount; - } - } motion; - - struct Finished { - uint32_t seq; - bool handled; - - inline size_t size() const { - return sizeof(Finished); - } - } finished; - } __attribute__((aligned(8))) body; - - bool isValid(size_t actualSize) const; - size_t size() const; -}; - -/* - * An input channel consists of a local unix domain socket used to send and receive - * input messages across processes. Each channel has a descriptive name for debugging purposes. - * - * Each endpoint has its own InputChannel object that specifies its file descriptor. - * - * The input channel is closed when all references to it are released. - */ -class InputChannel : public RefBase { -protected: - virtual ~InputChannel(); - -public: - InputChannel(const String8& name, int fd); - - /* Creates a pair of input channels. - * - * Returns OK on success. - */ - static status_t openInputChannelPair(const String8& name, - sp& outServerChannel, sp& outClientChannel); - - inline String8 getName() const { return mName; } - inline int getFd() const { return mFd; } - - /* Sends a message to the other endpoint. - * - * If the channel is full then the message is guaranteed not to have been sent at all. - * Try again after the consumer has sent a finished signal indicating that it has - * consumed some of the pending messages from the channel. - * - * Returns OK on success. - * Returns WOULD_BLOCK if the channel is full. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Other errors probably indicate that the channel is broken. - */ - status_t sendMessage(const InputMessage* msg); - - /* Receives a message sent by the other endpoint. - * - * If there is no message present, try again after poll() indicates that the fd - * is readable. - * - * Returns OK on success. - * Returns WOULD_BLOCK if there is no message present. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Other errors probably indicate that the channel is broken. - */ - status_t receiveMessage(InputMessage* msg); - - /* Returns a new object that has a duplicate of this channel's fd. */ - sp dup() const; - -private: - String8 mName; - int mFd; -}; - -/* - * Publishes input events to an input channel. - */ -class InputPublisher { -public: - /* Creates a publisher associated with an input channel. */ - explicit InputPublisher(const sp& channel); - - /* Destroys the publisher and releases its input channel. */ - ~InputPublisher(); - - /* Gets the underlying input channel. */ - inline sp getChannel() { return mChannel; } - - /* Publishes a key event to the input channel. - * - * Returns OK on success. - * Returns WOULD_BLOCK if the channel is full. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Returns BAD_VALUE if seq is 0. - * Other errors probably indicate that the channel is broken. - */ - status_t publishKeyEvent( - uint32_t seq, - int32_t deviceId, - int32_t source, - int32_t action, - int32_t flags, - int32_t keyCode, - int32_t scanCode, - int32_t metaState, - int32_t repeatCount, - nsecs_t downTime, - nsecs_t eventTime); - - /* Publishes a motion event to the input channel. - * - * Returns OK on success. - * Returns WOULD_BLOCK if the channel is full. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Returns BAD_VALUE if seq is 0 or if pointerCount is less than 1 or greater than MAX_POINTERS. - * Other errors probably indicate that the channel is broken. - */ - status_t publishMotionEvent( - uint32_t seq, - int32_t deviceId, - int32_t source, - int32_t action, - int32_t actionButton, - int32_t flags, - int32_t edgeFlags, - int32_t metaState, - int32_t buttonState, - float xOffset, - float yOffset, - float xPrecision, - float yPrecision, - nsecs_t downTime, - nsecs_t eventTime, - uint32_t pointerCount, - const PointerProperties* pointerProperties, - const PointerCoords* pointerCoords); - - /* Receives the finished signal from the consumer in reply to the original dispatch signal. - * If a signal was received, returns the message sequence number, - * and whether the consumer handled the message. - * - * The returned sequence number is never 0 unless the operation failed. - * - * Returns OK on success. - * Returns WOULD_BLOCK if there is no signal present. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Other errors probably indicate that the channel is broken. - */ - status_t receiveFinishedSignal(uint32_t* outSeq, bool* outHandled); - -private: - sp mChannel; -}; - -/* - * Consumes input events from an input channel. - */ -class InputConsumer { -public: - /* Creates a consumer associated with an input channel. */ - explicit InputConsumer(const sp& channel); - - /* Destroys the consumer and releases its input channel. */ - ~InputConsumer(); - - /* Gets the underlying input channel. */ - inline sp getChannel() { return mChannel; } - - /* Consumes an input event from the input channel and copies its contents into - * an InputEvent object created using the specified factory. - * - * Tries to combine a series of move events into larger batches whenever possible. - * - * If consumeBatches is false, then defers consuming pending batched events if it - * is possible for additional samples to be added to them later. Call hasPendingBatch() - * to determine whether a pending batch is available to be consumed. - * - * If consumeBatches is true, then events are still batched but they are consumed - * immediately as soon as the input channel is exhausted. - * - * The frameTime parameter specifies the time when the current display frame started - * rendering in the CLOCK_MONOTONIC time base, or -1 if unknown. - * - * The returned sequence number is never 0 unless the operation failed. - * - * Returns OK on success. - * Returns WOULD_BLOCK if there is no event present. - * Returns DEAD_OBJECT if the channel's peer has been closed. - * Returns NO_MEMORY if the event could not be created. - * Other errors probably indicate that the channel is broken. - */ - status_t consume(InputEventFactoryInterface* factory, bool consumeBatches, - nsecs_t frameTime, uint32_t* outSeq, InputEvent** outEvent); - - /* Sends a finished signal to the publisher to inform it that the message - * with the specified sequence number has finished being process and whether - * the message was handled by the consumer. - * - * Returns OK on success. - * Returns BAD_VALUE if seq is 0. - * Other errors probably indicate that the channel is broken. - */ - status_t sendFinishedSignal(uint32_t seq, bool handled); - - /* Returns true if there is a deferred event waiting. - * - * Should be called after calling consume() to determine whether the consumer - * has a deferred event to be processed. Deferred events are somewhat special in - * that they have already been removed from the input channel. If the input channel - * becomes empty, the client may need to do extra work to ensure that it processes - * the deferred event despite the fact that the input channel's file descriptor - * is not readable. - * - * One option is simply to call consume() in a loop until it returns WOULD_BLOCK. - * This guarantees that all deferred events will be processed. - * - * Alternately, the caller can call hasDeferredEvent() to determine whether there is - * a deferred event waiting and then ensure that its event loop wakes up at least - * one more time to consume the deferred event. - */ - bool hasDeferredEvent() const; - - /* Returns true if there is a pending batch. - * - * Should be called after calling consume() with consumeBatches == false to determine - * whether consume() should be called again later on with consumeBatches == true. - */ - bool hasPendingBatch() const; - -private: - // True if touch resampling is enabled. - const bool mResampleTouch; - - // The input channel. - sp mChannel; - - // The current input message. - InputMessage mMsg; - - // True if mMsg contains a valid input message that was deferred from the previous - // call to consume and that still needs to be handled. - bool mMsgDeferred; - - // Batched motion events per device and source. - struct Batch { - Vector samples; - }; - Vector mBatches; - - // Touch state per device and source, only for sources of class pointer. - struct History { - nsecs_t eventTime; - BitSet32 idBits; - int32_t idToIndex[MAX_POINTER_ID + 1]; - PointerCoords pointers[MAX_POINTERS]; - - void initializeFrom(const InputMessage* msg) { - eventTime = msg->body.motion.eventTime; - idBits.clear(); - for (uint32_t i = 0; i < msg->body.motion.pointerCount; i++) { - uint32_t id = msg->body.motion.pointers[i].properties.id; - idBits.markBit(id); - idToIndex[id] = i; - pointers[i].copyFrom(msg->body.motion.pointers[i].coords); - } - } - - const PointerCoords& getPointerById(uint32_t id) const { - return pointers[idToIndex[id]]; - } - }; - struct TouchState { - int32_t deviceId; - int32_t source; - size_t historyCurrent; - size_t historySize; - History history[2]; - History lastResample; - - void initialize(int32_t deviceId, int32_t source) { - this->deviceId = deviceId; - this->source = source; - historyCurrent = 0; - historySize = 0; - lastResample.eventTime = 0; - lastResample.idBits.clear(); - } - - void addHistory(const InputMessage* msg) { - historyCurrent ^= 1; - if (historySize < 2) { - historySize += 1; - } - history[historyCurrent].initializeFrom(msg); - } - - const History* getHistory(size_t index) const { - return &history[(historyCurrent + index) & 1]; - } - }; - Vector mTouchStates; - - // Chain of batched sequence numbers. When multiple input messages are combined into - // a batch, we append a record here that associates the last sequence number in the - // batch with the previous one. When the finished signal is sent, we traverse the - // chain to individually finish all input messages that were part of the batch. - struct SeqChain { - uint32_t seq; // sequence number of batched input message - uint32_t chain; // sequence number of previous batched input message - }; - Vector mSeqChains; - - status_t consumeBatch(InputEventFactoryInterface* factory, - nsecs_t frameTime, uint32_t* outSeq, InputEvent** outEvent); - status_t consumeSamples(InputEventFactoryInterface* factory, - Batch& batch, size_t count, uint32_t* outSeq, InputEvent** outEvent); - - void updateTouchState(InputMessage* msg); - void rewriteMessage(const TouchState& state, InputMessage* msg); - void resampleTouchState(nsecs_t frameTime, MotionEvent* event, - const InputMessage *next); - - ssize_t findBatch(int32_t deviceId, int32_t source) const; - ssize_t findTouchState(int32_t deviceId, int32_t source) const; - - status_t sendUnchainedFinishedSignal(uint32_t seq, bool handled); - - static void initializeKeyEvent(KeyEvent* event, const InputMessage* msg); - static void initializeMotionEvent(MotionEvent* event, const InputMessage* msg); - static void addSample(MotionEvent* event, const InputMessage* msg); - static bool canAddSample(const Batch& batch, const InputMessage* msg); - static ssize_t findSampleNoLaterThan(const Batch& batch, nsecs_t time); - static bool shouldResampleTool(int32_t toolType); - - static bool isTouchResamplingEnabled(); -}; - -} // namespace android - -#endif // _LIBINPUT_INPUT_TRANSPORT_H diff --git a/phonelibs/android_frameworks_native/include/input/KeyCharacterMap.h b/phonelibs/android_frameworks_native/include/input/KeyCharacterMap.h deleted file mode 100644 index 3f0914b67..000000000 --- a/phonelibs/android_frameworks_native/include/input/KeyCharacterMap.h +++ /dev/null @@ -1,265 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_KEY_CHARACTER_MAP_H -#define _LIBINPUT_KEY_CHARACTER_MAP_H - -#include - -#if HAVE_ANDROID_OS -#include -#endif - -#include -#include -#include -#include -#include -#include -#include - -namespace android { - -/** - * Describes a mapping from Android key codes to characters. - * Also specifies other functions of the keyboard such as the keyboard type - * and key modifier semantics. - * - * This object is immutable after it has been loaded. - */ -class KeyCharacterMap : public RefBase { -public: - enum KeyboardType { - KEYBOARD_TYPE_UNKNOWN = 0, - KEYBOARD_TYPE_NUMERIC = 1, - KEYBOARD_TYPE_PREDICTIVE = 2, - KEYBOARD_TYPE_ALPHA = 3, - KEYBOARD_TYPE_FULL = 4, - KEYBOARD_TYPE_SPECIAL_FUNCTION = 5, - KEYBOARD_TYPE_OVERLAY = 6, - }; - - enum Format { - // Base keyboard layout, may contain device-specific options, such as "type" declaration. - FORMAT_BASE = 0, - // Overlay keyboard layout, more restrictive, may be published by applications, - // cannot override device-specific options. - FORMAT_OVERLAY = 1, - // Either base or overlay layout ok. - FORMAT_ANY = 2, - }; - - // Substitute key code and meta state for fallback action. - struct FallbackAction { - int32_t keyCode; - int32_t metaState; - }; - - /* Loads a key character map from a file. */ - static status_t load(const String8& filename, Format format, sp* outMap); - - /* Loads a key character map from its string contents. */ - static status_t loadContents(const String8& filename, - const char* contents, Format format, sp* outMap); - - /* Combines a base key character map and an overlay. */ - static sp combine(const sp& base, - const sp& overlay); - - /* Returns an empty key character map. */ - static sp empty(); - - /* Gets the keyboard type. */ - int32_t getKeyboardType() const; - - /* Gets the primary character for this key as in the label physically printed on it. - * Returns 0 if none (eg. for non-printing keys). */ - char16_t getDisplayLabel(int32_t keyCode) const; - - /* Gets the Unicode character for the number or symbol generated by the key - * when the keyboard is used as a dialing pad. - * Returns 0 if no number or symbol is generated. - */ - char16_t getNumber(int32_t keyCode) const; - - /* Gets the Unicode character generated by the key and meta key modifiers. - * Returns 0 if no character is generated. - */ - char16_t getCharacter(int32_t keyCode, int32_t metaState) const; - - /* Gets the fallback action to use by default if the application does not - * handle the specified key. - * Returns true if an action was available, false if none. - */ - bool getFallbackAction(int32_t keyCode, int32_t metaState, - FallbackAction* outFallbackAction) const; - - /* Gets the first matching Unicode character that can be generated by the key, - * preferring the one with the specified meta key modifiers. - * Returns 0 if no matching character is generated. - */ - char16_t getMatch(int32_t keyCode, const char16_t* chars, - size_t numChars, int32_t metaState) const; - - /* Gets a sequence of key events that could plausibly generate the specified - * character sequence. Returns false if some of the characters cannot be generated. - */ - bool getEvents(int32_t deviceId, const char16_t* chars, size_t numChars, - Vector& outEvents) const; - - /* Maps a scan code and usage code to a key code, in case this key map overrides - * the mapping in some way. */ - status_t mapKey(int32_t scanCode, int32_t usageCode, int32_t* outKeyCode) const; - - /* Tries to find a replacement key code for a given key code and meta state - * in character map. */ - void tryRemapKey(int32_t scanCode, int32_t metaState, - int32_t* outKeyCode, int32_t* outMetaState) const; - -#if HAVE_ANDROID_OS - /* Reads a key map from a parcel. */ - static sp readFromParcel(Parcel* parcel); - - /* Writes a key map to a parcel. */ - void writeToParcel(Parcel* parcel) const; -#endif - -protected: - virtual ~KeyCharacterMap(); - -private: - struct Behavior { - Behavior(); - Behavior(const Behavior& other); - - /* The next behavior in the list, or NULL if none. */ - Behavior* next; - - /* The meta key modifiers for this behavior. */ - int32_t metaState; - - /* The character to insert. */ - char16_t character; - - /* The fallback keycode if the key is not handled. */ - int32_t fallbackKeyCode; - - /* The replacement keycode if the key has to be replaced outright. */ - int32_t replacementKeyCode; - }; - - struct Key { - Key(); - Key(const Key& other); - ~Key(); - - /* The single character label printed on the key, or 0 if none. */ - char16_t label; - - /* The number or symbol character generated by the key, or 0 if none. */ - char16_t number; - - /* The list of key behaviors sorted from most specific to least specific - * meta key binding. */ - Behavior* firstBehavior; - }; - - class Parser { - enum State { - STATE_TOP = 0, - STATE_KEY = 1, - }; - - enum { - PROPERTY_LABEL = 1, - PROPERTY_NUMBER = 2, - PROPERTY_META = 3, - }; - - struct Property { - inline Property(int32_t property = 0, int32_t metaState = 0) : - property(property), metaState(metaState) { } - - int32_t property; - int32_t metaState; - }; - - KeyCharacterMap* mMap; - Tokenizer* mTokenizer; - Format mFormat; - State mState; - int32_t mKeyCode; - - public: - Parser(KeyCharacterMap* map, Tokenizer* tokenizer, Format format); - ~Parser(); - status_t parse(); - - private: - status_t parseType(); - status_t parseMap(); - status_t parseMapKey(); - status_t parseKey(); - status_t parseKeyProperty(); - status_t finishKey(Key* key); - status_t parseModifier(const String8& token, int32_t* outMetaState); - status_t parseCharacterLiteral(char16_t* outCharacter); - }; - - static sp sEmpty; - - KeyedVector mKeys; - int mType; - - KeyedVector mKeysByScanCode; - KeyedVector mKeysByUsageCode; - - KeyCharacterMap(); - KeyCharacterMap(const KeyCharacterMap& other); - - bool getKey(int32_t keyCode, const Key** outKey) const; - bool getKeyBehavior(int32_t keyCode, int32_t metaState, - const Key** outKey, const Behavior** outBehavior) const; - static bool matchesMetaState(int32_t eventMetaState, int32_t behaviorMetaState); - - bool findKey(char16_t ch, int32_t* outKeyCode, int32_t* outMetaState) const; - - static status_t load(Tokenizer* tokenizer, Format format, sp* outMap); - - static void addKey(Vector& outEvents, - int32_t deviceId, int32_t keyCode, int32_t metaState, bool down, nsecs_t time); - static void addMetaKeys(Vector& outEvents, - int32_t deviceId, int32_t metaState, bool down, nsecs_t time, - int32_t* currentMetaState); - static bool addSingleEphemeralMetaKey(Vector& outEvents, - int32_t deviceId, int32_t metaState, bool down, nsecs_t time, - int32_t keyCode, int32_t keyMetaState, - int32_t* currentMetaState); - static void addDoubleEphemeralMetaKey(Vector& outEvents, - int32_t deviceId, int32_t metaState, bool down, nsecs_t time, - int32_t leftKeyCode, int32_t leftKeyMetaState, - int32_t rightKeyCode, int32_t rightKeyMetaState, - int32_t eitherKeyMetaState, - int32_t* currentMetaState); - static void addLockedMetaKey(Vector& outEvents, - int32_t deviceId, int32_t metaState, nsecs_t time, - int32_t keyCode, int32_t keyMetaState, - int32_t* currentMetaState); -}; - -} // namespace android - -#endif // _LIBINPUT_KEY_CHARACTER_MAP_H diff --git a/phonelibs/android_frameworks_native/include/input/KeyLayoutMap.h b/phonelibs/android_frameworks_native/include/input/KeyLayoutMap.h deleted file mode 100644 index 1e8de7173..000000000 --- a/phonelibs/android_frameworks_native/include/input/KeyLayoutMap.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_KEY_LAYOUT_MAP_H -#define _LIBINPUT_KEY_LAYOUT_MAP_H - -#include -#include -#include -#include -#include - -namespace android { - -struct AxisInfo { - enum Mode { - // Axis value is reported directly. - MODE_NORMAL = 0, - // Axis value should be inverted before reporting. - MODE_INVERT = 1, - // Axis value should be split into two axes - MODE_SPLIT = 2, - }; - - // Axis mode. - Mode mode; - - // Axis id. - // When split, this is the axis used for values smaller than the split position. - int32_t axis; - - // When split, this is the axis used for values after higher than the split position. - int32_t highAxis; - - // The split value, or 0 if not split. - int32_t splitValue; - - // The flat value, or -1 if none. - int32_t flatOverride; - - AxisInfo() : mode(MODE_NORMAL), axis(-1), highAxis(-1), splitValue(0), flatOverride(-1) { - } -}; - -/** - * Describes a mapping from keyboard scan codes and joystick axes to Android key codes and axes. - * - * This object is immutable after it has been loaded. - */ -class KeyLayoutMap : public RefBase { -public: - static status_t load(const String8& filename, sp* outMap); - - status_t mapKey(int32_t scanCode, int32_t usageCode, - int32_t* outKeyCode, uint32_t* outFlags) const; - status_t findScanCodesForKey(int32_t keyCode, Vector* outScanCodes) const; - status_t findScanCodeForLed(int32_t ledCode, int32_t* outScanCode) const; - status_t findUsageCodeForLed(int32_t ledCode, int32_t* outUsageCode) const; - - status_t mapAxis(int32_t scanCode, AxisInfo* outAxisInfo) const; - -protected: - virtual ~KeyLayoutMap(); - -private: - struct Key { - int32_t keyCode; - uint32_t flags; - }; - - struct Led { - int32_t ledCode; - }; - - - KeyedVector mKeysByScanCode; - KeyedVector mKeysByUsageCode; - KeyedVector mAxes; - KeyedVector mLedsByScanCode; - KeyedVector mLedsByUsageCode; - - KeyLayoutMap(); - - const Key* getKey(int32_t scanCode, int32_t usageCode) const; - - class Parser { - KeyLayoutMap* mMap; - Tokenizer* mTokenizer; - - public: - Parser(KeyLayoutMap* map, Tokenizer* tokenizer); - ~Parser(); - status_t parse(); - - private: - status_t parseKey(); - status_t parseAxis(); - status_t parseLed(); - }; -}; - -} // namespace android - -#endif // _LIBINPUT_KEY_LAYOUT_MAP_H diff --git a/phonelibs/android_frameworks_native/include/input/Keyboard.h b/phonelibs/android_frameworks_native/include/input/Keyboard.h deleted file mode 100644 index d4903e98d..000000000 --- a/phonelibs/android_frameworks_native/include/input/Keyboard.h +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_KEYBOARD_H -#define _LIBINPUT_KEYBOARD_H - -#include -#include -#include -#include -#include -#include - -namespace android { - -enum { - /* Device id of the built in keyboard. */ - DEVICE_ID_BUILT_IN_KEYBOARD = 0, - - /* Device id of a generic virtual keyboard with a full layout that can be used - * to synthesize key events. */ - DEVICE_ID_VIRTUAL_KEYBOARD = -1, -}; - -class KeyLayoutMap; -class KeyCharacterMap; - -/** - * Loads the key layout map and key character map for a keyboard device. - */ -class KeyMap { -public: - String8 keyLayoutFile; - sp keyLayoutMap; - - String8 keyCharacterMapFile; - sp keyCharacterMap; - - KeyMap(); - ~KeyMap(); - - status_t load(const InputDeviceIdentifier& deviceIdenfier, - const PropertyMap* deviceConfiguration); - - inline bool haveKeyLayout() const { - return !keyLayoutFile.isEmpty(); - } - - inline bool haveKeyCharacterMap() const { - return !keyCharacterMapFile.isEmpty(); - } - - inline bool isComplete() const { - return haveKeyLayout() && haveKeyCharacterMap(); - } - -private: - bool probeKeyMap(const InputDeviceIdentifier& deviceIdentifier, const String8& name); - status_t loadKeyLayout(const InputDeviceIdentifier& deviceIdentifier, const String8& name); - status_t loadKeyCharacterMap(const InputDeviceIdentifier& deviceIdentifier, - const String8& name); - String8 getPath(const InputDeviceIdentifier& deviceIdentifier, - const String8& name, InputDeviceConfigurationFileType type); -}; - -/** - * Returns true if the keyboard is eligible for use as a built-in keyboard. - */ -extern bool isEligibleBuiltInKeyboard(const InputDeviceIdentifier& deviceIdentifier, - const PropertyMap* deviceConfiguration, const KeyMap* keyMap); - -/** - * Updates a meta state field when a key is pressed or released. - */ -extern int32_t updateMetaState(int32_t keyCode, bool down, int32_t oldMetaState); - -/** - * Normalizes the meta state such that if either the left or right modifier - * meta state bits are set then the result will also include the universal - * bit for that modifier. - */ -extern int32_t normalizeMetaState(int32_t oldMetaState); - -/** - * Returns true if a key is a meta key like ALT or CAPS_LOCK. - */ -extern bool isMetaKey(int32_t keyCode); - -} // namespace android - -#endif // _LIBINPUT_KEYBOARD_H diff --git a/phonelibs/android_frameworks_native/include/input/VelocityControl.h b/phonelibs/android_frameworks_native/include/input/VelocityControl.h deleted file mode 100644 index 1acc2aef7..000000000 --- a/phonelibs/android_frameworks_native/include/input/VelocityControl.h +++ /dev/null @@ -1,107 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_VELOCITY_CONTROL_H -#define _LIBINPUT_VELOCITY_CONTROL_H - -#include -#include -#include - -namespace android { - -/* - * Specifies parameters that govern pointer or wheel acceleration. - */ -struct VelocityControlParameters { - // A scale factor that is multiplied with the raw velocity deltas - // prior to applying any other velocity control factors. The scale - // factor should be used to adapt the input device resolution - // (eg. counts per inch) to the output device resolution (eg. pixels per inch). - // - // Must be a positive value. - // Default is 1.0 (no scaling). - float scale; - - // The scaled speed at which acceleration begins to be applied. - // This value establishes the upper bound of a low speed regime for - // small precise motions that are performed without any acceleration. - // - // Must be a non-negative value. - // Default is 0.0 (no low threshold). - float lowThreshold; - - // The scaled speed at which maximum acceleration is applied. - // The difference between highThreshold and lowThreshold controls - // the range of speeds over which the acceleration factor is interpolated. - // The wider the range, the smoother the acceleration. - // - // Must be a non-negative value greater than or equal to lowThreshold. - // Default is 0.0 (no high threshold). - float highThreshold; - - // The acceleration factor. - // When the speed is above the low speed threshold, the velocity will scaled - // by an interpolated value between 1.0 and this amount. - // - // Must be a positive greater than or equal to 1.0. - // Default is 1.0 (no acceleration). - float acceleration; - - VelocityControlParameters() : - scale(1.0f), lowThreshold(0.0f), highThreshold(0.0f), acceleration(1.0f) { - } - - VelocityControlParameters(float scale, float lowThreshold, - float highThreshold, float acceleration) : - scale(scale), lowThreshold(lowThreshold), - highThreshold(highThreshold), acceleration(acceleration) { - } -}; - -/* - * Implements mouse pointer and wheel speed control and acceleration. - */ -class VelocityControl { -public: - VelocityControl(); - - /* Sets the various parameters. */ - void setParameters(const VelocityControlParameters& parameters); - - /* Resets the current movement counters to zero. - * This has the effect of nullifying any acceleration. */ - void reset(); - - /* Translates a raw movement delta into an appropriately - * scaled / accelerated delta based on the current velocity. */ - void move(nsecs_t eventTime, float* deltaX, float* deltaY); - -private: - // If no movements are received within this amount of time, - // we assume the movement has stopped and reset the movement counters. - static const nsecs_t STOP_TIME = 500 * 1000000; // 500 ms - - VelocityControlParameters mParameters; - - nsecs_t mLastMovementTime; - VelocityTracker::Position mRawPosition; - VelocityTracker mVelocityTracker; -}; - -} // namespace android - -#endif // _LIBINPUT_VELOCITY_CONTROL_H diff --git a/phonelibs/android_frameworks_native/include/input/VelocityTracker.h b/phonelibs/android_frameworks_native/include/input/VelocityTracker.h deleted file mode 100644 index 795f575a2..000000000 --- a/phonelibs/android_frameworks_native/include/input/VelocityTracker.h +++ /dev/null @@ -1,269 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_VELOCITY_TRACKER_H -#define _LIBINPUT_VELOCITY_TRACKER_H - -#include -#include -#include - -namespace android { - -class VelocityTrackerStrategy; - -/* - * Calculates the velocity of pointer movements over time. - */ -class VelocityTracker { -public: - struct Position { - float x, y; - }; - - struct Estimator { - static const size_t MAX_DEGREE = 4; - - // Estimator time base. - nsecs_t time; - - // Polynomial coefficients describing motion in X and Y. - float xCoeff[MAX_DEGREE + 1], yCoeff[MAX_DEGREE + 1]; - - // Polynomial degree (number of coefficients), or zero if no information is - // available. - uint32_t degree; - - // Confidence (coefficient of determination), between 0 (no fit) and 1 (perfect fit). - float confidence; - - inline void clear() { - time = 0; - degree = 0; - confidence = 0; - for (size_t i = 0; i <= MAX_DEGREE; i++) { - xCoeff[i] = 0; - yCoeff[i] = 0; - } - } - }; - - // Creates a velocity tracker using the specified strategy. - // If strategy is NULL, uses the default strategy for the platform. - VelocityTracker(const char* strategy = NULL); - - ~VelocityTracker(); - - // Resets the velocity tracker state. - void clear(); - - // Resets the velocity tracker state for specific pointers. - // Call this method when some pointers have changed and may be reusing - // an id that was assigned to a different pointer earlier. - void clearPointers(BitSet32 idBits); - - // Adds movement information for a set of pointers. - // The idBits bitfield specifies the pointer ids of the pointers whose positions - // are included in the movement. - // The positions array contains position information for each pointer in order by - // increasing id. Its size should be equal to the number of one bits in idBits. - void addMovement(nsecs_t eventTime, BitSet32 idBits, const Position* positions); - - // Adds movement information for all pointers in a MotionEvent, including historical samples. - void addMovement(const MotionEvent* event); - - // Gets the velocity of the specified pointer id in position units per second. - // Returns false and sets the velocity components to zero if there is - // insufficient movement information for the pointer. - bool getVelocity(uint32_t id, float* outVx, float* outVy) const; - - // Gets an estimator for the recent movements of the specified pointer id. - // Returns false and clears the estimator if there is no information available - // about the pointer. - bool getEstimator(uint32_t id, Estimator* outEstimator) const; - - // Gets the active pointer id, or -1 if none. - inline int32_t getActivePointerId() const { return mActivePointerId; } - - // Gets a bitset containing all pointer ids from the most recent movement. - inline BitSet32 getCurrentPointerIdBits() const { return mCurrentPointerIdBits; } - -private: - static const char* DEFAULT_STRATEGY; - - nsecs_t mLastEventTime; - BitSet32 mCurrentPointerIdBits; - int32_t mActivePointerId; - VelocityTrackerStrategy* mStrategy; - - bool configureStrategy(const char* strategy); - - static VelocityTrackerStrategy* createStrategy(const char* strategy); -}; - - -/* - * Implements a particular velocity tracker algorithm. - */ -class VelocityTrackerStrategy { -protected: - VelocityTrackerStrategy() { } - -public: - virtual ~VelocityTrackerStrategy() { } - - virtual void clear() = 0; - virtual void clearPointers(BitSet32 idBits) = 0; - virtual void addMovement(nsecs_t eventTime, BitSet32 idBits, - const VelocityTracker::Position* positions) = 0; - virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const = 0; -}; - - -/* - * Velocity tracker algorithm based on least-squares linear regression. - */ -class LeastSquaresVelocityTrackerStrategy : public VelocityTrackerStrategy { -public: - enum Weighting { - // No weights applied. All data points are equally reliable. - WEIGHTING_NONE, - - // Weight by time delta. Data points clustered together are weighted less. - WEIGHTING_DELTA, - - // Weight such that points within a certain horizon are weighed more than those - // outside of that horizon. - WEIGHTING_CENTRAL, - - // Weight such that points older than a certain amount are weighed less. - WEIGHTING_RECENT, - }; - - // Degree must be no greater than Estimator::MAX_DEGREE. - LeastSquaresVelocityTrackerStrategy(uint32_t degree, Weighting weighting = WEIGHTING_NONE); - virtual ~LeastSquaresVelocityTrackerStrategy(); - - virtual void clear(); - virtual void clearPointers(BitSet32 idBits); - virtual void addMovement(nsecs_t eventTime, BitSet32 idBits, - const VelocityTracker::Position* positions); - virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const; - -private: - // Sample horizon. - // We don't use too much history by default since we want to react to quick - // changes in direction. - static const nsecs_t HORIZON = 100 * 1000000; // 100 ms - - // Number of samples to keep. - static const uint32_t HISTORY_SIZE = 20; - - struct Movement { - nsecs_t eventTime; - BitSet32 idBits; - VelocityTracker::Position positions[MAX_POINTERS]; - - inline const VelocityTracker::Position& getPosition(uint32_t id) const { - return positions[idBits.getIndexOfBit(id)]; - } - }; - - float chooseWeight(uint32_t index) const; - - const uint32_t mDegree; - const Weighting mWeighting; - uint32_t mIndex; - Movement mMovements[HISTORY_SIZE]; -}; - - -/* - * Velocity tracker algorithm that uses an IIR filter. - */ -class IntegratingVelocityTrackerStrategy : public VelocityTrackerStrategy { -public: - // Degree must be 1 or 2. - IntegratingVelocityTrackerStrategy(uint32_t degree); - ~IntegratingVelocityTrackerStrategy(); - - virtual void clear(); - virtual void clearPointers(BitSet32 idBits); - virtual void addMovement(nsecs_t eventTime, BitSet32 idBits, - const VelocityTracker::Position* positions); - virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const; - -private: - // Current state estimate for a particular pointer. - struct State { - nsecs_t updateTime; - uint32_t degree; - - float xpos, xvel, xaccel; - float ypos, yvel, yaccel; - }; - - const uint32_t mDegree; - BitSet32 mPointerIdBits; - State mPointerState[MAX_POINTER_ID + 1]; - - void initState(State& state, nsecs_t eventTime, float xpos, float ypos) const; - void updateState(State& state, nsecs_t eventTime, float xpos, float ypos) const; - void populateEstimator(const State& state, VelocityTracker::Estimator* outEstimator) const; -}; - - -/* - * Velocity tracker strategy used prior to ICS. - */ -class LegacyVelocityTrackerStrategy : public VelocityTrackerStrategy { -public: - LegacyVelocityTrackerStrategy(); - virtual ~LegacyVelocityTrackerStrategy(); - - virtual void clear(); - virtual void clearPointers(BitSet32 idBits); - virtual void addMovement(nsecs_t eventTime, BitSet32 idBits, - const VelocityTracker::Position* positions); - virtual bool getEstimator(uint32_t id, VelocityTracker::Estimator* outEstimator) const; - -private: - // Oldest sample to consider when calculating the velocity. - static const nsecs_t HORIZON = 200 * 1000000; // 100 ms - - // Number of samples to keep. - static const uint32_t HISTORY_SIZE = 20; - - // The minimum duration between samples when estimating velocity. - static const nsecs_t MIN_DURATION = 10 * 1000000; // 10 ms - - struct Movement { - nsecs_t eventTime; - BitSet32 idBits; - VelocityTracker::Position positions[MAX_POINTERS]; - - inline const VelocityTracker::Position& getPosition(uint32_t id) const { - return positions[idBits.getIndexOfBit(id)]; - } - }; - - uint32_t mIndex; - Movement mMovements[HISTORY_SIZE]; -}; - -} // namespace android - -#endif // _LIBINPUT_VELOCITY_TRACKER_H diff --git a/phonelibs/android_frameworks_native/include/input/VirtualKeyMap.h b/phonelibs/android_frameworks_native/include/input/VirtualKeyMap.h deleted file mode 100644 index e245ead68..000000000 --- a/phonelibs/android_frameworks_native/include/input/VirtualKeyMap.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * Copyright (C) 2010 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _LIBINPUT_VIRTUAL_KEY_MAP_H -#define _LIBINPUT_VIRTUAL_KEY_MAP_H - -#include - -#include -#include -#include -#include -#include -#include - -namespace android { - -/* Describes a virtual key. */ -struct VirtualKeyDefinition { - int32_t scanCode; - - // configured position data, specified in display coords - int32_t centerX; - int32_t centerY; - int32_t width; - int32_t height; -}; - - -/** - * Describes a collection of virtual keys on a touch screen in terms of - * virtual scan codes and hit rectangles. - * - * This object is immutable after it has been loaded. - */ -class VirtualKeyMap { -public: - ~VirtualKeyMap(); - - static status_t load(const String8& filename, VirtualKeyMap** outMap); - - inline const Vector& getVirtualKeys() const { - return mVirtualKeys; - } - -private: - class Parser { - VirtualKeyMap* mMap; - Tokenizer* mTokenizer; - - public: - Parser(VirtualKeyMap* map, Tokenizer* tokenizer); - ~Parser(); - status_t parse(); - - private: - bool consumeFieldDelimiterAndSkipWhitespace(); - bool parseNextIntField(int32_t* outValue); - }; - - Vector mVirtualKeys; - - VirtualKeyMap(); -}; - -} // namespace android - -#endif // _LIBINPUT_KEY_CHARACTER_MAP_H diff --git a/phonelibs/android_frameworks_native/include/media/drm/DrmAPI.h b/phonelibs/android_frameworks_native/include/media/drm/DrmAPI.h deleted file mode 100644 index 272881b9f..000000000 --- a/phonelibs/android_frameworks_native/include/media/drm/DrmAPI.h +++ /dev/null @@ -1,419 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef DRM_API_H_ -#define DRM_API_H_ - -#include -#include -#include -#include -#include -#include -#include - -// Loadable DrmEngine shared libraries should define the entry points -// createDrmFactory and createCryptoFactory as shown below: -// -// extern "C" { -// extern android::DrmFactory *createDrmFactory(); -// extern android::CryptoFactory *createCryptoFactory(); -// } - -namespace android { - - class DrmPlugin; - class DrmPluginListener; - - // DRMs are implemented in DrmEngine plugins, which are dynamically - // loadable shared libraries that implement the entry points - // createDrmFactory and createCryptoFactory. createDrmFactory - // constructs and returns an instance of a DrmFactory object. Similarly, - // createCryptoFactory creates an instance of a CryptoFactory object. - // When a MediaCrypto or MediaDrm object needs to be constructed, all - // available DrmEngines present in the plugins directory on the device - // are scanned for a matching DrmEngine that can support the crypto - // scheme. When a match is found, the DrmEngine's createCryptoPlugin and - // createDrmPlugin methods are used to create CryptoPlugin or - // DrmPlugin instances to support that DRM scheme. - - class DrmFactory { - public: - DrmFactory() {} - virtual ~DrmFactory() {} - - // DrmFactory::isCryptoSchemeSupported can be called to determine - // if the plugin factory is able to construct plugins that support a - // given crypto scheme, which is specified by a UUID. - virtual bool isCryptoSchemeSupported(const uint8_t uuid[16]) = 0; - - // DrmFactory::isContentTypeSupported can be called to determine - // if the plugin factory is able to construct plugins that support a - // given media container format specified by mimeType - virtual bool isContentTypeSupported(const String8 &mimeType) = 0; - - // Construct a DrmPlugin for the crypto scheme specified by UUID. - virtual status_t createDrmPlugin( - const uint8_t uuid[16], DrmPlugin **plugin) = 0; - - private: - DrmFactory(const DrmFactory &); - DrmFactory &operator=(const DrmFactory &); - }; - - class DrmPlugin { - public: - enum EventType { - kDrmPluginEventProvisionRequired = 1, - kDrmPluginEventKeyNeeded, - kDrmPluginEventKeyExpired, - kDrmPluginEventVendorDefined, - kDrmPluginEventSessionReclaimed, - kDrmPluginEventExpirationUpdate, - kDrmPluginEventKeysChange, - }; - - // Drm keys can be for offline content or for online streaming. - // Offline keys are persisted on the device and may be used when the device - // is disconnected from the network. The Release type is used to request - // that offline keys be no longer restricted to offline use. - enum KeyType { - kKeyType_Offline, - kKeyType_Streaming, - kKeyType_Release - }; - - // Enumerate KeyRequestTypes to allow an app to determine the - // type of a key request returned from getKeyRequest. - enum KeyRequestType { - kKeyRequestType_Unknown, - kKeyRequestType_Initial, - kKeyRequestType_Renewal, - kKeyRequestType_Release - }; - - // Enumerate KeyStatusTypes which indicate the state of a key - enum KeyStatusType - { - kKeyStatusType_Usable, - kKeyStatusType_Expired, - kKeyStatusType_OutputNotAllowed, - kKeyStatusType_StatusPending, - kKeyStatusType_InternalError - }; - - // Used by sendKeysChange to report the usability status of each - // key to the app. - struct KeyStatus - { - Vector mKeyId; - KeyStatusType mType; - }; - - DrmPlugin() {} - virtual ~DrmPlugin() {} - - // Open a new session with the DrmPlugin object. A session ID is returned - // in the sessionId parameter. - virtual status_t openSession(Vector &sessionId) = 0; - - // Close a session on the DrmPlugin object. - virtual status_t closeSession(Vector const &sessionId) = 0; - - // A key request/response exchange occurs between the app and a License - // Server to obtain the keys required to decrypt the content. getKeyRequest() - // is used to obtain an opaque key request blob that is delivered to the - // license server. - // - // The scope parameter may be a sessionId or a keySetId, depending on the - // specified keyType. When the keyType is kKeyType_Offline or - // kKeyType_Streaming, scope should be set to the sessionId the keys will be - // provided to. When the keyType is kKeyType_Release, scope should be set to - // the keySetId of the keys being released. Releasing keys from a device - // invalidates them for all sessions. - // - // The init data passed to getKeyRequest is container-specific and its - // meaning is interpreted based on the mime type provided in the mimeType - // parameter to getKeyRequest. It could contain, for example, the content - // ID, key ID or other data obtained from the content metadata that is required - // in generating the key request. Init may be null when keyType is - // kKeyType_Release. - // - // mimeType identifies the mime type of the content - // - // keyType specifies if the keys are to be used for streaming or offline content - // - // optionalParameters are included in the key request message to allow a - // client application to provide additional message parameters to the server. - // - // If successful, the opaque key request blob is returned to the caller. - virtual status_t - getKeyRequest(Vector const &scope, - Vector const &initData, - String8 const &mimeType, KeyType keyType, - KeyedVector const &optionalParameters, - Vector &request, String8 &defaultUrl, - KeyRequestType *keyRequestType) = 0; - - // - // After a key response is received by the app, it is provided to the - // Drm plugin using provideKeyResponse. - // - // scope may be a sessionId or a keySetId depending on the type of the - // response. Scope should be set to the sessionId when the response is - // for either streaming or offline key requests. Scope should be set to the - // keySetId when the response is for a release request. - // - // When the response is for an offline key request, a keySetId is returned - // in the keySetId vector parameter that can be used to later restore the - // keys to a new session with the method restoreKeys. When the response is - // for a streaming or release request, no keySetId is returned. - // - virtual status_t provideKeyResponse(Vector const &scope, - Vector const &response, - Vector &keySetId) = 0; - - // Remove the current keys from a session - virtual status_t removeKeys(Vector const &sessionId) = 0; - - // Restore persisted offline keys into a new session. keySetId identifies - // the keys to load, obtained from a prior call to provideKeyResponse(). - virtual status_t restoreKeys(Vector const &sessionId, - Vector const &keySetId) = 0; - - // Request an informative description of the license for the session. The status - // is in the form of {name, value} pairs. Since DRM license policies vary by - // vendor, the specific status field names are determined by each DRM vendor. - // Refer to your DRM provider documentation for definitions of the field names - // for a particular DrmEngine. - virtual status_t - queryKeyStatus(Vector const &sessionId, - KeyedVector &infoMap) const = 0; - - // A provision request/response exchange occurs between the app and a - // provisioning server to retrieve a device certificate. getProvisionRequest - // is used to obtain an opaque key request blob that is delivered to the - // provisioning server. - // - // If successful, the opaque provision request blob is returned to the caller. - virtual status_t getProvisionRequest(String8 const &cert_type, - String8 const &cert_authority, - Vector &request, - String8 &defaultUrl) = 0; - - // After a provision response is received by the app, it is provided to the - // Drm plugin using provideProvisionResponse. - virtual status_t provideProvisionResponse(Vector const &response, - Vector &certificate, - Vector &wrapped_key) = 0; - - // Remove device provisioning. - virtual status_t unprovisionDevice() = 0; - - // A means of enforcing the contractual requirement for a concurrent stream - // limit per subscriber across devices is provided via SecureStop. SecureStop - // is a means of securely monitoring the lifetime of sessions. Since playback - // on a device can be interrupted due to reboot, power failure, etc. a means - // of persisting the lifetime information on the device is needed. - // - // A signed version of the sessionID is written to persistent storage on the - // device when each MediaCrypto object is created. The sessionID is signed by - // the device private key to prevent tampering. - // - // In the normal case, playback will be completed, the session destroyed and - // the Secure Stops will be queried. The App queries secure stops and forwards - // the secure stop message to the server which verifies the signature and - // notifies the server side database that the session destruction has been - // confirmed. The persisted record on the client is only removed after positive - // confirmation that the server received the message using releaseSecureStops(). - virtual status_t getSecureStops(List > &secureStops) = 0; - virtual status_t getSecureStop(Vector const &ssid, Vector &secureStop) = 0; - virtual status_t releaseSecureStops(Vector const &ssRelease) = 0; - virtual status_t releaseAllSecureStops() = 0; - - // Read a property value given the device property string. There are a few forms - // of property access methods, depending on the data type returned. - // Since DRM plugin properties may vary, additional field names may be defined - // by each DRM vendor. Refer to your DRM provider documentation for definitions - // of its additional field names. - // - // Standard values are: - // "vendor" [string] identifies the maker of the plugin - // "version" [string] identifies the version of the plugin - // "description" [string] describes the plugin - // 'deviceUniqueId' [byte array] The device unique identifier is established - // during device provisioning and provides a means of uniquely identifying - // each device. - virtual status_t getPropertyString(String8 const &name, String8 &value ) const = 0; - virtual status_t getPropertyByteArray(String8 const &name, - Vector &value ) const = 0; - - // Write a property value given the device property string. There are a few forms - // of property setting methods, depending on the data type. - // Since DRM plugin properties may vary, additional field names may be defined - // by each DRM vendor. Refer to your DRM provider documentation for definitions - // of its field names. - virtual status_t setPropertyString(String8 const &name, - String8 const &value ) = 0; - virtual status_t setPropertyByteArray(String8 const &name, - Vector const &value ) = 0; - - // The following methods implement operations on a CryptoSession to support - // encrypt, decrypt, sign verify operations on operator-provided - // session keys. - - // - // The algorithm string conforms to JCA Standard Names for Cipher - // Transforms and is case insensitive. For example "AES/CBC/PKCS5Padding". - // - // Return OK if the algorithm is supported, otherwise return BAD_VALUE - // - virtual status_t setCipherAlgorithm(Vector const &sessionId, - String8 const &algorithm) = 0; - - // - // The algorithm string conforms to JCA Standard Names for Mac - // Algorithms and is case insensitive. For example "HmacSHA256". - // - // Return OK if the algorithm is supported, otherwise return BAD_VALUE - // - virtual status_t setMacAlgorithm(Vector const &sessionId, - String8 const &algorithm) = 0; - - // Encrypt the provided input buffer with the cipher algorithm - // specified by setCipherAlgorithm and the key selected by keyId, - // and return the encrypted data. - virtual status_t encrypt(Vector const &sessionId, - Vector const &keyId, - Vector const &input, - Vector const &iv, - Vector &output) = 0; - - // Decrypt the provided input buffer with the cipher algorithm - // specified by setCipherAlgorithm and the key selected by keyId, - // and return the decrypted data. - virtual status_t decrypt(Vector const &sessionId, - Vector const &keyId, - Vector const &input, - Vector const &iv, - Vector &output) = 0; - - // Compute a signature on the provided message using the mac algorithm - // specified by setMacAlgorithm and the key selected by keyId, - // and return the signature. - virtual status_t sign(Vector const &sessionId, - Vector const &keyId, - Vector const &message, - Vector &signature) = 0; - - // Compute a signature on the provided message using the mac algorithm - // specified by setMacAlgorithm and the key selected by keyId, - // and compare with the expected result. Set result to true or - // false depending on the outcome. - virtual status_t verify(Vector const &sessionId, - Vector const &keyId, - Vector const &message, - Vector const &signature, - bool &match) = 0; - - - // Compute an RSA signature on the provided message using the algorithm - // specified by algorithm. - virtual status_t signRSA(Vector const &sessionId, - String8 const &algorithm, - Vector const &message, - Vector const &wrapped_key, - Vector &signature) = 0; - - - status_t setListener(const sp& listener) { - Mutex::Autolock lock(mEventLock); - mListener = listener; - return OK; - } - - protected: - // Plugins call these methods to deliver events to the java app - void sendEvent(EventType eventType, int extra, - Vector const *sessionId, - Vector const *data); - - void sendExpirationUpdate(Vector const *sessionId, - int64_t expiryTimeInMS); - - void sendKeysChange(Vector const *sessionId, - Vector const *keyStatusList, - bool hasNewUsableKey); - - private: - Mutex mEventLock; - sp mListener; - - DISALLOW_EVIL_CONSTRUCTORS(DrmPlugin); - }; - - class DrmPluginListener: virtual public RefBase - { - public: - virtual void sendEvent(DrmPlugin::EventType eventType, int extra, - Vector const *sessionId, - Vector const *data) = 0; - - virtual void sendExpirationUpdate(Vector const *sessionId, - int64_t expiryTimeInMS) = 0; - - virtual void sendKeysChange(Vector const *sessionId, - Vector const *keyStatusList, - bool hasNewUsableKey) = 0; - }; - - inline void DrmPlugin::sendEvent(EventType eventType, int extra, - Vector const *sessionId, - Vector const *data) { - mEventLock.lock(); - sp listener = mListener; - mEventLock.unlock(); - - if (listener != NULL) { - listener->sendEvent(eventType, extra, sessionId, data); - } - } - - inline void DrmPlugin::sendExpirationUpdate(Vector const *sessionId, - int64_t expiryTimeInMS) { - mEventLock.lock(); - sp listener = mListener; - mEventLock.unlock(); - - if (listener != NULL) { - listener->sendExpirationUpdate(sessionId, expiryTimeInMS); - } - } - - inline void DrmPlugin::sendKeysChange(Vector const *sessionId, - Vector const *keyStatusList, - bool hasNewUsableKey) { - mEventLock.lock(); - sp listener = mListener; - mEventLock.unlock(); - - if (listener != NULL) { - listener->sendKeysChange(sessionId, keyStatusList, hasNewUsableKey); - } - } -} // namespace android - -#endif // DRM_API_H_ diff --git a/phonelibs/android_frameworks_native/include/media/editor/II420ColorConverter.h b/phonelibs/android_frameworks_native/include/media/editor/II420ColorConverter.h deleted file mode 100644 index 33af61ff9..000000000 --- a/phonelibs/android_frameworks_native/include/media/editor/II420ColorConverter.h +++ /dev/null @@ -1,126 +0,0 @@ -/* - * Copyright (C) 2011 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef II420_COLOR_CONVERTER_H - -#define II420_COLOR_CONVERTER_H - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -typedef struct II420ColorConverter { - - /* - * getDecoderOutputFormat - * Returns the color format (OMX_COLOR_FORMATTYPE) of the decoder output. - * If it is I420 (OMX_COLOR_FormatYUV420Planar), no conversion is needed, - * and convertDecoderOutputToI420() can be a no-op. - */ - int (*getDecoderOutputFormat)(); - - /* - * convertDecoderOutputToI420 - * @Desc Converts from the decoder output format to I420 format. - * @note Caller (e.g. VideoEditor) owns the buffers - * @param decoderBits (IN) Pointer to the buffer contains decoder output - * @param decoderWidth (IN) Buffer width, as reported by the decoder - * metadata (kKeyWidth) - * @param decoderHeight (IN) Buffer height, as reported by the decoder - * metadata (kKeyHeight) - * @param decoderRect (IN) The rectangle of the actual frame, as - * reported by decoder metadata (kKeyCropRect) - * @param dstBits (OUT) Pointer to the output I420 buffer - * @return -1 Any error - * @return 0 No Error - */ - int (*convertDecoderOutputToI420)( - void* decoderBits, int decoderWidth, int decoderHeight, - ARect decoderRect, void* dstBits); - - /* - * getEncoderIntputFormat - * Returns the color format (OMX_COLOR_FORMATTYPE) of the encoder input. - * If it is I420 (OMX_COLOR_FormatYUV420Planar), no conversion is needed, - * and convertI420ToEncoderInput() and getEncoderInputBufferInfo() can - * be no-ops. - */ - int (*getEncoderInputFormat)(); - - /* convertI420ToEncoderInput - * @Desc This function converts from I420 to the encoder input format - * @note Caller (e.g. VideoEditor) owns the buffers - * @param srcBits (IN) Pointer to the input I420 buffer - * @param srcWidth (IN) Width of the I420 frame - * @param srcHeight (IN) Height of the I420 frame - * @param encoderWidth (IN) Encoder buffer width, as calculated by - * getEncoderBufferInfo() - * @param encoderHeight (IN) Encoder buffer height, as calculated by - * getEncoderBufferInfo() - * @param encoderRect (IN) Rect coordinates of the actual frame inside - * the encoder buffer, as calculated by - * getEncoderBufferInfo(). - * @param encoderBits (OUT) Pointer to the output buffer. The size of - * this buffer is calculated by - * getEncoderBufferInfo() - * @return -1 Any error - * @return 0 No Error - */ - int (*convertI420ToEncoderInput)( - void* srcBits, int srcWidth, int srcHeight, - int encoderWidth, int encoderHeight, ARect encoderRect, - void* encoderBits); - - /* getEncoderInputBufferInfo - * @Desc This function returns metadata for the encoder input buffer - * based on the actual I420 frame width and height. - * @note This API should be be used to obtain the necessary information - * before calling convertI420ToEncoderInput(). - * VideoEditor knows only the width and height of the I420 buffer, - * but it also needs know the width, height, and size of the - * encoder input buffer. The encoder input buffer width and height - * are used to set the metadata for the encoder. - * @param srcWidth (IN) Width of the I420 frame - * @param srcHeight (IN) Height of the I420 frame - * @param encoderWidth (OUT) Encoder buffer width needed - * @param encoderHeight (OUT) Encoder buffer height needed - * @param encoderRect (OUT) Rect coordinates of the actual frame inside - * the encoder buffer - * @param encoderBufferSize (OUT) The size of the buffer that need to be - * allocated by the caller before invoking - * convertI420ToEncoderInput(). - * @return -1 Any error - * @return 0 No Error - */ - int (*getEncoderInputBufferInfo)( - int srcWidth, int srcHeight, - int* encoderWidth, int* encoderHeight, - ARect* encoderRect, int* encoderBufferSize); - -} II420ColorConverter; - -/* The only function that the shared library needs to expose: It fills the - function pointers in II420ColorConverter */ -void getI420ColorConverter(II420ColorConverter *converter); - -#if defined(__cplusplus) -} -#endif - -#endif // II420_COLOR_CONVERTER_H diff --git a/phonelibs/android_frameworks_native/include/media/hardware/CryptoAPI.h b/phonelibs/android_frameworks_native/include/media/hardware/CryptoAPI.h deleted file mode 100644 index 3e3257f95..000000000 --- a/phonelibs/android_frameworks_native/include/media/hardware/CryptoAPI.h +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include - -#ifndef CRYPTO_API_H_ - -#define CRYPTO_API_H_ - -namespace android { - -struct AString; -struct CryptoPlugin; - -struct CryptoFactory { - CryptoFactory() {} - virtual ~CryptoFactory() {} - - virtual bool isCryptoSchemeSupported(const uint8_t uuid[16]) const = 0; - - virtual status_t createPlugin( - const uint8_t uuid[16], const void *data, size_t size, - CryptoPlugin **plugin) = 0; - -private: - CryptoFactory(const CryptoFactory &); - CryptoFactory &operator=(const CryptoFactory &); -}; - -struct CryptoPlugin { - enum Mode { - kMode_Unencrypted = 0, - kMode_AES_CTR = 1, - - // Neither key nor iv are being used in this mode. - // Each subsample is encrypted w/ an iv of all zeroes. - kMode_AES_WV = 2, // FIX constant - }; - - struct SubSample { - uint32_t mNumBytesOfClearData; - uint32_t mNumBytesOfEncryptedData; - }; - - CryptoPlugin() {} - virtual ~CryptoPlugin() {} - - // If this method returns false, a non-secure decoder will be used to - // decode the data after decryption. The decrypt API below will have - // to support insecure decryption of the data (secure = false) for - // media data of the given mime type. - virtual bool requiresSecureDecoderComponent(const char *mime) const = 0; - - // To implement resolution constraints, the crypto plugin needs to know - // the resolution of the video being decrypted. The media player should - // call this method when the resolution is determined and any time it - // is subsequently changed. - - virtual void notifyResolution(uint32_t /* width */, uint32_t /* height */) {} - - // A MediaDrm session may be associated with a MediaCrypto session. The - // associated MediaDrm session is used to load decryption keys - // into the crypto/drm plugin. The keys are then referenced by key-id - // in the 'key' parameter to the decrypt() method. - // Should return NO_ERROR on success, ERROR_DRM_SESSION_NOT_OPENED if - // the session is not opened and a code from MediaErrors.h otherwise. - virtual status_t setMediaDrmSession(const Vector & /*sessionId */) { - return ERROR_UNSUPPORTED; - } - - // If the error returned falls into the range - // ERROR_DRM_VENDOR_MIN..ERROR_DRM_VENDOR_MAX, errorDetailMsg should be - // filled in with an appropriate string. - // At the java level these special errors will then trigger a - // MediaCodec.CryptoException that gives clients access to both - // the error code and the errorDetailMsg. - // Returns a non-negative result to indicate the number of bytes written - // to the dstPtr, or a negative result to indicate an error. - virtual ssize_t decrypt( - bool secure, - const uint8_t key[16], - const uint8_t iv[16], - Mode mode, - const void *srcPtr, - const SubSample *subSamples, size_t numSubSamples, - void *dstPtr, - AString *errorDetailMsg) = 0; - -private: - CryptoPlugin(const CryptoPlugin &); - CryptoPlugin &operator=(const CryptoPlugin &); -}; - -} // namespace android - -extern "C" { - extern android::CryptoFactory *createCryptoFactory(); -} - -#endif // CRYPTO_API_H_ diff --git a/phonelibs/android_frameworks_native/include/media/hardware/HDCPAPI.h b/phonelibs/android_frameworks_native/include/media/hardware/HDCPAPI.h deleted file mode 100644 index 3a53e9fc9..000000000 --- a/phonelibs/android_frameworks_native/include/media/hardware/HDCPAPI.h +++ /dev/null @@ -1,164 +0,0 @@ -/* - * Copyright (C) 2012 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef HDCP_API_H_ - -#define HDCP_API_H_ - -#include -#include - -namespace android { - -// Two different kinds of modules are covered under the same HDCPModule -// structure below, a module either implements decryption or encryption. -struct HDCPModule { - typedef void (*ObserverFunc)(void *cookie, int msg, int ext1, int ext2); - - // The msg argument in calls to the observer notification function. - enum { - // Sent in response to a call to "HDCPModule::initAsync" once - // initialization has either been successfully completed, - // i.e. the HDCP session is now fully setup (AKE, Locality Check, - // SKE and any authentication with repeaters completed) or failed. - // ext1 should be a suitable error code (status_t), ext2 is - // unused for ENCRYPTION and in the case of HDCP_INITIALIZATION_COMPLETE - // holds the local TCP port the module is listening on. - HDCP_INITIALIZATION_COMPLETE, - HDCP_INITIALIZATION_FAILED, - - // Sent upon completion of a call to "HDCPModule::shutdownAsync". - // ext1 should be a suitable error code, ext2 is unused. - HDCP_SHUTDOWN_COMPLETE, - HDCP_SHUTDOWN_FAILED, - - HDCP_UNAUTHENTICATED_CONNECTION, - HDCP_UNAUTHORIZED_CONNECTION, - HDCP_REVOKED_CONNECTION, - HDCP_TOPOLOGY_EXECEEDED, - HDCP_UNKNOWN_ERROR, - - // DECRYPTION only: Indicates that a client has successfully connected, - // a secure session established and the module is ready to accept - // future calls to "decrypt". - HDCP_SESSION_ESTABLISHED, - }; - - // HDCPModule capability bit masks - enum { - // HDCP_CAPS_ENCRYPT: mandatory, meaning the HDCP module can encrypt - // from an input byte-array buffer to an output byte-array buffer - HDCP_CAPS_ENCRYPT = (1 << 0), - // HDCP_CAPS_ENCRYPT_NATIVE: the HDCP module supports encryption from - // a native buffer to an output byte-array buffer. The format of the - // input native buffer is specific to vendor's encoder implementation. - // It is the same format as that used by the encoder when - // "storeMetaDataInBuffers" extension is enabled on its output port. - HDCP_CAPS_ENCRYPT_NATIVE = (1 << 1), - }; - - // Module can call the notification function to signal completion/failure - // of asynchronous operations (such as initialization) or out of band - // events. - HDCPModule(void *cookie, ObserverFunc observerNotify) {}; - - virtual ~HDCPModule() {}; - - // ENCRYPTION: Request to setup an HDCP session with the host specified - // by addr and listening on the specified port. - // DECRYPTION: Request to setup an HDCP session, addr is the interface - // address the module should bind its socket to. port will be 0. - // The module will pick the port to listen on itself and report its choice - // in the "ext2" argument of the HDCP_INITIALIZATION_COMPLETE callback. - virtual status_t initAsync(const char *addr, unsigned port) = 0; - - // Request to shutdown the active HDCP session. - virtual status_t shutdownAsync() = 0; - - // Returns the capability bitmask of this HDCP session. - virtual uint32_t getCaps() { - return HDCP_CAPS_ENCRYPT; - } - - // ENCRYPTION only: - // Encrypt data according to the HDCP spec. "size" bytes of data are - // available at "inData" (virtual address), "size" may not be a multiple - // of 128 bits (16 bytes). An equal number of encrypted bytes should be - // written to the buffer at "outData" (virtual address). - // This operation is to be synchronous, i.e. this call does not return - // until outData contains size bytes of encrypted data. - // streamCTR will be assigned by the caller (to 0 for the first PES stream, - // 1 for the second and so on) - // inputCTR _will_be_maintained_by_the_callee_ for each PES stream. - virtual status_t encrypt( - const void *inData, size_t size, uint32_t streamCTR, - uint64_t *outInputCTR, void *outData) { - return INVALID_OPERATION; - } - - // Encrypt data according to the HDCP spec. "size" bytes of data starting - // at location "offset" are available in "buffer" (buffer handle). "size" - // may not be a multiple of 128 bits (16 bytes). An equal number of - // encrypted bytes should be written to the buffer at "outData" (virtual - // address). This operation is to be synchronous, i.e. this call does not - // return until outData contains size bytes of encrypted data. - // streamCTR will be assigned by the caller (to 0 for the first PES stream, - // 1 for the second and so on) - // inputCTR _will_be_maintained_by_the_callee_ for each PES stream. - virtual status_t encryptNative( - buffer_handle_t buffer, size_t offset, size_t size, - uint32_t streamCTR, uint64_t *outInputCTR, void *outData) { - return INVALID_OPERATION; - } - // DECRYPTION only: - // Decrypt data according to the HDCP spec. - // "size" bytes of encrypted data are available at "inData" - // (virtual address), "size" may not be a multiple of 128 bits (16 bytes). - // An equal number of decrypted bytes should be written to the buffer - // at "outData" (virtual address). - // This operation is to be synchronous, i.e. this call does not return - // until outData contains size bytes of decrypted data. - // Both streamCTR and inputCTR will be provided by the caller. - virtual status_t decrypt( - const void *inData, size_t size, - uint32_t streamCTR, uint64_t inputCTR, - void *outData) { - return INVALID_OPERATION; - } - -private: - HDCPModule(const HDCPModule &); - HDCPModule &operator=(const HDCPModule &); -}; - -} // namespace android - -// A shared library exporting the following methods should be included to -// support HDCP functionality. The shared library must be called -// "libstagefright_hdcp.so", it will be dynamically loaded into the -// mediaserver process. -extern "C" { - // Create a module for ENCRYPTION. - extern android::HDCPModule *createHDCPModule( - void *cookie, android::HDCPModule::ObserverFunc); - - // Create a module for DECRYPTION. - extern android::HDCPModule *createHDCPModuleForDecryption( - void *cookie, android::HDCPModule::ObserverFunc); -} - -#endif // HDCP_API_H_ - diff --git a/phonelibs/android_frameworks_native/include/media/hardware/HardwareAPI.h b/phonelibs/android_frameworks_native/include/media/hardware/HardwareAPI.h deleted file mode 100644 index 1008c22d7..000000000 --- a/phonelibs/android_frameworks_native/include/media/hardware/HardwareAPI.h +++ /dev/null @@ -1,288 +0,0 @@ -/* - * Copyright (C) 2009 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef HARDWARE_API_H_ - -#define HARDWARE_API_H_ - -#include -#include -#include -#include - -#include - -namespace android { - -// A pointer to this struct is passed to the OMX_SetParameter when the extension -// index for the 'OMX.google.android.index.enableAndroidNativeBuffers' extension -// is given. -// -// When Android native buffer use is disabled for a port (the default state), -// the OMX node should operate as normal, and expect UseBuffer calls to set its -// buffers. This is the mode that will be used when CPU access to the buffer is -// required. -// -// When Android native buffer use has been enabled for a given port, the video -// color format for the port is to be interpreted as an Android pixel format -// rather than an OMX color format. Enabling Android native buffers may also -// change how the component receives the native buffers. If store-metadata-mode -// is enabled on the port, the component will receive the buffers as specified -// in the section below. Otherwise, unless the node supports the -// 'OMX.google.android.index.useAndroidNativeBuffer2' extension, it should -// expect to receive UseAndroidNativeBuffer calls (via OMX_SetParameter) rather -// than UseBuffer calls for that port. -struct EnableAndroidNativeBuffersParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL enable; -}; - -// A pointer to this struct is passed to OMX_SetParameter() when the extension index -// "OMX.google.android.index.storeMetaDataInBuffers" or -// "OMX.google.android.index.storeANWBufferInMetadata" is given. -// -// When meta data is stored in the video buffers passed between OMX clients -// and OMX components, interpretation of the buffer data is up to the -// buffer receiver, and the data may or may not be the actual video data, but -// some information helpful for the receiver to locate the actual data. -// The buffer receiver thus needs to know how to interpret what is stored -// in these buffers, with mechanisms pre-determined externally. How to -// interpret the meta data is outside of the scope of this parameter. -// -// Currently, this is used to pass meta data from video source (camera component, for instance) to -// video encoder to avoid memcpying of input video frame data, as well as to pass dynamic output -// buffer to video decoder. To do this, bStoreMetaData is set to OMX_TRUE. -// -// If bStoreMetaData is set to false, real YUV frame data will be stored in input buffers, and -// the output buffers contain either real YUV frame data, or are themselves native handles as -// directed by enable/use-android-native-buffer parameter settings. -// In addition, if no OMX_SetParameter() call is made on a port with the corresponding extension -// index, the component should not assume that the client is not using metadata mode for the port. -// -// If the component supports this using the "OMX.google.android.index.storeANWBufferInMetadata" -// extension and bStoreMetaData is set to OMX_TRUE, data is passed using the VideoNativeMetadata -// layout as defined below. Each buffer will be accompanied by a fence. The fence must signal -// before the buffer can be used (e.g. read from or written into). When returning such buffer to -// the client, component must provide a new fence that must signal before the returned buffer can -// be used (e.g. read from or written into). The component owns the incoming fenceFd, and must close -// it when fence has signaled. The client will own and close the returned fence file descriptor. -// -// If the component supports this using the "OMX.google.android.index.storeMetaDataInBuffers" -// extension and bStoreMetaData is set to OMX_TRUE, data is passed using VideoGrallocMetadata -// (the layout of which is the VideoGrallocMetadata defined below). Camera input can be also passed -// as "CameraSource", the layout of which is vendor dependent. -// -// Metadata buffers are registered with the component using UseBuffer calls, or can be allocated -// by the component for encoder-metadata-output buffers. -struct StoreMetaDataInBuffersParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bStoreMetaData; -}; - -// Meta data buffer layout used to transport output frames to the decoder for -// dynamic buffer handling. -struct VideoGrallocMetadata { - MetadataBufferType eType; // must be kMetadataBufferTypeGrallocSource -#ifdef OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS - OMX_PTR pHandle; -#else - buffer_handle_t pHandle; -#endif -}; - -// Legacy name for VideoGrallocMetadata struct. -struct VideoDecoderOutputMetaData : public VideoGrallocMetadata {}; - -struct VideoNativeMetadata { - MetadataBufferType eType; // must be kMetadataBufferTypeANWBuffer -#ifdef OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS - OMX_PTR pBuffer; -#else - struct ANativeWindowBuffer* pBuffer; -#endif - int nFenceFd; // -1 if unused -}; - -// A pointer to this struct is passed to OMX_SetParameter() when the extension -// index "OMX.google.android.index.prepareForAdaptivePlayback" is given. -// -// This method is used to signal a video decoder, that the user has requested -// seamless resolution change support (if bEnable is set to OMX_TRUE). -// nMaxFrameWidth and nMaxFrameHeight are the dimensions of the largest -// anticipated frames in the video. If bEnable is OMX_FALSE, no resolution -// change is expected, and the nMaxFrameWidth/Height fields are unused. -// -// If the decoder supports dynamic output buffers, it may ignore this -// request. Otherwise, it shall request resources in such a way so that it -// avoids full port-reconfiguration (due to output port-definition change) -// during resolution changes. -// -// DO NOT USE THIS STRUCTURE AS IT WILL BE REMOVED. INSTEAD, IMPLEMENT -// METADATA SUPPORT FOR VIDEO DECODERS. -struct PrepareForAdaptivePlaybackParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bEnable; - OMX_U32 nMaxFrameWidth; - OMX_U32 nMaxFrameHeight; -}; - -// A pointer to this struct is passed to OMX_SetParameter when the extension -// index for the 'OMX.google.android.index.useAndroidNativeBuffer' extension is -// given. This call will only be performed if a prior call was made with the -// 'OMX.google.android.index.enableAndroidNativeBuffers' extension index, -// enabling use of Android native buffers. -struct UseAndroidNativeBufferParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_PTR pAppPrivate; - OMX_BUFFERHEADERTYPE **bufferHeader; - const sp& nativeBuffer; -}; - -// A pointer to this struct is passed to OMX_GetParameter when the extension -// index for the 'OMX.google.android.index.getAndroidNativeBufferUsage' -// extension is given. The usage bits returned from this query will be used to -// allocate the Gralloc buffers that get passed to the useAndroidNativeBuffer -// command. -struct GetAndroidNativeBufferUsageParams { - OMX_U32 nSize; // IN - OMX_VERSIONTYPE nVersion; // IN - OMX_U32 nPortIndex; // IN - OMX_U32 nUsage; // OUT -}; - -// An enum OMX_COLOR_FormatAndroidOpaque to indicate an opaque colorformat -// is declared in media/stagefright/openmax/OMX_IVCommon.h -// This will inform the encoder that the actual -// colorformat will be relayed by the GRalloc Buffers. -// OMX_COLOR_FormatAndroidOpaque = 0x7F000001, - -// A pointer to this struct is passed to OMX_SetParameter when the extension -// index for the 'OMX.google.android.index.prependSPSPPSToIDRFrames' extension -// is given. -// A successful result indicates that future IDR frames will be prefixed by -// SPS/PPS. -struct PrependSPSPPSToIDRFramesParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_BOOL bEnable; -}; - -// Structure describing a media image (frame) -// Currently only supporting YUV -struct MediaImage { - enum Type { - MEDIA_IMAGE_TYPE_UNKNOWN = 0, - MEDIA_IMAGE_TYPE_YUV, - }; - - enum PlaneIndex { - Y = 0, - U, - V, - MAX_NUM_PLANES - }; - - Type mType; - uint32_t mNumPlanes; // number of planes - uint32_t mWidth; // width of largest plane (unpadded, as in nFrameWidth) - uint32_t mHeight; // height of largest plane (unpadded, as in nFrameHeight) - uint32_t mBitDepth; // useable bit depth - struct PlaneInfo { - uint32_t mOffset; // offset of first pixel of the plane in bytes - // from buffer offset - uint32_t mColInc; // column increment in bytes - uint32_t mRowInc; // row increment in bytes - uint32_t mHorizSubsampling; // subsampling compared to the largest plane - uint32_t mVertSubsampling; // subsampling compared to the largest plane - }; - PlaneInfo mPlane[MAX_NUM_PLANES]; -}; - -// A pointer to this struct is passed to OMX_GetParameter when the extension -// index for the 'OMX.google.android.index.describeColorFormat' -// extension is given. This method can be called from any component state -// other than invalid. The color-format, frame width/height, and stride/ -// slice-height parameters are ones that are associated with a raw video -// port (input or output), but the stride/slice height parameters may be -// incorrect. bUsingNativeBuffers is OMX_TRUE if native android buffers will -// be used (while specifying this color format). -// -// The component shall fill out the MediaImage structure that -// corresponds to the described raw video format, and the potentially corrected -// stride and slice-height info. -// -// The behavior is slightly different if bUsingNativeBuffers is OMX_TRUE, -// though most implementations can ignore this difference. When using native buffers, -// the component may change the configured color format to an optimized format. -// Additionally, when allocating these buffers for flexible usecase, the framework -// will set the SW_READ/WRITE_OFTEN usage flags. In this case (if bUsingNativeBuffers -// is OMX_TRUE), the component shall fill out the MediaImage information for the -// scenario when these SW-readable/writable buffers are locked using gralloc_lock. -// Note, that these buffers may also be locked using gralloc_lock_ycbcr, which must -// be supported for vendor-specific formats. -// -// For non-YUV packed planar/semiplanar image formats, or if bUsingNativeBuffers -// is OMX_TRUE and the component does not support this color format with native -// buffers, the component shall set mNumPlanes to 0, and mType to MEDIA_IMAGE_TYPE_UNKNOWN. -struct DescribeColorFormatParams { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - // input: parameters from OMX_VIDEO_PORTDEFINITIONTYPE - OMX_COLOR_FORMATTYPE eColorFormat; - OMX_U32 nFrameWidth; - OMX_U32 nFrameHeight; - OMX_U32 nStride; - OMX_U32 nSliceHeight; - OMX_BOOL bUsingNativeBuffers; - - // output: fill out the MediaImage fields - MediaImage sMediaImage; -}; - -// A pointer to this struct is passed to OMX_SetParameter or OMX_GetParameter -// when the extension index for the -// 'OMX.google.android.index.configureVideoTunnelMode' extension is given. -// If the extension is supported then tunneled playback mode should be supported -// by the codec. If bTunneled is set to OMX_TRUE then the video decoder should -// operate in "tunneled" mode and output its decoded frames directly to the -// sink. In this case nAudioHwSync is the HW SYNC ID of the audio HAL Output -// stream to sync the video with. If bTunneled is set to OMX_FALSE, "tunneled" -// mode should be disabled and nAudioHwSync should be ignored. -// OMX_GetParameter is used to query tunneling configuration. bTunneled should -// return whether decoder is operating in tunneled mode, and if it is, -// pSidebandWindow should contain the codec allocated sideband window handle. -struct ConfigureVideoTunnelModeParams { - OMX_U32 nSize; // IN - OMX_VERSIONTYPE nVersion; // IN - OMX_U32 nPortIndex; // IN - OMX_BOOL bTunneled; // IN/OUT - OMX_U32 nAudioHwSync; // IN - OMX_PTR pSidebandWindow; // OUT -}; - -} // namespace android - -extern android::OMXPluginBase *createOMXPlugin(); - -#endif // HARDWARE_API_H_ diff --git a/phonelibs/android_frameworks_native/include/media/hardware/MetadataBufferType.h b/phonelibs/android_frameworks_native/include/media/hardware/MetadataBufferType.h deleted file mode 100644 index b76520395..000000000 --- a/phonelibs/android_frameworks_native/include/media/hardware/MetadataBufferType.h +++ /dev/null @@ -1,127 +0,0 @@ -/* - * Copyright (C) 2011 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef METADATA_BUFFER_TYPE_H -#define METADATA_BUFFER_TYPE_H - -#ifdef __cplusplus -extern "C" { -namespace android { -#endif - -/* - * MetadataBufferType defines the type of the metadata buffers that - * can be passed to video encoder component for encoding, via Stagefright - * media recording framework. To see how to work with the metadata buffers - * in media recording framework, please consult HardwareAPI.h - * - * The creator of metadata buffers and video encoder share common knowledge - * on what is actually being stored in these metadata buffers, and - * how the information can be used by the video encoder component - * to locate the actual pixel data as the source input for video - * encoder, plus whatever other information that is necessary. Stagefright - * media recording framework does not need to know anything specific about the - * metadata buffers, except for receving each individual metadata buffer - * as the source input, making a copy of the metadata buffer, and passing the - * copy via OpenMAX API to the video encoder component. - * - * The creator of the metadata buffers must ensure that the first - * 4 bytes in every metadata buffer indicates its buffer type, - * and the rest of the metadata buffer contains the - * actual metadata information. When a video encoder component receives - * a metadata buffer, it uses the first 4 bytes in that buffer to find - * out the type of the metadata buffer, and takes action appropriate - * to that type of metadata buffers (for instance, locate the actual - * pixel data input and then encoding the input data to produce a - * compressed output buffer). - * - * The following shows the layout of a metadata buffer, - * where buffer type is a 4-byte field of MetadataBufferType, - * and the payload is the metadata information. - * - * -------------------------------------------------------------- - * | buffer type | payload | - * -------------------------------------------------------------- - * - */ -typedef enum { - - /* - * kMetadataBufferTypeCameraSource is used to indicate that - * the source of the metadata buffer is the camera component. - */ - kMetadataBufferTypeCameraSource = 0, - - /* - * kMetadataBufferTypeGrallocSource is used to indicate that - * the payload of the metadata buffers can be interpreted as - * a buffer_handle_t. - * So in this case,the metadata that the encoder receives - * will have a byte stream that consists of two parts: - * 1. First, there is an integer indicating that it is a GRAlloc - * source (kMetadataBufferTypeGrallocSource) - * 2. This is followed by the buffer_handle_t that is a handle to the - * GRalloc buffer. The encoder needs to interpret this GRalloc handle - * and encode the frames. - * -------------------------------------------------------------- - * | kMetadataBufferTypeGrallocSource | buffer_handle_t buffer | - * -------------------------------------------------------------- - * - * See the VideoGrallocMetadata structure. - */ - kMetadataBufferTypeGrallocSource = 1, - - /* - * kMetadataBufferTypeGraphicBuffer is used to indicate that - * the payload of the metadata buffers can be interpreted as - * an ANativeWindowBuffer, and that a fence is provided. - * - * In this case, the metadata will have a byte stream that consists of three parts: - * 1. First, there is an integer indicating that the metadata - * contains an ANativeWindowBuffer (kMetadataBufferTypeANWBuffer) - * 2. This is followed by the pointer to the ANativeWindowBuffer. - * Codec must not free this buffer as it does not actually own this buffer. - * 3. Finally, there is an integer containing a fence file descriptor. - * The codec must wait on the fence before encoding or decoding into this - * buffer. When the buffer is returned, codec must replace this file descriptor - * with a new fence, that will be waited on before the buffer is replaced - * (encoder) or read (decoder). - * --------------------------------- - * | kMetadataBufferTypeANWBuffer | - * --------------------------------- - * | ANativeWindowBuffer *buffer | - * --------------------------------- - * | int fenceFd | - * --------------------------------- - * - * See the VideoNativeMetadata structure. - */ - kMetadataBufferTypeANWBuffer = 2, - - /* This value is used by framework, but is never used inside a metadata buffer */ - kMetadataBufferTypeInvalid = -1, - - - // Add more here... - -} MetadataBufferType; - -#ifdef __cplusplus -} // namespace android -} -#endif - -#endif // METADATA_BUFFER_TYPE_H diff --git a/phonelibs/android_frameworks_native/include/media/hardware/OMXPluginBase.h b/phonelibs/android_frameworks_native/include/media/hardware/OMXPluginBase.h deleted file mode 100644 index 7bf414739..000000000 --- a/phonelibs/android_frameworks_native/include/media/hardware/OMXPluginBase.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2009 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef OMX_PLUGIN_BASE_H_ - -#define OMX_PLUGIN_BASE_H_ - -#include - -#include - -#include -#include - -namespace android { - -struct OMXPluginBase { - OMXPluginBase() {} - virtual ~OMXPluginBase() {} - - virtual OMX_ERRORTYPE makeComponentInstance( - const char *name, - const OMX_CALLBACKTYPE *callbacks, - OMX_PTR appData, - OMX_COMPONENTTYPE **component) = 0; - - virtual OMX_ERRORTYPE destroyComponentInstance( - OMX_COMPONENTTYPE *component) = 0; - - virtual OMX_ERRORTYPE enumerateComponents( - OMX_STRING name, - size_t size, - OMX_U32 index) = 0; - - virtual OMX_ERRORTYPE getRolesOfComponent( - const char *name, - Vector *roles) = 0; - -private: - OMXPluginBase(const OMXPluginBase &); - OMXPluginBase &operator=(const OMXPluginBase &); -}; - -} // namespace android - -#endif // OMX_PLUGIN_BASE_H_ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_AsString.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_AsString.h deleted file mode 100644 index ae8430d85..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_AsString.h +++ /dev/null @@ -1,947 +0,0 @@ -/* - * Copyright 2014 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -/* NOTE: This file contains several sections for individual OMX include files. - Each section has its own include guard. This file should be included AFTER - the OMX include files. */ - -#ifdef OMX_Audio_h -/* asString definitions if media/openmax/OMX_Audio.h was included */ - -#ifndef AS_STRING_FOR_OMX_AUDIO_H -#define AS_STRING_FOR_OMX_AUDIO_H - -inline static const char *asString(OMX_AUDIO_CODINGTYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_CodingUnused: return "Unused"; // unused - case OMX_AUDIO_CodingAutoDetect: return "AutoDetect"; // unused - case OMX_AUDIO_CodingPCM: return "PCM"; - case OMX_AUDIO_CodingADPCM: return "ADPCM"; // unused - case OMX_AUDIO_CodingAMR: return "AMR"; - case OMX_AUDIO_CodingGSMFR: return "GSMFR"; - case OMX_AUDIO_CodingGSMEFR: return "GSMEFR"; // unused - case OMX_AUDIO_CodingGSMHR: return "GSMHR"; // unused - case OMX_AUDIO_CodingPDCFR: return "PDCFR"; // unused - case OMX_AUDIO_CodingPDCEFR: return "PDCEFR"; // unused - case OMX_AUDIO_CodingPDCHR: return "PDCHR"; // unused - case OMX_AUDIO_CodingTDMAFR: return "TDMAFR"; // unused - case OMX_AUDIO_CodingTDMAEFR: return "TDMAEFR"; // unused - case OMX_AUDIO_CodingQCELP8: return "QCELP8"; // unused - case OMX_AUDIO_CodingQCELP13: return "QCELP13"; // unused - case OMX_AUDIO_CodingEVRC: return "EVRC"; // unused - case OMX_AUDIO_CodingSMV: return "SMV"; // unused - case OMX_AUDIO_CodingG711: return "G711"; - case OMX_AUDIO_CodingG723: return "G723"; // unused - case OMX_AUDIO_CodingG726: return "G726"; // unused - case OMX_AUDIO_CodingG729: return "G729"; // unused - case OMX_AUDIO_CodingAAC: return "AAC"; - case OMX_AUDIO_CodingMP3: return "MP3"; - case OMX_AUDIO_CodingSBC: return "SBC"; // unused - case OMX_AUDIO_CodingVORBIS: return "VORBIS"; - case OMX_AUDIO_CodingWMA: return "WMA"; // unused - case OMX_AUDIO_CodingRA: return "RA"; // unused - case OMX_AUDIO_CodingMIDI: return "MIDI"; // unused - case OMX_AUDIO_CodingFLAC: return "FLAC"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_PCMMODETYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_PCMModeLinear: return "Linear"; - case OMX_AUDIO_PCMModeALaw: return "ALaw"; - case OMX_AUDIO_PCMModeMULaw: return "MULaw"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_CHANNELTYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_ChannelNone: return "None"; // unused - case OMX_AUDIO_ChannelLF: return "LF"; - case OMX_AUDIO_ChannelRF: return "RF"; - case OMX_AUDIO_ChannelCF: return "CF"; - case OMX_AUDIO_ChannelLS: return "LS"; - case OMX_AUDIO_ChannelRS: return "RS"; - case OMX_AUDIO_ChannelLFE: return "LFE"; - case OMX_AUDIO_ChannelCS: return "CS"; - case OMX_AUDIO_ChannelLR: return "LR"; - case OMX_AUDIO_ChannelRR: return "RR"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_CHANNELMODETYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_ChannelModeStereo: return "Stereo"; -// case OMX_AUDIO_ChannelModeJointStereo: return "JointStereo"; -// case OMX_AUDIO_ChannelModeDual: return "Dual"; - case OMX_AUDIO_ChannelModeMono: return "Mono"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_AACSTREAMFORMATTYPE i, const char *def = "??") { - switch (i) { -// case OMX_AUDIO_AACStreamFormatMP2ADTS: return "MP2ADTS"; - case OMX_AUDIO_AACStreamFormatMP4ADTS: return "MP4ADTS"; -// case OMX_AUDIO_AACStreamFormatMP4LOAS: return "MP4LOAS"; -// case OMX_AUDIO_AACStreamFormatMP4LATM: return "MP4LATM"; -// case OMX_AUDIO_AACStreamFormatADIF: return "ADIF"; - case OMX_AUDIO_AACStreamFormatMP4FF: return "MP4FF"; -// case OMX_AUDIO_AACStreamFormatRAW: return "RAW"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_AMRFRAMEFORMATTYPE i, const char *def = "??") { - switch (i) { -// case OMX_AUDIO_AMRFrameFormatConformance: return "Conformance"; -// case OMX_AUDIO_AMRFrameFormatIF1: return "IF1"; -// case OMX_AUDIO_AMRFrameFormatIF2: return "IF2"; - case OMX_AUDIO_AMRFrameFormatFSF: return "FSF"; -// case OMX_AUDIO_AMRFrameFormatRTPPayload: return "RTPPayload"; -// case OMX_AUDIO_AMRFrameFormatITU: return "ITU"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_AMRBANDMODETYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_AMRBandModeUnused: return "Unused"; - case OMX_AUDIO_AMRBandModeNB0: return "NB0"; - case OMX_AUDIO_AMRBandModeNB1: return "NB1"; - case OMX_AUDIO_AMRBandModeNB2: return "NB2"; - case OMX_AUDIO_AMRBandModeNB3: return "NB3"; - case OMX_AUDIO_AMRBandModeNB4: return "NB4"; - case OMX_AUDIO_AMRBandModeNB5: return "NB5"; - case OMX_AUDIO_AMRBandModeNB6: return "NB6"; - case OMX_AUDIO_AMRBandModeNB7: return "NB7"; - case OMX_AUDIO_AMRBandModeWB0: return "WB0"; - case OMX_AUDIO_AMRBandModeWB1: return "WB1"; - case OMX_AUDIO_AMRBandModeWB2: return "WB2"; - case OMX_AUDIO_AMRBandModeWB3: return "WB3"; - case OMX_AUDIO_AMRBandModeWB4: return "WB4"; - case OMX_AUDIO_AMRBandModeWB5: return "WB5"; - case OMX_AUDIO_AMRBandModeWB6: return "WB6"; - case OMX_AUDIO_AMRBandModeWB7: return "WB7"; - case OMX_AUDIO_AMRBandModeWB8: return "WB8"; - default: return def; - } -} - -inline static const char *asString(OMX_AUDIO_AMRDTXMODETYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_AMRDTXModeOff: return "ModeOff"; -// case OMX_AUDIO_AMRDTXModeOnVAD1: return "ModeOnVAD1"; -// case OMX_AUDIO_AMRDTXModeOnVAD2: return "ModeOnVAD2"; -// case OMX_AUDIO_AMRDTXModeOnAuto: return "ModeOnAuto"; -// case OMX_AUDIO_AMRDTXasEFR: return "asEFR"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_AUDIO_H - -#endif // OMX_Audio_h - -#ifdef OMX_AudioExt_h -/* asString definitions if media/openmax/OMX_AudioExt.h was included */ - -#ifndef AS_STRING_FOR_OMX_AUDIOEXT_H -#define AS_STRING_FOR_OMX_AUDIOEXT_H - -inline static const char *asString(OMX_AUDIO_CODINGEXTTYPE i, const char *def = "??") { - switch (i) { - case OMX_AUDIO_CodingAndroidAC3: return "AndroidAC3"; - case OMX_AUDIO_CodingAndroidOPUS: return "AndroidOPUS"; - default: return asString((OMX_AUDIO_CODINGTYPE)i, def); - } -} - -#endif // AS_STRING_FOR_OMX_AUDIOEXT_H - -#endif // OMX_AudioExt_h - -#ifdef OMX_Component_h -/* asString definitions if media/openmax/OMX_Component.h was included */ - -#ifndef AS_STRING_FOR_OMX_COMPONENT_H -#define AS_STRING_FOR_OMX_COMPONENT_H - -inline static const char *asString(OMX_PORTDOMAINTYPE i, const char *def = "??") { - switch (i) { - case OMX_PortDomainAudio: return "Audio"; - case OMX_PortDomainVideo: return "Video"; - case OMX_PortDomainImage: return "Image"; -// case OMX_PortDomainOther: return "Other"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_COMPONENT_H - -#endif // OMX_Component_h - -#ifdef OMX_Core_h -/* asString definitions if media/openmax/OMX_Core.h was included */ - -#ifndef AS_STRING_FOR_OMX_CORE_H -#define AS_STRING_FOR_OMX_CORE_H - -inline static const char *asString(OMX_COMMANDTYPE i, const char *def = "??") { - switch (i) { - case OMX_CommandStateSet: return "StateSet"; - case OMX_CommandFlush: return "Flush"; - case OMX_CommandPortDisable: return "PortDisable"; - case OMX_CommandPortEnable: return "PortEnable"; -// case OMX_CommandMarkBuffer: return "MarkBuffer"; - default: return def; - } -} - -inline static const char *asString(OMX_STATETYPE i, const char *def = "??") { - switch (i) { - case OMX_StateInvalid: return "Invalid"; - case OMX_StateLoaded: return "Loaded"; - case OMX_StateIdle: return "Idle"; - case OMX_StateExecuting: return "Executing"; -// case OMX_StatePause: return "Pause"; -// case OMX_StateWaitForResources: return "WaitForResources"; - default: return def; - } -} - -inline static const char *asString(OMX_ERRORTYPE i, const char *def = "??") { - switch (i) { - case OMX_ErrorNone: return "None"; - case OMX_ErrorInsufficientResources: return "InsufficientResources"; - case OMX_ErrorUndefined: return "Undefined"; - case OMX_ErrorInvalidComponentName: return "InvalidComponentName"; - case OMX_ErrorComponentNotFound: return "ComponentNotFound"; - case OMX_ErrorInvalidComponent: return "InvalidComponent"; // unused - case OMX_ErrorBadParameter: return "BadParameter"; - case OMX_ErrorNotImplemented: return "NotImplemented"; - case OMX_ErrorUnderflow: return "Underflow"; // unused - case OMX_ErrorOverflow: return "Overflow"; // unused - case OMX_ErrorHardware: return "Hardware"; // unused - case OMX_ErrorInvalidState: return "InvalidState"; - case OMX_ErrorStreamCorrupt: return "StreamCorrupt"; - case OMX_ErrorPortsNotCompatible: return "PortsNotCompatible"; // unused - case OMX_ErrorResourcesLost: return "ResourcesLost"; - case OMX_ErrorNoMore: return "NoMore"; - case OMX_ErrorVersionMismatch: return "VersionMismatch"; // unused - case OMX_ErrorNotReady: return "NotReady"; // unused - case OMX_ErrorTimeout: return "Timeout"; // unused - case OMX_ErrorSameState: return "SameState"; // unused - case OMX_ErrorResourcesPreempted: return "ResourcesPreempted"; // unused - case OMX_ErrorPortUnresponsiveDuringAllocation: - return "PortUnresponsiveDuringAllocation"; // unused - case OMX_ErrorPortUnresponsiveDuringDeallocation: - return "PortUnresponsiveDuringDeallocation"; // unused - case OMX_ErrorPortUnresponsiveDuringStop: - return "PortUnresponsiveDuringStop"; // unused - case OMX_ErrorIncorrectStateTransition: - return "IncorrectStateTransition"; // unused - case OMX_ErrorIncorrectStateOperation: - return "IncorrectStateOperation"; // unused - case OMX_ErrorUnsupportedSetting: return "UnsupportedSetting"; - case OMX_ErrorUnsupportedIndex: return "UnsupportedIndex"; - case OMX_ErrorBadPortIndex: return "BadPortIndex"; - case OMX_ErrorPortUnpopulated: return "PortUnpopulated"; // unused - case OMX_ErrorComponentSuspended: return "ComponentSuspended"; // unused - case OMX_ErrorDynamicResourcesUnavailable: - return "DynamicResourcesUnavailable"; // unused - case OMX_ErrorMbErrorsInFrame: return "MbErrorsInFrame"; // unused - case OMX_ErrorFormatNotDetected: return "FormatNotDetected"; // unused - case OMX_ErrorContentPipeOpenFailed: return "ContentPipeOpenFailed"; // unused - case OMX_ErrorContentPipeCreationFailed: - return "ContentPipeCreationFailed"; // unused - case OMX_ErrorSeperateTablesUsed: return "SeperateTablesUsed"; // unused - case OMX_ErrorTunnelingUnsupported: return "TunnelingUnsupported"; // unused - default: return def; - } -} - -inline static const char *asString(OMX_EVENTTYPE i, const char *def = "??") { - switch (i) { - case OMX_EventCmdComplete: return "CmdComplete"; - case OMX_EventError: return "Error"; -// case OMX_EventMark: return "Mark"; - case OMX_EventPortSettingsChanged: return "PortSettingsChanged"; - case OMX_EventBufferFlag: return "BufferFlag"; -// case OMX_EventResourcesAcquired: return "ResourcesAcquired"; -// case OMX_EventComponentResumed: return "ComponentResumed"; -// case OMX_EventDynamicResourcesAvailable: return "DynamicResourcesAvailable"; -// case OMX_EventPortFormatDetected: return "PortFormatDetected"; - case OMX_EventOutputRendered: return "OutputRendered"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_CORE_H - -#endif // OMX_Core_h - -#ifdef OMX_Image_h -/* asString definitions if media/openmax/OMX_Image.h was included */ - -#ifndef AS_STRING_FOR_OMX_IMAGE_H -#define AS_STRING_FOR_OMX_IMAGE_H - -inline static const char *asString(OMX_IMAGE_CODINGTYPE i, const char *def = "??") { - switch (i) { - case OMX_IMAGE_CodingUnused: return "Unused"; - case OMX_IMAGE_CodingAutoDetect: return "AutoDetect"; // unused - case OMX_IMAGE_CodingJPEG: return "JPEG"; - case OMX_IMAGE_CodingJPEG2K: return "JPEG2K"; // unused - case OMX_IMAGE_CodingEXIF: return "EXIF"; // unused - case OMX_IMAGE_CodingTIFF: return "TIFF"; // unused - case OMX_IMAGE_CodingGIF: return "GIF"; // unused - case OMX_IMAGE_CodingPNG: return "PNG"; // unused - case OMX_IMAGE_CodingLZW: return "LZW"; // unused - case OMX_IMAGE_CodingBMP: return "BMP"; // unused - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_IMAGE_H - -#endif // OMX_Image_h - -#ifdef OMX_Index_h -/* asString definitions if media/openmax/OMX_Index.h was included */ - -#ifndef AS_STRING_FOR_OMX_INDEX_H -#define AS_STRING_FOR_OMX_INDEX_H - -inline static const char *asString(OMX_INDEXTYPE i, const char *def = "??") { - switch (i) { -// case OMX_IndexParamPriorityMgmt: return "ParamPriorityMgmt"; -// case OMX_IndexParamAudioInit: return "ParamAudioInit"; -// case OMX_IndexParamImageInit: return "ParamImageInit"; -// case OMX_IndexParamVideoInit: return "ParamVideoInit"; -// case OMX_IndexParamOtherInit: return "ParamOtherInit"; -// case OMX_IndexParamNumAvailableStreams: return "ParamNumAvailableStreams"; -// case OMX_IndexParamActiveStream: return "ParamActiveStream"; -// case OMX_IndexParamSuspensionPolicy: return "ParamSuspensionPolicy"; -// case OMX_IndexParamComponentSuspended: return "ParamComponentSuspended"; -// case OMX_IndexConfigCapturing: return "ConfigCapturing"; -// case OMX_IndexConfigCaptureMode: return "ConfigCaptureMode"; -// case OMX_IndexAutoPauseAfterCapture: return "AutoPauseAfterCapture"; -// case OMX_IndexParamContentURI: return "ParamContentURI"; -// case OMX_IndexParamCustomContentPipe: return "ParamCustomContentPipe"; -// case OMX_IndexParamDisableResourceConcealment: -// return "ParamDisableResourceConcealment"; -// case OMX_IndexConfigMetadataItemCount: return "ConfigMetadataItemCount"; -// case OMX_IndexConfigContainerNodeCount: return "ConfigContainerNodeCount"; -// case OMX_IndexConfigMetadataItem: return "ConfigMetadataItem"; -// case OMX_IndexConfigCounterNodeID: return "ConfigCounterNodeID"; -// case OMX_IndexParamMetadataFilterType: return "ParamMetadataFilterType"; -// case OMX_IndexParamMetadataKeyFilter: return "ParamMetadataKeyFilter"; -// case OMX_IndexConfigPriorityMgmt: return "ConfigPriorityMgmt"; - case OMX_IndexParamStandardComponentRole: return "ParamStandardComponentRole"; - case OMX_IndexParamPortDefinition: return "ParamPortDefinition"; -// case OMX_IndexParamCompBufferSupplier: return "ParamCompBufferSupplier"; - case OMX_IndexParamAudioPortFormat: return "ParamAudioPortFormat"; - case OMX_IndexParamAudioPcm: return "ParamAudioPcm"; - case OMX_IndexParamAudioAac: return "ParamAudioAac"; -// case OMX_IndexParamAudioRa: return "ParamAudioRa"; - case OMX_IndexParamAudioMp3: return "ParamAudioMp3"; -// case OMX_IndexParamAudioAdpcm: return "ParamAudioAdpcm"; -// case OMX_IndexParamAudioG723: return "ParamAudioG723"; -// case OMX_IndexParamAudioG729: return "ParamAudioG729"; - case OMX_IndexParamAudioAmr: return "ParamAudioAmr"; -// case OMX_IndexParamAudioWma: return "ParamAudioWma"; -// case OMX_IndexParamAudioSbc: return "ParamAudioSbc"; -// case OMX_IndexParamAudioMidi: return "ParamAudioMidi"; -// case OMX_IndexParamAudioGsm_FR: return "ParamAudioGsm_FR"; -// case OMX_IndexParamAudioMidiLoadUserSound: return "ParamAudioMidiLoadUserSound"; -// case OMX_IndexParamAudioG726: return "ParamAudioG726"; -// case OMX_IndexParamAudioGsm_EFR: return "ParamAudioGsm_EFR"; -// case OMX_IndexParamAudioGsm_HR: return "ParamAudioGsm_HR"; -// case OMX_IndexParamAudioPdc_FR: return "ParamAudioPdc_FR"; -// case OMX_IndexParamAudioPdc_EFR: return "ParamAudioPdc_EFR"; -// case OMX_IndexParamAudioPdc_HR: return "ParamAudioPdc_HR"; -// case OMX_IndexParamAudioTdma_FR: return "ParamAudioTdma_FR"; -// case OMX_IndexParamAudioTdma_EFR: return "ParamAudioTdma_EFR"; -// case OMX_IndexParamAudioQcelp8: return "ParamAudioQcelp8"; -// case OMX_IndexParamAudioQcelp13: return "ParamAudioQcelp13"; -// case OMX_IndexParamAudioEvrc: return "ParamAudioEvrc"; -// case OMX_IndexParamAudioSmv: return "ParamAudioSmv"; - case OMX_IndexParamAudioVorbis: return "ParamAudioVorbis"; - case OMX_IndexParamAudioFlac: return "ParamAudioFlac"; -// case OMX_IndexConfigAudioMidiImmediateEvent: return "ConfigAudioMidiImmediateEvent"; -// case OMX_IndexConfigAudioMidiControl: return "ConfigAudioMidiControl"; -// case OMX_IndexConfigAudioMidiSoundBankProgram: -// return "ConfigAudioMidiSoundBankProgram"; -// case OMX_IndexConfigAudioMidiStatus: return "ConfigAudioMidiStatus"; -// case OMX_IndexConfigAudioMidiMetaEvent: return "ConfigAudioMidiMetaEvent"; -// case OMX_IndexConfigAudioMidiMetaEventData: return "ConfigAudioMidiMetaEventData"; -// case OMX_IndexConfigAudioVolume: return "ConfigAudioVolume"; -// case OMX_IndexConfigAudioBalance: return "ConfigAudioBalance"; -// case OMX_IndexConfigAudioChannelMute: return "ConfigAudioChannelMute"; -// case OMX_IndexConfigAudioMute: return "ConfigAudioMute"; -// case OMX_IndexConfigAudioLoudness: return "ConfigAudioLoudness"; -// case OMX_IndexConfigAudioEchoCancelation: return "ConfigAudioEchoCancelation"; -// case OMX_IndexConfigAudioNoiseReduction: return "ConfigAudioNoiseReduction"; -// case OMX_IndexConfigAudioBass: return "ConfigAudioBass"; -// case OMX_IndexConfigAudioTreble: return "ConfigAudioTreble"; -// case OMX_IndexConfigAudioStereoWidening: return "ConfigAudioStereoWidening"; -// case OMX_IndexConfigAudioChorus: return "ConfigAudioChorus"; -// case OMX_IndexConfigAudioEqualizer: return "ConfigAudioEqualizer"; -// case OMX_IndexConfigAudioReverberation: return "ConfigAudioReverberation"; -// case OMX_IndexConfigAudioChannelVolume: return "ConfigAudioChannelVolume"; -// case OMX_IndexParamImagePortFormat: return "ParamImagePortFormat"; -// case OMX_IndexParamFlashControl: return "ParamFlashControl"; -// case OMX_IndexConfigFocusControl: return "ConfigFocusControl"; -// case OMX_IndexParamQFactor: return "ParamQFactor"; -// case OMX_IndexParamQuantizationTable: return "ParamQuantizationTable"; -// case OMX_IndexParamHuffmanTable: return "ParamHuffmanTable"; -// case OMX_IndexConfigFlashControl: return "ConfigFlashControl"; - case OMX_IndexParamVideoPortFormat: return "ParamVideoPortFormat"; -// case OMX_IndexParamVideoQuantization: return "ParamVideoQuantization"; -// case OMX_IndexParamVideoFastUpdate: return "ParamVideoFastUpdate"; - case OMX_IndexParamVideoBitrate: return "ParamVideoBitrate"; -// case OMX_IndexParamVideoMotionVector: return "ParamVideoMotionVector"; - case OMX_IndexParamVideoIntraRefresh: return "ParamVideoIntraRefresh"; - case OMX_IndexParamVideoErrorCorrection: return "ParamVideoErrorCorrection"; -// case OMX_IndexParamVideoVBSMC: return "ParamVideoVBSMC"; -// case OMX_IndexParamVideoMpeg2: return "ParamVideoMpeg2"; - case OMX_IndexParamVideoMpeg4: return "ParamVideoMpeg4"; -// case OMX_IndexParamVideoWmv: return "ParamVideoWmv"; -// case OMX_IndexParamVideoRv: return "ParamVideoRv"; - case OMX_IndexParamVideoAvc: return "ParamVideoAvc"; - case OMX_IndexParamVideoH263: return "ParamVideoH263"; - case OMX_IndexParamVideoProfileLevelQuerySupported: - return "ParamVideoProfileLevelQuerySupported"; - case OMX_IndexParamVideoProfileLevelCurrent: return "ParamVideoProfileLevelCurrent"; - case OMX_IndexConfigVideoBitrate: return "ConfigVideoBitrate"; -// case OMX_IndexConfigVideoFramerate: return "ConfigVideoFramerate"; - case OMX_IndexConfigVideoIntraVOPRefresh: return "ConfigVideoIntraVOPRefresh"; -// case OMX_IndexConfigVideoIntraMBRefresh: return "ConfigVideoIntraMBRefresh"; -// case OMX_IndexConfigVideoMBErrorReporting: return "ConfigVideoMBErrorReporting"; -// case OMX_IndexParamVideoMacroblocksPerFrame: return "ParamVideoMacroblocksPerFrame"; -// case OMX_IndexConfigVideoMacroBlockErrorMap: return "ConfigVideoMacroBlockErrorMap"; -// case OMX_IndexParamVideoSliceFMO: return "ParamVideoSliceFMO"; -// case OMX_IndexConfigVideoAVCIntraPeriod: return "ConfigVideoAVCIntraPeriod"; -// case OMX_IndexConfigVideoNalSize: return "ConfigVideoNalSize"; -// case OMX_IndexParamCommonDeblocking: return "ParamCommonDeblocking"; -// case OMX_IndexParamCommonSensorMode: return "ParamCommonSensorMode"; -// case OMX_IndexParamCommonInterleave: return "ParamCommonInterleave"; -// case OMX_IndexConfigCommonColorFormatConversion: -// return "ConfigCommonColorFormatConversion"; - case OMX_IndexConfigCommonScale: return "ConfigCommonScale"; -// case OMX_IndexConfigCommonImageFilter: return "ConfigCommonImageFilter"; -// case OMX_IndexConfigCommonColorEnhancement: return "ConfigCommonColorEnhancement"; -// case OMX_IndexConfigCommonColorKey: return "ConfigCommonColorKey"; -// case OMX_IndexConfigCommonColorBlend: return "ConfigCommonColorBlend"; -// case OMX_IndexConfigCommonFrameStabilisation: return "ConfigCommonFrameStabilisation"; -// case OMX_IndexConfigCommonRotate: return "ConfigCommonRotate"; -// case OMX_IndexConfigCommonMirror: return "ConfigCommonMirror"; -// case OMX_IndexConfigCommonOutputPosition: return "ConfigCommonOutputPosition"; - case OMX_IndexConfigCommonInputCrop: return "ConfigCommonInputCrop"; - case OMX_IndexConfigCommonOutputCrop: return "ConfigCommonOutputCrop"; -// case OMX_IndexConfigCommonDigitalZoom: return "ConfigCommonDigitalZoom"; -// case OMX_IndexConfigCommonOpticalZoom: return "ConfigCommonOpticalZoom"; -// case OMX_IndexConfigCommonWhiteBalance: return "ConfigCommonWhiteBalance"; -// case OMX_IndexConfigCommonExposure: return "ConfigCommonExposure"; -// case OMX_IndexConfigCommonContrast: return "ConfigCommonContrast"; -// case OMX_IndexConfigCommonBrightness: return "ConfigCommonBrightness"; -// case OMX_IndexConfigCommonBacklight: return "ConfigCommonBacklight"; -// case OMX_IndexConfigCommonGamma: return "ConfigCommonGamma"; -// case OMX_IndexConfigCommonSaturation: return "ConfigCommonSaturation"; -// case OMX_IndexConfigCommonLightness: return "ConfigCommonLightness"; -// case OMX_IndexConfigCommonExclusionRect: return "ConfigCommonExclusionRect"; -// case OMX_IndexConfigCommonDithering: return "ConfigCommonDithering"; -// case OMX_IndexConfigCommonPlaneBlend: return "ConfigCommonPlaneBlend"; -// case OMX_IndexConfigCommonExposureValue: return "ConfigCommonExposureValue"; -// case OMX_IndexConfigCommonOutputSize: return "ConfigCommonOutputSize"; -// case OMX_IndexParamCommonExtraQuantData: return "ParamCommonExtraQuantData"; -// case OMX_IndexConfigCommonFocusRegion: return "ConfigCommonFocusRegion"; -// case OMX_IndexConfigCommonFocusStatus: return "ConfigCommonFocusStatus"; -// case OMX_IndexConfigCommonTransitionEffect: return "ConfigCommonTransitionEffect"; -// case OMX_IndexParamOtherPortFormat: return "ParamOtherPortFormat"; -// case OMX_IndexConfigOtherPower: return "ConfigOtherPower"; -// case OMX_IndexConfigOtherStats: return "ConfigOtherStats"; -// case OMX_IndexConfigTimeScale: return "ConfigTimeScale"; -// case OMX_IndexConfigTimeClockState: return "ConfigTimeClockState"; -// case OMX_IndexConfigTimeActiveRefClock: return "ConfigTimeActiveRefClock"; -// case OMX_IndexConfigTimeCurrentMediaTime: return "ConfigTimeCurrentMediaTime"; -// case OMX_IndexConfigTimeCurrentWallTime: return "ConfigTimeCurrentWallTime"; -// case OMX_IndexConfigTimeCurrentAudioReference: -// return "ConfigTimeCurrentAudioReference"; -// case OMX_IndexConfigTimeCurrentVideoReference: -// return "ConfigTimeCurrentVideoReference"; -// case OMX_IndexConfigTimeMediaTimeRequest: return "ConfigTimeMediaTimeRequest"; -// case OMX_IndexConfigTimeClientStartTime: return "ConfigTimeClientStartTime"; -// case OMX_IndexConfigTimePosition: return "ConfigTimePosition"; -// case OMX_IndexConfigTimeSeekMode: return "ConfigTimeSeekMode"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_INDEX_H - -#endif // OMX_Index_h - -#ifdef OMX_IndexExt_h -/* asString definitions if media/openmax/OMX_IndexExt.h was included */ - -#ifndef AS_STRING_FOR_OMX_INDEXEXT_H -#define AS_STRING_FOR_OMX_INDEXEXT_H - -inline static const char *asString(OMX_INDEXEXTTYPE i, const char *def = "??") { - switch (i) { -// case OMX_IndexConfigCallbackRequest: return "ConfigCallbackRequest"; -// case OMX_IndexConfigCommitMode: return "ConfigCommitMode"; -// case OMX_IndexConfigCommit: return "ConfigCommit"; - case OMX_IndexParamAudioAndroidAc3: return "ParamAudioAndroidAc3"; - case OMX_IndexParamAudioAndroidOpus: return "ParamAudioAndroidOpus"; - case OMX_IndexParamAudioAndroidAacPresentation: return "ParamAudioAndroidAacPresentation"; -// case OMX_IndexParamNalStreamFormatSupported: return "ParamNalStreamFormatSupported"; -// case OMX_IndexParamNalStreamFormat: return "ParamNalStreamFormat"; -// case OMX_IndexParamNalStreamFormatSelect: return "ParamNalStreamFormatSelect"; - case OMX_IndexParamVideoVp8: return "ParamVideoVp8"; -// case OMX_IndexConfigVideoVp8ReferenceFrame: return "ConfigVideoVp8ReferenceFrame"; -// case OMX_IndexConfigVideoVp8ReferenceFrameType: return "ConfigVideoVp8ReferenceFrameType"; - case OMX_IndexParamVideoAndroidVp8Encoder: return "ParamVideoAndroidVp8Encoder"; - case OMX_IndexParamVideoHevc: return "ParamVideoHevc"; -// case OMX_IndexParamSliceSegments: return "ParamSliceSegments"; - case OMX_IndexConfigAutoFramerateConversion: return "ConfigAutoFramerateConversion"; - case OMX_IndexConfigPriority: return "ConfigPriority"; - case OMX_IndexConfigOperatingRate: return "ConfigOperatingRate"; - case OMX_IndexParamConsumerUsageBits: return "ParamConsumerUsageBits"; - default: return asString((OMX_INDEXTYPE)i, def); - } -} - -#endif // AS_STRING_FOR_OMX_INDEXEXT_H - -#endif // OMX_IndexExt_h - -#ifdef OMX_IVCommon_h -/* asString definitions if media/openmax/OMX_IVCommon.h was included */ - -#ifndef AS_STRING_FOR_OMX_IVCOMMON_H -#define AS_STRING_FOR_OMX_IVCOMMON_H - -inline static const char *asString(OMX_COLOR_FORMATTYPE i, const char *def = "??") { - switch (i) { - case OMX_COLOR_FormatUnused: - return "COLOR_FormatUnused"; - case OMX_COLOR_FormatMonochrome: - return "COLOR_FormatMonochrome"; - case OMX_COLOR_Format8bitRGB332: - return "COLOR_Format8bitRGB332"; - case OMX_COLOR_Format12bitRGB444: - return "COLOR_Format12bitRGB444"; - case OMX_COLOR_Format16bitARGB4444: - return "COLOR_Format16bitARGB4444"; - case OMX_COLOR_Format16bitARGB1555: - return "COLOR_Format16bitARGB1555"; - case OMX_COLOR_Format16bitRGB565: - return "COLOR_Format16bitRGB565"; - case OMX_COLOR_Format16bitBGR565: - return "COLOR_Format16bitBGR565"; - case OMX_COLOR_Format18bitRGB666: - return "COLOR_Format18bitRGB666"; - case OMX_COLOR_Format18bitARGB1665: - return "COLOR_Format18bitARGB1665"; - case OMX_COLOR_Format19bitARGB1666: - return "COLOR_Format19bitARGB1666"; - case OMX_COLOR_Format24bitRGB888: - return "COLOR_Format24bitRGB888"; - case OMX_COLOR_Format24bitBGR888: - return "COLOR_Format24bitBGR888"; - case OMX_COLOR_Format24bitARGB1887: - return "COLOR_Format24bitARGB1887"; - case OMX_COLOR_Format25bitARGB1888: - return "COLOR_Format25bitARGB1888"; - case OMX_COLOR_Format32bitBGRA8888: - return "COLOR_Format32bitBGRA8888"; - case OMX_COLOR_Format32bitARGB8888: - return "COLOR_Format32bitARGB8888"; - case OMX_COLOR_FormatYUV411Planar: - return "COLOR_FormatYUV411Planar"; - case OMX_COLOR_FormatYUV411PackedPlanar: - return "COLOR_FormatYUV411PackedPlanar"; - case OMX_COLOR_FormatYUV420Planar: - return "COLOR_FormatYUV420Planar"; - case OMX_COLOR_FormatYUV420PackedPlanar: - return "COLOR_FormatYUV420PackedPlanar"; - case OMX_COLOR_FormatYUV420SemiPlanar: - return "COLOR_FormatYUV420SemiPlanar"; - case OMX_COLOR_FormatYUV422Planar: - return "COLOR_FormatYUV422Planar"; - case OMX_COLOR_FormatYUV422PackedPlanar: - return "COLOR_FormatYUV422PackedPlanar"; - case OMX_COLOR_FormatYUV422SemiPlanar: - return "COLOR_FormatYUV422SemiPlanar"; - case OMX_COLOR_FormatYCbYCr: - return "COLOR_FormatYCbYCr"; - case OMX_COLOR_FormatYCrYCb: - return "COLOR_FormatYCrYCb"; - case OMX_COLOR_FormatCbYCrY: - return "COLOR_FormatCbYCrY"; - case OMX_COLOR_FormatCrYCbY: - return "COLOR_FormatCrYCbY"; - case OMX_COLOR_FormatYUV444Interleaved: - return "COLOR_FormatYUV444Interleaved"; - case OMX_COLOR_FormatRawBayer8bit: - return "COLOR_FormatRawBayer8bit"; - case OMX_COLOR_FormatRawBayer10bit: - return "COLOR_FormatRawBayer10bit"; - case OMX_COLOR_FormatRawBayer8bitcompressed: - return "COLOR_FormatRawBayer8bitcompressed"; - case OMX_COLOR_FormatL2: - return "COLOR_FormatL2"; - case OMX_COLOR_FormatL4: - return "COLOR_FormatL4"; - case OMX_COLOR_FormatL8: - return "COLOR_FormatL8"; - case OMX_COLOR_FormatL16: - return "COLOR_FormatL16"; - case OMX_COLOR_FormatL24: - return "COLOR_FormatL24"; - case OMX_COLOR_FormatL32: - return "COLOR_FormatL32"; - case OMX_COLOR_FormatYUV420PackedSemiPlanar: - return "COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_COLOR_FormatYUV422PackedSemiPlanar: - return "COLOR_FormatYUV422PackedSemiPlanar"; - case OMX_COLOR_Format18BitBGR666: - return "COLOR_Format18BitBGR666"; - case OMX_COLOR_Format24BitARGB6666: - return "COLOR_Format24BitARGB6666"; - case OMX_COLOR_Format24BitABGR6666: - return "COLOR_Format24BitABGR6666"; - case OMX_COLOR_FormatAndroidOpaque: - return "COLOR_FormatAndroidOpaque"; - case OMX_COLOR_FormatYUV420Flexible: - return "COLOR_FormatYUV420Flexible"; - case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: - return "TI_COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: - return "QCOM_COLOR_FormatYVU420SemiPlanar"; -// case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: -// return "QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; -// case OMX_SEC_COLOR_FormatNV12Tiled: -// return "SEC_COLOR_FormatNV12Tiled"; -// case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: -// return "QCOM_COLOR_FormatYUV420PackedSemiPlanar32m"; - default: - return def; - } -} - -#endif // AS_STRING_FOR_OMX_IVCOMMON_H - -#endif // OMX_IVCommon_h - -#ifdef OMX_Types_h -/* asString definitions if media/openmax/OMX_Types.h was included */ - -#ifndef AS_STRING_FOR_OMX_TYPES_H -#define AS_STRING_FOR_OMX_TYPES_H - -inline static const char *asString(OMX_BOOL i, const char *def = "??") { - switch (i) { - case OMX_FALSE: return "FALSE"; - case OMX_TRUE: return "TRUE"; - default: return def; - } -} - -inline static const char *asString(OMX_DIRTYPE i, const char *def = "??") { - switch (i) { - case OMX_DirInput: return "Input"; - case OMX_DirOutput: return "Output"; - default: return def; - } -} - -inline static const char *asString(OMX_ENDIANTYPE i, const char *def = "??") { - switch (i) { - case OMX_EndianBig: return "Big"; -// case OMX_EndianLittle: return "Little"; - default: return def; - } -} - -inline static const char *asString(OMX_NUMERICALDATATYPE i, const char *def = "??") { - switch (i) { - case OMX_NumericalDataSigned: return "Signed"; -// case OMX_NumericalDataUnsigned: return "Unsigned"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_TYPES_H - -#endif // OMX_Types_h - -#ifdef OMX_Video_h -/* asString definitions if media/openmax/OMX_Video.h was included */ - -#ifndef AS_STRING_FOR_OMX_VIDEO_H -#define AS_STRING_FOR_OMX_VIDEO_H - -inline static const char *asString(OMX_VIDEO_CODINGTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_CodingUnused: return "Unused"; - case OMX_VIDEO_CodingAutoDetect: return "AutoDetect"; // unused - case OMX_VIDEO_CodingMPEG2: return "MPEG2"; - case OMX_VIDEO_CodingH263: return "H263"; - case OMX_VIDEO_CodingMPEG4: return "MPEG4"; - case OMX_VIDEO_CodingWMV: return "WMV"; // unused - case OMX_VIDEO_CodingRV: return "RV"; // unused - case OMX_VIDEO_CodingAVC: return "AVC"; - case OMX_VIDEO_CodingMJPEG: return "MJPEG"; // unused - case OMX_VIDEO_CodingVP8: return "VP8"; - case OMX_VIDEO_CodingVP9: return "VP9"; - case OMX_VIDEO_CodingHEVC: return "HEVC"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_CONTROLRATETYPE i, const char *def = "??") { - switch (i) { -// case OMX_Video_ControlRateDisable: return "Disable"; - case OMX_Video_ControlRateVariable: return "Variable"; - case OMX_Video_ControlRateConstant: return "Constant"; -// case OMX_Video_ControlRateVariableSkipFrames: return "VariableSkipFrames"; -// case OMX_Video_ControlRateConstantSkipFrames: return "ConstantSkipFrames"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_INTRAREFRESHTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_IntraRefreshCyclic: return "Cyclic"; - case OMX_VIDEO_IntraRefreshAdaptive: return "Adaptive"; - case OMX_VIDEO_IntraRefreshBoth: return "Both"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_H263PROFILETYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_H263ProfileBaseline: return "Baseline"; - case OMX_VIDEO_H263ProfileH320Coding: return "H320Coding"; - case OMX_VIDEO_H263ProfileBackwardCompatible: return "BackwardCompatible"; - case OMX_VIDEO_H263ProfileISWV2: return "ISWV2"; - case OMX_VIDEO_H263ProfileISWV3: return "ISWV3"; - case OMX_VIDEO_H263ProfileHighCompression: return "HighCompression"; - case OMX_VIDEO_H263ProfileInternet: return "Internet"; - case OMX_VIDEO_H263ProfileInterlace: return "Interlace"; - case OMX_VIDEO_H263ProfileHighLatency: return "HighLatency"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_H263LEVELTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_H263Level10: return "Level10"; - case OMX_VIDEO_H263Level20: return "Level20"; - case OMX_VIDEO_H263Level30: return "Level30"; - case OMX_VIDEO_H263Level40: return "Level40"; - case OMX_VIDEO_H263Level45: return "Level45"; - case OMX_VIDEO_H263Level50: return "Level50"; - case OMX_VIDEO_H263Level60: return "Level60"; - case OMX_VIDEO_H263Level70: return "Level70"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_PICTURETYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_PictureTypeI: return "I"; - case OMX_VIDEO_PictureTypeP: return "P"; - case OMX_VIDEO_PictureTypeB: return "B"; -// case OMX_VIDEO_PictureTypeSI: return "SI"; -// case OMX_VIDEO_PictureTypeSP: return "SP"; -// case OMX_VIDEO_PictureTypeEI: return "EI"; -// case OMX_VIDEO_PictureTypeEP: return "EP"; -// case OMX_VIDEO_PictureTypeS: return "S"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_MPEG4PROFILETYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_MPEG4ProfileSimple: return "Simple"; - case OMX_VIDEO_MPEG4ProfileSimpleScalable: return "SimpleScalable"; - case OMX_VIDEO_MPEG4ProfileCore: return "Core"; - case OMX_VIDEO_MPEG4ProfileMain: return "Main"; - case OMX_VIDEO_MPEG4ProfileNbit: return "Nbit"; - case OMX_VIDEO_MPEG4ProfileScalableTexture: return "ScalableTexture"; - case OMX_VIDEO_MPEG4ProfileSimpleFace: return "SimpleFace"; - case OMX_VIDEO_MPEG4ProfileSimpleFBA: return "SimpleFBA"; - case OMX_VIDEO_MPEG4ProfileBasicAnimated: return "BasicAnimated"; - case OMX_VIDEO_MPEG4ProfileHybrid: return "Hybrid"; - case OMX_VIDEO_MPEG4ProfileAdvancedRealTime: return "AdvancedRealTime"; - case OMX_VIDEO_MPEG4ProfileCoreScalable: return "CoreScalable"; - case OMX_VIDEO_MPEG4ProfileAdvancedCoding: return "AdvancedCoding"; - case OMX_VIDEO_MPEG4ProfileAdvancedCore: return "AdvancedCore"; - case OMX_VIDEO_MPEG4ProfileAdvancedScalable: return "AdvancedScalable"; - case OMX_VIDEO_MPEG4ProfileAdvancedSimple: return "AdvancedSimple"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_MPEG4LEVELTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_MPEG4Level0: return "Level0"; - case OMX_VIDEO_MPEG4Level0b: return "Level0b"; - case OMX_VIDEO_MPEG4Level1: return "Level1"; - case OMX_VIDEO_MPEG4Level2: return "Level2"; - case OMX_VIDEO_MPEG4Level3: return "Level3"; - case OMX_VIDEO_MPEG4Level4: return "Level4"; - case OMX_VIDEO_MPEG4Level4a: return "Level4a"; - case OMX_VIDEO_MPEG4Level5: return "Level5"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_AVCPROFILETYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_AVCProfileBaseline: return "Baseline"; - case OMX_VIDEO_AVCProfileMain: return "Main"; - case OMX_VIDEO_AVCProfileExtended: return "Extended"; - case OMX_VIDEO_AVCProfileHigh: return "High"; - case OMX_VIDEO_AVCProfileHigh10: return "High10"; - case OMX_VIDEO_AVCProfileHigh422: return "High422"; - case OMX_VIDEO_AVCProfileHigh444: return "High444"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_AVCLEVELTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_AVCLevel1: return "Level1"; - case OMX_VIDEO_AVCLevel1b: return "Level1b"; - case OMX_VIDEO_AVCLevel11: return "Level11"; - case OMX_VIDEO_AVCLevel12: return "Level12"; - case OMX_VIDEO_AVCLevel13: return "Level13"; - case OMX_VIDEO_AVCLevel2: return "Level2"; - case OMX_VIDEO_AVCLevel21: return "Level21"; - case OMX_VIDEO_AVCLevel22: return "Level22"; - case OMX_VIDEO_AVCLevel3: return "Level3"; - case OMX_VIDEO_AVCLevel31: return "Level31"; - case OMX_VIDEO_AVCLevel32: return "Level32"; - case OMX_VIDEO_AVCLevel4: return "Level4"; - case OMX_VIDEO_AVCLevel41: return "Level41"; - case OMX_VIDEO_AVCLevel42: return "Level42"; - case OMX_VIDEO_AVCLevel5: return "Level5"; - case OMX_VIDEO_AVCLevel51: return "Level51"; - case OMX_VIDEO_AVCLevel52: return "Level52"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_AVCLOOPFILTERTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_AVCLoopFilterEnable: return "Enable"; -// case OMX_VIDEO_AVCLoopFilterDisable: return "Disable"; -// case OMX_VIDEO_AVCLoopFilterDisableSliceBoundary: return "DisableSliceBoundary"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_VIDEO_H - -#endif // OMX_Video_h - -#ifdef OMX_VideoExt_h -/* asString definitions if media/openmax/OMX_VideoExt.h was included */ - -#ifndef AS_STRING_FOR_OMX_VIDEOEXT_H -#define AS_STRING_FOR_OMX_VIDEOEXT_H - -inline static const char *asString(OMX_VIDEO_VP8PROFILETYPE i, const char *def = "!!") { - switch (i) { - case OMX_VIDEO_VP8ProfileMain: return "Main"; - case OMX_VIDEO_VP8ProfileUnknown: return "Unknown"; // unused - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_VP8LEVELTYPE i, const char *def = "!!") { - switch (i) { - case OMX_VIDEO_VP8Level_Version0: return "_Version0"; - case OMX_VIDEO_VP8Level_Version1: return "_Version1"; - case OMX_VIDEO_VP8Level_Version2: return "_Version2"; - case OMX_VIDEO_VP8Level_Version3: return "_Version3"; - case OMX_VIDEO_VP8LevelUnknown: return "Unknown"; // unused - default: return def; - } -} - -inline static const char *asString( - OMX_VIDEO_ANDROID_VPXTEMPORALLAYERPATTERNTYPE i, const char *def = "??") { - switch (i) { - case OMX_VIDEO_VPXTemporalLayerPatternNone: return "VPXTemporalLayerPatternNone"; - case OMX_VIDEO_VPXTemporalLayerPatternWebRTC: return "VPXTemporalLayerPatternWebRTC"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_HEVCPROFILETYPE i, const char *def = "!!") { - switch (i) { - case OMX_VIDEO_HEVCProfileUnknown: return "Unknown"; // unused - case OMX_VIDEO_HEVCProfileMain: return "Main"; - case OMX_VIDEO_HEVCProfileMain10: return "Main10"; - default: return def; - } -} - -inline static const char *asString(OMX_VIDEO_HEVCLEVELTYPE i, const char *def = "!!") { - switch (i) { - case OMX_VIDEO_HEVCLevelUnknown: return "LevelUnknown"; // unused - case OMX_VIDEO_HEVCMainTierLevel1: return "MainTierLevel1"; - case OMX_VIDEO_HEVCHighTierLevel1: return "HighTierLevel1"; - case OMX_VIDEO_HEVCMainTierLevel2: return "MainTierLevel2"; - case OMX_VIDEO_HEVCHighTierLevel2: return "HighTierLevel2"; - case OMX_VIDEO_HEVCMainTierLevel21: return "MainTierLevel21"; - case OMX_VIDEO_HEVCHighTierLevel21: return "HighTierLevel21"; - case OMX_VIDEO_HEVCMainTierLevel3: return "MainTierLevel3"; - case OMX_VIDEO_HEVCHighTierLevel3: return "HighTierLevel3"; - case OMX_VIDEO_HEVCMainTierLevel31: return "MainTierLevel31"; - case OMX_VIDEO_HEVCHighTierLevel31: return "HighTierLevel31"; - case OMX_VIDEO_HEVCMainTierLevel4: return "MainTierLevel4"; - case OMX_VIDEO_HEVCHighTierLevel4: return "HighTierLevel4"; - case OMX_VIDEO_HEVCMainTierLevel41: return "MainTierLevel41"; - case OMX_VIDEO_HEVCHighTierLevel41: return "HighTierLevel41"; - case OMX_VIDEO_HEVCMainTierLevel5: return "MainTierLevel5"; - case OMX_VIDEO_HEVCHighTierLevel5: return "HighTierLevel5"; - case OMX_VIDEO_HEVCMainTierLevel51: return "MainTierLevel51"; - case OMX_VIDEO_HEVCHighTierLevel51: return "HighTierLevel51"; - case OMX_VIDEO_HEVCMainTierLevel52: return "MainTierLevel52"; - case OMX_VIDEO_HEVCHighTierLevel52: return "HighTierLevel52"; - case OMX_VIDEO_HEVCMainTierLevel6: return "MainTierLevel6"; - case OMX_VIDEO_HEVCHighTierLevel6: return "HighTierLevel6"; - case OMX_VIDEO_HEVCMainTierLevel61: return "MainTierLevel61"; - case OMX_VIDEO_HEVCHighTierLevel61: return "HighTierLevel61"; - case OMX_VIDEO_HEVCMainTierLevel62: return "MainTierLevel62"; - case OMX_VIDEO_HEVCHighTierLevel62: return "HighTierLevel62"; - default: return def; - } -} - -#endif // AS_STRING_FOR_OMX_VIDEOEXT_H - -#endif // OMX_VideoExt_h diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Audio.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Audio.h deleted file mode 100644 index a0cbd3bb9..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Audio.h +++ /dev/null @@ -1,1342 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** @file OMX_Audio.h - OpenMax IL version 1.1.2 - * The structures needed by Audio components to exchange - * parameters and configuration data with the componenmilts. - */ - -#ifndef OMX_Audio_h -#define OMX_Audio_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/* Each OMX header must include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ - -#include - -/** @defgroup midi MIDI - * @ingroup audio - */ - -/** @defgroup effects Audio effects - * @ingroup audio - */ - -/** @defgroup audio OpenMAX IL Audio Domain - * Structures for OpenMAX IL Audio domain - * @{ - */ - -/** Enumeration used to define the possible audio codings. - * If "OMX_AUDIO_CodingUnused" is selected, the coding selection must - * be done in a vendor specific way. Since this is for an audio - * processing element this enum is relevant. However, for another - * type of component other enums would be in this area. - */ -typedef enum OMX_AUDIO_CODINGTYPE { - OMX_AUDIO_CodingUnused = 0, /**< Placeholder value when coding is N/A */ - OMX_AUDIO_CodingAutoDetect, /**< auto detection of audio format */ - OMX_AUDIO_CodingPCM, /**< Any variant of PCM coding */ - OMX_AUDIO_CodingADPCM, /**< Any variant of ADPCM encoded data */ - OMX_AUDIO_CodingAMR, /**< Any variant of AMR encoded data */ - OMX_AUDIO_CodingGSMFR, /**< Any variant of GSM fullrate (i.e. GSM610) */ - OMX_AUDIO_CodingGSMEFR, /**< Any variant of GSM Enhanced Fullrate encoded data*/ - OMX_AUDIO_CodingGSMHR, /**< Any variant of GSM Halfrate encoded data */ - OMX_AUDIO_CodingPDCFR, /**< Any variant of PDC Fullrate encoded data */ - OMX_AUDIO_CodingPDCEFR, /**< Any variant of PDC Enhanced Fullrate encoded data */ - OMX_AUDIO_CodingPDCHR, /**< Any variant of PDC Halfrate encoded data */ - OMX_AUDIO_CodingTDMAFR, /**< Any variant of TDMA Fullrate encoded data (TIA/EIA-136-420) */ - OMX_AUDIO_CodingTDMAEFR, /**< Any variant of TDMA Enhanced Fullrate encoded data (TIA/EIA-136-410) */ - OMX_AUDIO_CodingQCELP8, /**< Any variant of QCELP 8kbps encoded data */ - OMX_AUDIO_CodingQCELP13, /**< Any variant of QCELP 13kbps encoded data */ - OMX_AUDIO_CodingEVRC, /**< Any variant of EVRC encoded data */ - OMX_AUDIO_CodingSMV, /**< Any variant of SMV encoded data */ - OMX_AUDIO_CodingG711, /**< Any variant of G.711 encoded data */ - OMX_AUDIO_CodingG723, /**< Any variant of G.723 dot 1 encoded data */ - OMX_AUDIO_CodingG726, /**< Any variant of G.726 encoded data */ - OMX_AUDIO_CodingG729, /**< Any variant of G.729 encoded data */ - OMX_AUDIO_CodingAAC, /**< Any variant of AAC encoded data */ - OMX_AUDIO_CodingMP3, /**< Any variant of MP3 encoded data */ - OMX_AUDIO_CodingSBC, /**< Any variant of SBC encoded data */ - OMX_AUDIO_CodingVORBIS, /**< Any variant of VORBIS encoded data */ - OMX_AUDIO_CodingWMA, /**< Any variant of WMA encoded data */ - OMX_AUDIO_CodingRA, /**< Any variant of RA encoded data */ - OMX_AUDIO_CodingMIDI, /**< Any variant of MIDI encoded data */ - OMX_AUDIO_CodingFLAC, /**< Any variant of FLAC encoded data */ - OMX_AUDIO_CodingKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_CodingVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_CodingMax = 0x7FFFFFFF -} OMX_AUDIO_CODINGTYPE; - - -/** The PortDefinition structure is used to define all of the parameters - * necessary for the compliant component to setup an input or an output audio - * path. If additional information is needed to define the parameters of the - * port (such as frequency), additional structures must be sent such as the - * OMX_AUDIO_PARAM_PCMMODETYPE structure to supply the extra parameters for the port. - */ -typedef struct OMX_AUDIO_PORTDEFINITIONTYPE { - OMX_STRING cMIMEType; /**< MIME type of data for the port */ - OMX_NATIVE_DEVICETYPE pNativeRender; /** < platform specific reference - for an output device, - otherwise this field is 0 */ - OMX_BOOL bFlagErrorConcealment; /**< Turns on error concealment if it is - supported by the OMX component */ - OMX_AUDIO_CODINGTYPE eEncoding; /**< Type of data expected for this - port (e.g. PCM, AMR, MP3, etc) */ -} OMX_AUDIO_PORTDEFINITIONTYPE; - - -/** Port format parameter. This structure is used to enumerate - * the various data input/output format supported by the port. - */ -typedef struct OMX_AUDIO_PARAM_PORTFORMATTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Indicates which port to set */ - OMX_U32 nIndex; /**< Indicates the enumeration index for the format from 0x0 to N-1 */ - OMX_AUDIO_CODINGTYPE eEncoding; /**< Type of data expected for this port (e.g. PCM, AMR, MP3, etc) */ -} OMX_AUDIO_PARAM_PORTFORMATTYPE; - - -/** PCM mode type */ -typedef enum OMX_AUDIO_PCMMODETYPE { - OMX_AUDIO_PCMModeLinear = 0, /**< Linear PCM encoded data */ - OMX_AUDIO_PCMModeALaw, /**< A law PCM encoded data (G.711) */ - OMX_AUDIO_PCMModeMULaw, /**< Mu law PCM encoded data (G.711) */ - OMX_AUDIO_PCMModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_PCMModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_PCMModeMax = 0x7FFFFFFF -} OMX_AUDIO_PCMMODETYPE; - - -typedef enum OMX_AUDIO_CHANNELTYPE { - OMX_AUDIO_ChannelNone = 0x0, /**< Unused or empty */ - OMX_AUDIO_ChannelLF = 0x1, /**< Left front */ - OMX_AUDIO_ChannelRF = 0x2, /**< Right front */ - OMX_AUDIO_ChannelCF = 0x3, /**< Center front */ - OMX_AUDIO_ChannelLS = 0x4, /**< Left surround */ - OMX_AUDIO_ChannelRS = 0x5, /**< Right surround */ - OMX_AUDIO_ChannelLFE = 0x6, /**< Low frequency effects */ - OMX_AUDIO_ChannelCS = 0x7, /**< Back surround */ - OMX_AUDIO_ChannelLR = 0x8, /**< Left rear. */ - OMX_AUDIO_ChannelRR = 0x9, /**< Right rear. */ - OMX_AUDIO_ChannelKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_ChannelVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_ChannelMax = 0x7FFFFFFF -} OMX_AUDIO_CHANNELTYPE; - -#define OMX_AUDIO_MAXCHANNELS 16 /**< maximum number distinct audio channels that a buffer may contain */ -#define OMX_MIN_PCMPAYLOAD_MSEC 5 /**< Minimum audio buffer payload size for uncompressed (PCM) audio */ - -/** PCM format description */ -typedef struct OMX_AUDIO_PARAM_PCMMODETYPE { - OMX_U32 nSize; /**< Size of this structure, in Bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels (e.g. 2 for stereo) */ - OMX_NUMERICALDATATYPE eNumData; /**< indicates PCM data as signed or unsigned */ - OMX_ENDIANTYPE eEndian; /**< indicates PCM data as little or big endian */ - OMX_BOOL bInterleaved; /**< True for normal interleaved data; false for - non-interleaved data (e.g. block data) */ - OMX_U32 nBitPerSample; /**< Bit per sample */ - OMX_U32 nSamplingRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_AUDIO_PCMMODETYPE ePCMMode; /**< PCM mode enumeration */ - OMX_AUDIO_CHANNELTYPE eChannelMapping[OMX_AUDIO_MAXCHANNELS]; /**< Slot i contains channel defined by eChannelMap[i] */ - -} OMX_AUDIO_PARAM_PCMMODETYPE; - - -/** Audio channel mode. This is used by both AAC and MP3, although the names are more appropriate - * for the MP3. For example, JointStereo for MP3 is CouplingChannels for AAC. - */ -typedef enum OMX_AUDIO_CHANNELMODETYPE { - OMX_AUDIO_ChannelModeStereo = 0, /**< 2 channels, the bitrate allocation between those - two channels changes accordingly to each channel information */ - OMX_AUDIO_ChannelModeJointStereo, /**< mode that takes advantage of what is common between - 2 channels for higher compression gain */ - OMX_AUDIO_ChannelModeDual, /**< 2 mono-channels, each channel is encoded with half - the bitrate of the overall bitrate */ - OMX_AUDIO_ChannelModeMono, /**< Mono channel mode */ - OMX_AUDIO_ChannelModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_ChannelModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_ChannelModeMax = 0x7FFFFFFF -} OMX_AUDIO_CHANNELMODETYPE; - - -typedef enum OMX_AUDIO_MP3STREAMFORMATTYPE { - OMX_AUDIO_MP3StreamFormatMP1Layer3 = 0, /**< MP3 Audio MPEG 1 Layer 3 Stream format */ - OMX_AUDIO_MP3StreamFormatMP2Layer3, /**< MP3 Audio MPEG 2 Layer 3 Stream format */ - OMX_AUDIO_MP3StreamFormatMP2_5Layer3, /**< MP3 Audio MPEG2.5 Layer 3 Stream format */ - OMX_AUDIO_MP3StreamFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_MP3StreamFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_MP3StreamFormatMax = 0x7FFFFFFF -} OMX_AUDIO_MP3STREAMFORMATTYPE; - -/** MP3 params */ -typedef struct OMX_AUDIO_PARAM_MP3TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate of the input data. Use 0 for variable - rate or unknown bit rates */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_U32 nAudioBandWidth; /**< Audio band width (in Hz) to which an encoder should - limit the audio signal. Use 0 to let encoder decide */ - OMX_AUDIO_CHANNELMODETYPE eChannelMode; /**< Channel mode enumeration */ - OMX_AUDIO_MP3STREAMFORMATTYPE eFormat; /**< MP3 stream format */ -} OMX_AUDIO_PARAM_MP3TYPE; - - -typedef enum OMX_AUDIO_AACSTREAMFORMATTYPE { - OMX_AUDIO_AACStreamFormatMP2ADTS = 0, /**< AAC Audio Data Transport Stream 2 format */ - OMX_AUDIO_AACStreamFormatMP4ADTS, /**< AAC Audio Data Transport Stream 4 format */ - OMX_AUDIO_AACStreamFormatMP4LOAS, /**< AAC Low Overhead Audio Stream format */ - OMX_AUDIO_AACStreamFormatMP4LATM, /**< AAC Low overhead Audio Transport Multiplex */ - OMX_AUDIO_AACStreamFormatADIF, /**< AAC Audio Data Interchange Format */ - OMX_AUDIO_AACStreamFormatMP4FF, /**< AAC inside MPEG-4/ISO File Format */ - OMX_AUDIO_AACStreamFormatRAW, /**< AAC Raw Format */ - OMX_AUDIO_AACStreamFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_AACStreamFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_AACStreamFormatMax = 0x7FFFFFFF -} OMX_AUDIO_AACSTREAMFORMATTYPE; - - -/** AAC mode type. Note that the term profile is used with the MPEG-2 - * standard and the term object type and profile is used with MPEG-4 */ -typedef enum OMX_AUDIO_AACPROFILETYPE{ - OMX_AUDIO_AACObjectNull = 0, /**< Null, not used */ - OMX_AUDIO_AACObjectMain = 1, /**< AAC Main object */ - OMX_AUDIO_AACObjectLC, /**< AAC Low Complexity object (AAC profile) */ - OMX_AUDIO_AACObjectSSR, /**< AAC Scalable Sample Rate object */ - OMX_AUDIO_AACObjectLTP, /**< AAC Long Term Prediction object */ - OMX_AUDIO_AACObjectHE, /**< AAC High Efficiency (object type SBR, HE-AAC profile) */ - OMX_AUDIO_AACObjectScalable, /**< AAC Scalable object */ - OMX_AUDIO_AACObjectERLC = 17, /**< ER AAC Low Complexity object (Error Resilient AAC-LC) */ - OMX_AUDIO_AACObjectLD = 23, /**< AAC Low Delay object (Error Resilient) */ - OMX_AUDIO_AACObjectHE_PS = 29, /**< AAC High Efficiency with Parametric Stereo coding (HE-AAC v2, object type PS) */ - OMX_AUDIO_AACObjectELD = 39, /** AAC Enhanced Low Delay. NOTE: Pending Khronos standardization **/ - OMX_AUDIO_AACObjectKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_AACObjectVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_AACObjectMax = 0x7FFFFFFF -} OMX_AUDIO_AACPROFILETYPE; - - -/** AAC tool usage (for nAACtools in OMX_AUDIO_PARAM_AACPROFILETYPE). - * Required for encoder configuration and optional as decoder info output. - * For MP3, OMX_AUDIO_CHANNELMODETYPE is sufficient. */ -#define OMX_AUDIO_AACToolNone 0x00000000 /**< no AAC tools allowed (encoder config) or active (decoder info output) */ -#define OMX_AUDIO_AACToolMS 0x00000001 /**< MS: Mid/side joint coding tool allowed or active */ -#define OMX_AUDIO_AACToolIS 0x00000002 /**< IS: Intensity stereo tool allowed or active */ -#define OMX_AUDIO_AACToolTNS 0x00000004 /**< TNS: Temporal Noise Shaping tool allowed or active */ -#define OMX_AUDIO_AACToolPNS 0x00000008 /**< PNS: MPEG-4 Perceptual Noise substitution tool allowed or active */ -#define OMX_AUDIO_AACToolLTP 0x00000010 /**< LTP: MPEG-4 Long Term Prediction tool allowed or active */ -#define OMX_AUDIO_AACToolVendor 0x00010000 /**< NOT A KHRONOS VALUE, offset for vendor-specific additions */ -#define OMX_AUDIO_AACToolAll 0x7FFFFFFF /**< all AAC tools allowed or active (*/ - -/** MPEG-4 AAC error resilience (ER) tool usage (for nAACERtools in OMX_AUDIO_PARAM_AACPROFILETYPE). - * Required for ER encoder configuration and optional as decoder info output */ -#define OMX_AUDIO_AACERNone 0x00000000 /**< no AAC ER tools allowed/used */ -#define OMX_AUDIO_AACERVCB11 0x00000001 /**< VCB11: Virtual Code Books for AAC section data */ -#define OMX_AUDIO_AACERRVLC 0x00000002 /**< RVLC: Reversible Variable Length Coding */ -#define OMX_AUDIO_AACERHCR 0x00000004 /**< HCR: Huffman Codeword Reordering */ -#define OMX_AUDIO_AACERAll 0x7FFFFFFF /**< all AAC ER tools allowed/used */ - - -/** AAC params */ -typedef struct OMX_AUDIO_PARAM_AACPROFILETYPE { - OMX_U32 nSize; /**< Size of this structure, in Bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_U32 nBitRate; /**< Bit rate of the input data. Use 0 for variable - rate or unknown bit rates */ - OMX_U32 nAudioBandWidth; /**< Audio band width (in Hz) to which an encoder should - limit the audio signal. Use 0 to let encoder decide */ - OMX_U32 nFrameLength; /**< Frame length (in audio samples per channel) of the codec. - Can be 1024 or 960 (AAC-LC), 2048 (HE-AAC), 480 or 512 (AAC-LD). - Use 0 to let encoder decide */ - OMX_U32 nAACtools; /**< AAC tool usage */ - OMX_U32 nAACERtools; /**< MPEG-4 AAC error resilience tool usage */ - OMX_AUDIO_AACPROFILETYPE eAACProfile; /**< AAC profile enumeration */ - OMX_AUDIO_AACSTREAMFORMATTYPE eAACStreamFormat; /**< AAC stream format enumeration */ - OMX_AUDIO_CHANNELMODETYPE eChannelMode; /**< Channel mode enumeration */ -} OMX_AUDIO_PARAM_AACPROFILETYPE; - - -/** VORBIS params */ -typedef struct OMX_AUDIO_PARAM_VORBISTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate of the encoded data data. Use 0 for variable - rate or unknown bit rates. Encoding is set to the - bitrate closest to specified value (in bps) */ - OMX_U32 nMinBitRate; /**< Sets minimum bitrate (in bps). */ - OMX_U32 nMaxBitRate; /**< Sets maximum bitrate (in bps). */ - - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_U32 nAudioBandWidth; /**< Audio band width (in Hz) to which an encoder should - limit the audio signal. Use 0 to let encoder decide */ - OMX_S32 nQuality; /**< Sets encoding quality to n, between -1 (low) and 10 (high). - In the default mode of operation, teh quality level is 3. - Normal quality range is 0 - 10. */ - OMX_BOOL bManaged; /**< Set bitrate management mode. This turns off the - normal VBR encoding, but allows hard or soft bitrate - constraints to be enforced by the encoder. This mode can - be slower, and may also be lower quality. It is - primarily useful for streaming. */ - OMX_BOOL bDownmix; /**< Downmix input from stereo to mono (has no effect on - non-stereo streams). Useful for lower-bitrate encoding. */ -} OMX_AUDIO_PARAM_VORBISTYPE; - - -/** FLAC params */ -typedef struct OMX_AUDIO_PARAM_FLACTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - unknown sampling rate. */ - OMX_U32 nCompressionLevel;/**< FLAC compression level, from 0 (fastest compression) - to 8 (highest compression */ -} OMX_AUDIO_PARAM_FLACTYPE; - - -/** WMA Version */ -typedef enum OMX_AUDIO_WMAFORMATTYPE { - OMX_AUDIO_WMAFormatUnused = 0, /**< format unused or unknown */ - OMX_AUDIO_WMAFormat7, /**< Windows Media Audio format 7 */ - OMX_AUDIO_WMAFormat8, /**< Windows Media Audio format 8 */ - OMX_AUDIO_WMAFormat9, /**< Windows Media Audio format 9 */ - OMX_AUDIO_WMAFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_WMAFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_WMAFormatMax = 0x7FFFFFFF -} OMX_AUDIO_WMAFORMATTYPE; - - -/** WMA Profile */ -typedef enum OMX_AUDIO_WMAPROFILETYPE { - OMX_AUDIO_WMAProfileUnused = 0, /**< profile unused or unknown */ - OMX_AUDIO_WMAProfileL1, /**< Windows Media audio version 9 profile L1 */ - OMX_AUDIO_WMAProfileL2, /**< Windows Media audio version 9 profile L2 */ - OMX_AUDIO_WMAProfileL3, /**< Windows Media audio version 9 profile L3 */ - OMX_AUDIO_WMAProfileKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_WMAProfileVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_WMAProfileMax = 0x7FFFFFFF -} OMX_AUDIO_WMAPROFILETYPE; - - -/** WMA params */ -typedef struct OMX_AUDIO_PARAM_WMATYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U16 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate of the input data. Use 0 for variable - rate or unknown bit rates */ - OMX_AUDIO_WMAFORMATTYPE eFormat; /**< Version of WMA stream / data */ - OMX_AUDIO_WMAPROFILETYPE eProfile; /**< Profile of WMA stream / data */ - OMX_U32 nSamplingRate; /**< Sampling rate of the source data */ - OMX_U16 nBlockAlign; /**< is the block alignment, or block size, in bytes of the audio codec */ - OMX_U16 nEncodeOptions; /**< WMA Type-specific data */ - OMX_U32 nSuperBlockAlign; /**< WMA Type-specific data */ -} OMX_AUDIO_PARAM_WMATYPE; - -/** - * RealAudio format - */ -typedef enum OMX_AUDIO_RAFORMATTYPE { - OMX_AUDIO_RAFormatUnused = 0, /**< Format unused or unknown */ - OMX_AUDIO_RA8, /**< RealAudio 8 codec */ - OMX_AUDIO_RA9, /**< RealAudio 9 codec */ - OMX_AUDIO_RA10_AAC, /**< MPEG-4 AAC codec for bitrates of more than 128kbps */ - OMX_AUDIO_RA10_CODEC, /**< RealAudio codec for bitrates less than 128 kbps */ - OMX_AUDIO_RA10_LOSSLESS, /**< RealAudio Lossless */ - OMX_AUDIO_RA10_MULTICHANNEL, /**< RealAudio Multichannel */ - OMX_AUDIO_RA10_VOICE, /**< RealAudio Voice for bitrates below 15 kbps */ - OMX_AUDIO_RAFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_RAFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_RAFormatMax = 0x7FFFFFFF -} OMX_AUDIO_RAFORMATTYPE; - -/** RA (Real Audio) params */ -typedef struct OMX_AUDIO_PARAM_RATYPE { - OMX_U32 nSize; /**< Size of this structure, in Bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nSamplingRate; /**< is the sampling rate of the source data */ - OMX_U32 nBitsPerFrame; /**< is the value for bits per frame */ - OMX_U32 nSamplePerFrame; /**< is the value for samples per frame */ - OMX_U32 nCouplingQuantBits; /**< is the number of coupling quantization bits in the stream */ - OMX_U32 nCouplingStartRegion; /**< is the coupling start region in the stream */ - OMX_U32 nNumRegions; /**< is the number of regions value */ - OMX_AUDIO_RAFORMATTYPE eFormat; /**< is the RealAudio audio format */ -} OMX_AUDIO_PARAM_RATYPE; - - -/** SBC Allocation Method Type */ -typedef enum OMX_AUDIO_SBCALLOCMETHODTYPE { - OMX_AUDIO_SBCAllocMethodLoudness, /**< Loudness allocation method */ - OMX_AUDIO_SBCAllocMethodSNR, /**< SNR allocation method */ - OMX_AUDIO_SBCAllocMethodKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_SBCAllocMethodVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_SBCAllocMethodMax = 0x7FFFFFFF -} OMX_AUDIO_SBCALLOCMETHODTYPE; - - -/** SBC params */ -typedef struct OMX_AUDIO_PARAM_SBCTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate of the input data. Use 0 for variable - rate or unknown bit rates */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_U32 nBlocks; /**< Number of blocks */ - OMX_U32 nSubbands; /**< Number of subbands */ - OMX_U32 nBitPool; /**< Bitpool value */ - OMX_BOOL bEnableBitrate; /**< Use bitrate value instead of bitpool */ - OMX_AUDIO_CHANNELMODETYPE eChannelMode; /**< Channel mode enumeration */ - OMX_AUDIO_SBCALLOCMETHODTYPE eSBCAllocType; /**< SBC Allocation method type */ -} OMX_AUDIO_PARAM_SBCTYPE; - - -/** ADPCM stream format parameters */ -typedef struct OMX_AUDIO_PARAM_ADPCMTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_U32 nBitsPerSample; /**< Number of bits in each sample */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ -} OMX_AUDIO_PARAM_ADPCMTYPE; - - -/** G723 rate */ -typedef enum OMX_AUDIO_G723RATE { - OMX_AUDIO_G723ModeUnused = 0, /**< AMRNB Mode unused / unknown */ - OMX_AUDIO_G723ModeLow, /**< 5300 bps */ - OMX_AUDIO_G723ModeHigh, /**< 6300 bps */ - OMX_AUDIO_G723ModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_G723ModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_G723ModeMax = 0x7FFFFFFF -} OMX_AUDIO_G723RATE; - - -/** G723 - Sample rate must be 8 KHz */ -typedef struct OMX_AUDIO_PARAM_G723TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_AUDIO_G723RATE eBitRate; /**< todo: Should this be moved to a config? */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ - OMX_BOOL bPostFilter; /**< Enable Post Filter */ -} OMX_AUDIO_PARAM_G723TYPE; - - -/** ITU G726 (ADPCM) rate */ -typedef enum OMX_AUDIO_G726MODE { - OMX_AUDIO_G726ModeUnused = 0, /**< G726 Mode unused / unknown */ - OMX_AUDIO_G726Mode16, /**< 16 kbps */ - OMX_AUDIO_G726Mode24, /**< 24 kbps */ - OMX_AUDIO_G726Mode32, /**< 32 kbps, most common rate, also G721 */ - OMX_AUDIO_G726Mode40, /**< 40 kbps */ - OMX_AUDIO_G726ModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_G726ModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_G726ModeMax = 0x7FFFFFFF -} OMX_AUDIO_G726MODE; - - -/** G.726 stream format parameters - must be at 8KHz */ -typedef struct OMX_AUDIO_PARAM_G726TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_AUDIO_G726MODE eG726Mode; -} OMX_AUDIO_PARAM_G726TYPE; - - -/** G729 coder type */ -typedef enum OMX_AUDIO_G729TYPE { - OMX_AUDIO_G729 = 0, /**< ITU G.729 encoded data */ - OMX_AUDIO_G729A, /**< ITU G.729 annex A encoded data */ - OMX_AUDIO_G729B, /**< ITU G.729 with annex B encoded data */ - OMX_AUDIO_G729AB, /**< ITU G.729 annexes A and B encoded data */ - OMX_AUDIO_G729KhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_G729VendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_G729Max = 0x7FFFFFFF -} OMX_AUDIO_G729TYPE; - - -/** G729 stream format parameters - fixed 6KHz sample rate */ -typedef struct OMX_AUDIO_PARAM_G729TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_AUDIO_G729TYPE eBitType; -} OMX_AUDIO_PARAM_G729TYPE; - - -/** AMR Frame format */ -typedef enum OMX_AUDIO_AMRFRAMEFORMATTYPE { - OMX_AUDIO_AMRFrameFormatConformance = 0, /**< Frame Format is AMR Conformance - (Standard) Format */ - OMX_AUDIO_AMRFrameFormatIF1, /**< Frame Format is AMR Interface - Format 1 */ - OMX_AUDIO_AMRFrameFormatIF2, /**< Frame Format is AMR Interface - Format 2*/ - OMX_AUDIO_AMRFrameFormatFSF, /**< Frame Format is AMR File Storage - Format */ - OMX_AUDIO_AMRFrameFormatRTPPayload, /**< Frame Format is AMR Real-Time - Transport Protocol Payload Format */ - OMX_AUDIO_AMRFrameFormatITU, /**< Frame Format is ITU Format (added at Motorola request) */ - OMX_AUDIO_AMRFrameFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_AMRFrameFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_AMRFrameFormatMax = 0x7FFFFFFF -} OMX_AUDIO_AMRFRAMEFORMATTYPE; - - -/** AMR band mode */ -typedef enum OMX_AUDIO_AMRBANDMODETYPE { - OMX_AUDIO_AMRBandModeUnused = 0, /**< AMRNB Mode unused / unknown */ - OMX_AUDIO_AMRBandModeNB0, /**< AMRNB Mode 0 = 4750 bps */ - OMX_AUDIO_AMRBandModeNB1, /**< AMRNB Mode 1 = 5150 bps */ - OMX_AUDIO_AMRBandModeNB2, /**< AMRNB Mode 2 = 5900 bps */ - OMX_AUDIO_AMRBandModeNB3, /**< AMRNB Mode 3 = 6700 bps */ - OMX_AUDIO_AMRBandModeNB4, /**< AMRNB Mode 4 = 7400 bps */ - OMX_AUDIO_AMRBandModeNB5, /**< AMRNB Mode 5 = 7950 bps */ - OMX_AUDIO_AMRBandModeNB6, /**< AMRNB Mode 6 = 10200 bps */ - OMX_AUDIO_AMRBandModeNB7, /**< AMRNB Mode 7 = 12200 bps */ - OMX_AUDIO_AMRBandModeWB0, /**< AMRWB Mode 0 = 6600 bps */ - OMX_AUDIO_AMRBandModeWB1, /**< AMRWB Mode 1 = 8850 bps */ - OMX_AUDIO_AMRBandModeWB2, /**< AMRWB Mode 2 = 12650 bps */ - OMX_AUDIO_AMRBandModeWB3, /**< AMRWB Mode 3 = 14250 bps */ - OMX_AUDIO_AMRBandModeWB4, /**< AMRWB Mode 4 = 15850 bps */ - OMX_AUDIO_AMRBandModeWB5, /**< AMRWB Mode 5 = 18250 bps */ - OMX_AUDIO_AMRBandModeWB6, /**< AMRWB Mode 6 = 19850 bps */ - OMX_AUDIO_AMRBandModeWB7, /**< AMRWB Mode 7 = 23050 bps */ - OMX_AUDIO_AMRBandModeWB8, /**< AMRWB Mode 8 = 23850 bps */ - OMX_AUDIO_AMRBandModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_AMRBandModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_AMRBandModeMax = 0x7FFFFFFF -} OMX_AUDIO_AMRBANDMODETYPE; - - -/** AMR Discontinuous Transmission mode */ -typedef enum OMX_AUDIO_AMRDTXMODETYPE { - OMX_AUDIO_AMRDTXModeOff = 0, /**< AMR Discontinuous Transmission Mode is disabled */ - OMX_AUDIO_AMRDTXModeOnVAD1, /**< AMR Discontinuous Transmission Mode using - Voice Activity Detector 1 (VAD1) is enabled */ - OMX_AUDIO_AMRDTXModeOnVAD2, /**< AMR Discontinuous Transmission Mode using - Voice Activity Detector 2 (VAD2) is enabled */ - OMX_AUDIO_AMRDTXModeOnAuto, /**< The codec will automatically select between - Off, VAD1 or VAD2 modes */ - - OMX_AUDIO_AMRDTXasEFR, /**< DTX as EFR instead of AMR standard (3GPP 26.101, frame type =8,9,10) */ - - OMX_AUDIO_AMRDTXModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_AMRDTXModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_AMRDTXModeMax = 0x7FFFFFFF -} OMX_AUDIO_AMRDTXMODETYPE; - - -/** AMR params */ -typedef struct OMX_AUDIO_PARAM_AMRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate read only field */ - OMX_AUDIO_AMRBANDMODETYPE eAMRBandMode; /**< AMR Band Mode enumeration */ - OMX_AUDIO_AMRDTXMODETYPE eAMRDTXMode; /**< AMR DTX Mode enumeration */ - OMX_AUDIO_AMRFRAMEFORMATTYPE eAMRFrameFormat; /**< AMR frame format enumeration */ -} OMX_AUDIO_PARAM_AMRTYPE; - - -/** GSM_FR (ETSI 06.10, 3GPP 46.010) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_GSMFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_GSMFRTYPE; - - -/** GSM-HR (ETSI 06.20, 3GPP 46.020) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_GSMHRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_GSMHRTYPE; - - -/** GSM-EFR (ETSI 06.60, 3GPP 46.060) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_GSMEFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_GSMEFRTYPE; - - -/** TDMA FR (TIA/EIA-136-420, VSELP 7.95kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_TDMAFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_TDMAFRTYPE; - - -/** TDMA EFR (TIA/EIA-136-410, ACELP 7.4kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_TDMAEFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_TDMAEFRTYPE; - - -/** PDC FR ( RCR-27, VSELP 6.7kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_PDCFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_PDCFRTYPE; - - -/** PDC EFR ( RCR-27, ACELP 6.7kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_PDCEFRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_PDCEFRTYPE; - -/** PDC HR ( RCR-27, PSI-CELP 3.45kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_PDCHRTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_BOOL bDTX; /**< Enable Discontinuous Transmisssion */ - OMX_BOOL bHiPassFilter; /**< Enable High Pass Filter */ -} OMX_AUDIO_PARAM_PDCHRTYPE; - - -/** CDMA Rate types */ -typedef enum OMX_AUDIO_CDMARATETYPE { - OMX_AUDIO_CDMARateBlank = 0, /**< CDMA encoded frame is blank */ - OMX_AUDIO_CDMARateFull, /**< CDMA encoded frame in full rate */ - OMX_AUDIO_CDMARateHalf, /**< CDMA encoded frame in half rate */ - OMX_AUDIO_CDMARateQuarter, /**< CDMA encoded frame in quarter rate */ - OMX_AUDIO_CDMARateEighth, /**< CDMA encoded frame in eighth rate (DTX)*/ - OMX_AUDIO_CDMARateErasure, /**< CDMA erasure frame */ - OMX_AUDIO_CDMARateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_CDMARateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_CDMARateMax = 0x7FFFFFFF -} OMX_AUDIO_CDMARATETYPE; - - -/** QCELP8 (TIA/EIA-96, up to 8kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_QCELP8TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_U32 nBitRate; /**< Bit rate of the input data. Use 0 for variable - rate or unknown bit rates */ - OMX_AUDIO_CDMARATETYPE eCDMARate; /**< Frame rate */ - OMX_U32 nMinBitRate; /**< minmal rate for the encoder = 1,2,3,4, default = 1 */ - OMX_U32 nMaxBitRate; /**< maximal rate for the encoder = 1,2,3,4, default = 4 */ -} OMX_AUDIO_PARAM_QCELP8TYPE; - - -/** QCELP13 ( CDMA, EIA/TIA-733, 13.3kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_QCELP13TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_AUDIO_CDMARATETYPE eCDMARate; /**< Frame rate */ - OMX_U32 nMinBitRate; /**< minmal rate for the encoder = 1,2,3,4, default = 1 */ - OMX_U32 nMaxBitRate; /**< maximal rate for the encoder = 1,2,3,4, default = 4 */ -} OMX_AUDIO_PARAM_QCELP13TYPE; - - -/** EVRC ( CDMA, EIA/TIA-127, RCELP up to 8.55kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_EVRCTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_AUDIO_CDMARATETYPE eCDMARate; /**< actual Frame rate */ - OMX_BOOL bRATE_REDUCon; /**< RATE_REDUCtion is requested for this frame */ - OMX_U32 nMinBitRate; /**< minmal rate for the encoder = 1,2,3,4, default = 1 */ - OMX_U32 nMaxBitRate; /**< maximal rate for the encoder = 1,2,3,4, default = 4 */ - OMX_BOOL bHiPassFilter; /**< Enable encoder's High Pass Filter */ - OMX_BOOL bNoiseSuppressor; /**< Enable encoder's noise suppressor pre-processing */ - OMX_BOOL bPostFilter; /**< Enable decoder's post Filter */ -} OMX_AUDIO_PARAM_EVRCTYPE; - - -/** SMV ( up to 8.55kbps coder) stream format parameters */ -typedef struct OMX_AUDIO_PARAM_SMVTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels in the data stream (not - necessarily the same as the number of channels - to be rendered. */ - OMX_AUDIO_CDMARATETYPE eCDMARate; /**< Frame rate */ - OMX_BOOL bRATE_REDUCon; /**< RATE_REDUCtion is requested for this frame */ - OMX_U32 nMinBitRate; /**< minmal rate for the encoder = 1,2,3,4, default = 1 ??*/ - OMX_U32 nMaxBitRate; /**< maximal rate for the encoder = 1,2,3,4, default = 4 ??*/ - OMX_BOOL bHiPassFilter; /**< Enable encoder's High Pass Filter ??*/ - OMX_BOOL bNoiseSuppressor; /**< Enable encoder's noise suppressor pre-processing */ - OMX_BOOL bPostFilter; /**< Enable decoder's post Filter ??*/ -} OMX_AUDIO_PARAM_SMVTYPE; - - -/** MIDI Format - * @ingroup midi - */ -typedef enum OMX_AUDIO_MIDIFORMATTYPE -{ - OMX_AUDIO_MIDIFormatUnknown = 0, /**< MIDI Format unknown or don't care */ - OMX_AUDIO_MIDIFormatSMF0, /**< Standard MIDI File Type 0 */ - OMX_AUDIO_MIDIFormatSMF1, /**< Standard MIDI File Type 1 */ - OMX_AUDIO_MIDIFormatSMF2, /**< Standard MIDI File Type 2 */ - OMX_AUDIO_MIDIFormatSPMIDI, /**< SP-MIDI */ - OMX_AUDIO_MIDIFormatXMF0, /**< eXtensible Music Format type 0 */ - OMX_AUDIO_MIDIFormatXMF1, /**< eXtensible Music Format type 1 */ - OMX_AUDIO_MIDIFormatMobileXMF, /**< Mobile XMF (eXtensible Music Format type 2) */ - OMX_AUDIO_MIDIFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_MIDIFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_MIDIFormatMax = 0x7FFFFFFF -} OMX_AUDIO_MIDIFORMATTYPE; - - -/** MIDI params - * @ingroup midi - */ -typedef struct OMX_AUDIO_PARAM_MIDITYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nFileSize; /**< size of the MIDI file in bytes, where the entire - MIDI file passed in, otherwise if 0x0, the MIDI data - is merged and streamed (instead of passed as an - entire MIDI file) */ - OMX_BU32 sMaxPolyphony; /**< Specifies the maximum simultaneous polyphonic - voices. A value of zero indicates that the default - polyphony of the device is used */ - OMX_BOOL bLoadDefaultSound; /**< Whether to load default sound - bank at initialization */ - OMX_AUDIO_MIDIFORMATTYPE eMidiFormat; /**< Version of the MIDI file */ -} OMX_AUDIO_PARAM_MIDITYPE; - - -/** Type of the MIDI sound bank - * @ingroup midi - */ -typedef enum OMX_AUDIO_MIDISOUNDBANKTYPE { - OMX_AUDIO_MIDISoundBankUnused = 0, /**< unused/unknown soundbank type */ - OMX_AUDIO_MIDISoundBankDLS1, /**< DLS version 1 */ - OMX_AUDIO_MIDISoundBankDLS2, /**< DLS version 2 */ - OMX_AUDIO_MIDISoundBankMobileDLSBase, /**< Mobile DLS, using the base functionality */ - OMX_AUDIO_MIDISoundBankMobileDLSPlusOptions, /**< Mobile DLS, using the specification-defined optional feature set */ - OMX_AUDIO_MIDISoundBankKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_MIDISoundBankVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_MIDISoundBankMax = 0x7FFFFFFF -} OMX_AUDIO_MIDISOUNDBANKTYPE; - - -/** Bank Layout describes how bank MSB & LSB are used in the DLS instrument definitions sound bank - * @ingroup midi - */ -typedef enum OMX_AUDIO_MIDISOUNDBANKLAYOUTTYPE { - OMX_AUDIO_MIDISoundBankLayoutUnused = 0, /**< unused/unknown soundbank type */ - OMX_AUDIO_MIDISoundBankLayoutGM, /**< GS layout (based on bank MSB 0x00) */ - OMX_AUDIO_MIDISoundBankLayoutGM2, /**< General MIDI 2 layout (using MSB 0x78/0x79, LSB 0x00) */ - OMX_AUDIO_MIDISoundBankLayoutUser, /**< Does not conform to any bank numbering standards */ - OMX_AUDIO_MIDISoundBankLayoutKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_MIDISoundBankLayoutVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_MIDISoundBankLayoutMax = 0x7FFFFFFF -} OMX_AUDIO_MIDISOUNDBANKLAYOUTTYPE; - - -/** MIDI params to load/unload user soundbank - * @ingroup midi - */ -typedef struct OMX_AUDIO_PARAM_MIDILOADUSERSOUNDTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nDLSIndex; /**< DLS file index to be loaded */ - OMX_U32 nDLSSize; /**< Size in bytes */ - OMX_PTR pDLSData; /**< Pointer to DLS file data */ - OMX_AUDIO_MIDISOUNDBANKTYPE eMidiSoundBank; /**< Midi sound bank type enumeration */ - OMX_AUDIO_MIDISOUNDBANKLAYOUTTYPE eMidiSoundBankLayout; /**< Midi sound bank layout enumeration */ -} OMX_AUDIO_PARAM_MIDILOADUSERSOUNDTYPE; - - -/** Structure for Live MIDI events and MIP messages. - * (MIP = Maximum Instantaneous Polyphony; part of the SP-MIDI standard.) - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDIIMMEDIATEEVENTTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port that this structure applies to */ - OMX_U32 nMidiEventSize; /**< Size of immediate MIDI events or MIP message in bytes */ - OMX_U8 nMidiEvents[1]; /**< MIDI event array to be rendered immediately, or an - array for the MIP message buffer, where the size is - indicated by nMidiEventSize */ -} OMX_AUDIO_CONFIG_MIDIIMMEDIATEEVENTTYPE; - - -/** MIDI sound bank/ program pair in a given channel - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDISOUNDBANKPROGRAMTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port that this structure applies to */ - OMX_U32 nChannel; /**< Valid channel values range from 1 to 16 */ - OMX_U16 nIDProgram; /**< Valid program ID range is 1 to 128 */ - OMX_U16 nIDSoundBank; /**< Sound bank ID */ - OMX_U32 nUserSoundBankIndex;/**< User soundbank index, easier to access soundbanks - by index if multiple banks are present */ -} OMX_AUDIO_CONFIG_MIDISOUNDBANKPROGRAMTYPE; - - -/** MIDI control - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDICONTROLTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BS32 sPitchTransposition; /**< Pitch transposition in semitones, stored as Q22.10 - format based on JAVA MMAPI (JSR-135) requirement */ - OMX_BU32 sPlayBackRate; /**< Relative playback rate, stored as Q14.17 fixed-point - number based on JSR-135 requirement */ - OMX_BU32 sTempo ; /**< Tempo in beats per minute (BPM), stored as Q22.10 - fixed-point number based on JSR-135 requirement */ - OMX_U32 nMaxPolyphony; /**< Specifies the maximum simultaneous polyphonic - voices. A value of zero indicates that the default - polyphony of the device is used */ - OMX_U32 nNumRepeat; /**< Number of times to repeat playback */ - OMX_U32 nStopTime; /**< Time in milliseconds to indicate when playback - will stop automatically. Set to zero if not used */ - OMX_U16 nChannelMuteMask; /**< 16 bit mask for channel mute status */ - OMX_U16 nChannelSoloMask; /**< 16 bit mask for channel solo status */ - OMX_U32 nTrack0031MuteMask; /**< 32 bit mask for track mute status. Note: This is for tracks 0-31 */ - OMX_U32 nTrack3263MuteMask; /**< 32 bit mask for track mute status. Note: This is for tracks 32-63 */ - OMX_U32 nTrack0031SoloMask; /**< 32 bit mask for track solo status. Note: This is for tracks 0-31 */ - OMX_U32 nTrack3263SoloMask; /**< 32 bit mask for track solo status. Note: This is for tracks 32-63 */ - -} OMX_AUDIO_CONFIG_MIDICONTROLTYPE; - - -/** MIDI Playback States - * @ingroup midi - */ -typedef enum OMX_AUDIO_MIDIPLAYBACKSTATETYPE { - OMX_AUDIO_MIDIPlayBackStateUnknown = 0, /**< Unknown state or state does not map to - other defined states */ - OMX_AUDIO_MIDIPlayBackStateClosedEngaged, /**< No MIDI resource is currently open. - The MIDI engine is currently processing - MIDI events. */ - OMX_AUDIO_MIDIPlayBackStateParsing, /**< A MIDI resource is open and is being - primed. The MIDI engine is currently - processing MIDI events. */ - OMX_AUDIO_MIDIPlayBackStateOpenEngaged, /**< A MIDI resource is open and primed but - not playing. The MIDI engine is currently - processing MIDI events. The transition to - this state is only possible from the - OMX_AUDIO_MIDIPlayBackStatePlaying state, - when the 'playback head' reaches the end - of media data or the playback stops due - to stop time set.*/ - OMX_AUDIO_MIDIPlayBackStatePlaying, /**< A MIDI resource is open and currently - playing. The MIDI engine is currently - processing MIDI events.*/ - OMX_AUDIO_MIDIPlayBackStatePlayingPartially, /**< Best-effort playback due to SP-MIDI/DLS - resource constraints */ - OMX_AUDIO_MIDIPlayBackStatePlayingSilently, /**< Due to system resource constraints and - SP-MIDI content constraints, there is - no audible MIDI content during playback - currently. The situation may change if - resources are freed later.*/ - OMX_AUDIO_MIDIPlayBackStateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_MIDIPlayBackStateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_MIDIPlayBackStateMax = 0x7FFFFFFF -} OMX_AUDIO_MIDIPLAYBACKSTATETYPE; - - -/** MIDI status - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDISTATUSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U16 nNumTracks; /**< Number of MIDI tracks in the file, read only field. - NOTE: May not return a meaningful value until the entire - file is parsed and buffered. */ - OMX_U32 nDuration; /**< The length of the currently open MIDI resource - in milliseconds. NOTE: May not return a meaningful value - until the entire file is parsed and buffered. */ - OMX_U32 nPosition; /**< Current Position of the MIDI resource being played - in milliseconds */ - OMX_BOOL bVibra; /**< Does Vibra track exist? NOTE: May not return a meaningful - value until the entire file is parsed and buffered. */ - OMX_U32 nNumMetaEvents; /**< Total number of MIDI Meta Events in the currently - open MIDI resource. NOTE: May not return a meaningful value - until the entire file is parsed and buffered. */ - OMX_U32 nNumActiveVoices; /**< Number of active voices in the currently playing - MIDI resource. NOTE: May not return a meaningful value until - the entire file is parsed and buffered. */ - OMX_AUDIO_MIDIPLAYBACKSTATETYPE eMIDIPlayBackState; /**< MIDI playback state enumeration, read only field */ -} OMX_AUDIO_CONFIG_MIDISTATUSTYPE; - - -/** MIDI Meta Event structure one per Meta Event. - * MIDI Meta Events are like audio metadata, except that they are interspersed - * with the MIDI content throughout the file and are not localized in the header. - * As such, it is necessary to retrieve information about these Meta Events from - * the engine, as it encounters these Meta Events within the MIDI content. - * For example, SMF files can have up to 14 types of MIDI Meta Events (copyright, - * author, default tempo, etc.) scattered throughout the file. - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDIMETAEVENTTYPE{ - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nIndex; /**< Index of Meta Event */ - OMX_U8 nMetaEventType; /**< Meta Event Type, 7bits (i.e. 0 - 127) */ - OMX_U32 nMetaEventSize; /**< size of the Meta Event in bytes */ - OMX_U32 nTrack; /**< track number for the meta event */ - OMX_U32 nPosition; /**< Position of the meta-event in milliseconds */ -} OMX_AUDIO_CONFIG_MIDIMETAEVENTTYPE; - - -/** MIDI Meta Event Data structure - one per Meta Event. - * @ingroup midi - */ -typedef struct OMX_AUDIO_CONFIG_MIDIMETAEVENTDATATYPE{ - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nIndex; /**< Index of Meta Event */ - OMX_U32 nMetaEventSize; /**< size of the Meta Event in bytes */ - OMX_U8 nData[1]; /**< array of one or more bytes of meta data - as indicated by the nMetaEventSize field */ -} OMX_AUDIO_CONFIG__MIDIMETAEVENTDATATYPE; - - -/** Audio Volume adjustment for a port */ -typedef struct OMX_AUDIO_CONFIG_VOLUMETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port index indicating which port to - set. Select the input port to set - just that port's volume. Select the - output port to adjust the master - volume. */ - OMX_BOOL bLinear; /**< Is the volume to be set in linear (0.100) - or logarithmic scale (mB) */ - OMX_BS32 sVolume; /**< Volume linear setting in the 0..100 range, OR - Volume logarithmic setting for this port. The values - for volume are in mB (millibels = 1/100 dB) relative - to a gain of 1 (e.g. the output is the same as the - input level). Values are in mB from nMax - (maximum volume) to nMin mB (typically negative). - Since the volume is "voltage" - and not a "power", it takes a setting of - -600 mB to decrease the volume by 1/2. If - a component cannot accurately set the - volume to the requested value, it must - set the volume to the closest value BELOW - the requested value. When getting the - volume setting, the current actual volume - must be returned. */ -} OMX_AUDIO_CONFIG_VOLUMETYPE; - - -/** Audio Volume adjustment for a channel */ -typedef struct OMX_AUDIO_CONFIG_CHANNELVOLUMETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port index indicating which port to - set. Select the input port to set - just that port's volume. Select the - output port to adjust the master - volume. */ - OMX_U32 nChannel; /**< channel to select from 0 to N-1, - using OMX_ALL to apply volume settings - to all channels */ - OMX_BOOL bLinear; /**< Is the volume to be set in linear (0.100) or - logarithmic scale (mB) */ - OMX_BS32 sVolume; /**< Volume linear setting in the 0..100 range, OR - Volume logarithmic setting for this port. - The values for volume are in mB - (millibels = 1/100 dB) relative to a gain - of 1 (e.g. the output is the same as the - input level). Values are in mB from nMax - (maximum volume) to nMin mB (typically negative). - Since the volume is "voltage" - and not a "power", it takes a setting of - -600 mB to decrease the volume by 1/2. If - a component cannot accurately set the - volume to the requested value, it must - set the volume to the closest value BELOW - the requested value. When getting the - volume setting, the current actual volume - must be returned. */ - OMX_BOOL bIsMIDI; /**< TRUE if nChannel refers to a MIDI channel, - FALSE otherwise */ -} OMX_AUDIO_CONFIG_CHANNELVOLUMETYPE; - - -/** Audio balance setting */ -typedef struct OMX_AUDIO_CONFIG_BALANCETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port index indicating which port to - set. Select the input port to set - just that port's balance. Select the - output port to adjust the master - balance. */ - OMX_S32 nBalance; /**< balance setting for this port - (-100 to 100, where -100 indicates - all left, and no right */ -} OMX_AUDIO_CONFIG_BALANCETYPE; - - -/** Audio Port mute */ -typedef struct OMX_AUDIO_CONFIG_MUTETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port index indicating which port to - set. Select the input port to set - just that port's mute. Select the - output port to adjust the master - mute. */ - OMX_BOOL bMute; /**< Mute setting for this port */ -} OMX_AUDIO_CONFIG_MUTETYPE; - - -/** Audio Channel mute */ -typedef struct OMX_AUDIO_CONFIG_CHANNELMUTETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannel; /**< channel to select from 0 to N-1, - using OMX_ALL to apply mute settings - to all channels */ - OMX_BOOL bMute; /**< Mute setting for this channel */ - OMX_BOOL bIsMIDI; /**< TRUE if nChannel refers to a MIDI channel, - FALSE otherwise */ -} OMX_AUDIO_CONFIG_CHANNELMUTETYPE; - - - -/** Enable / Disable for loudness control, which boosts bass and to a - * smaller extent high end frequencies to compensate for hearing - * ability at the extreme ends of the audio spectrum - */ -typedef struct OMX_AUDIO_CONFIG_LOUDNESSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bLoudness; /**< Enable/disable for loudness */ -} OMX_AUDIO_CONFIG_LOUDNESSTYPE; - - -/** Enable / Disable for bass, which controls low frequencies - */ -typedef struct OMX_AUDIO_CONFIG_BASSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for bass control */ - OMX_S32 nBass; /**< bass setting for the port, as a - continuous value from -100 to 100 - (0 means no change in bass level)*/ -} OMX_AUDIO_CONFIG_BASSTYPE; - - -/** Enable / Disable for treble, which controls high frequencies tones - */ -typedef struct OMX_AUDIO_CONFIG_TREBLETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for treble control */ - OMX_S32 nTreble; /**< treble setting for the port, as a - continuous value from -100 to 100 - (0 means no change in treble level) */ -} OMX_AUDIO_CONFIG_TREBLETYPE; - - -/** An equalizer is typically used for two reasons: to compensate for an - * sub-optimal frequency response of a system to make it sound more natural - * or to create intentionally some unnatural coloring to the sound to create - * an effect. - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_EQUALIZERTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for equalizer */ - OMX_BU32 sBandIndex; /**< Band number to be set. Upper Limit is - N-1, where N is the number of bands, lower limit is 0 */ - OMX_BU32 sCenterFreq; /**< Center frequecies in Hz. This is a - read only element and is used to determine - the lower, center and upper frequency of - this band. */ - OMX_BS32 sBandLevel; /**< band level in millibels */ -} OMX_AUDIO_CONFIG_EQUALIZERTYPE; - - -/** Stereo widening mode type - * @ingroup effects - */ -typedef enum OMX_AUDIO_STEREOWIDENINGTYPE { - OMX_AUDIO_StereoWideningHeadphones, /**< Stereo widening for loudspeakers */ - OMX_AUDIO_StereoWideningLoudspeakers, /**< Stereo widening for closely spaced loudspeakers */ - OMX_AUDIO_StereoWideningKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_StereoWideningVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_StereoWideningMax = 0x7FFFFFFF -} OMX_AUDIO_STEREOWIDENINGTYPE; - - -/** Control for stereo widening, which is a special 2-channel - * case of the audio virtualizer effect. For example, for 5.1-channel - * output, it translates to virtual surround sound. - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_STEREOWIDENINGTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for stereo widening control */ - OMX_AUDIO_STEREOWIDENINGTYPE eWideningType; /**< Stereo widening algorithm type */ - OMX_U32 nStereoWidening; /**< stereo widening setting for the port, - as a continuous value from 0 to 100 */ -} OMX_AUDIO_CONFIG_STEREOWIDENINGTYPE; - - -/** The chorus effect (or ``choralizer'') is any signal processor which makes - * one sound source (such as a voice) sound like many such sources singing - * (or playing) in unison. Since performance in unison is never exact, chorus - * effects simulate this by making independently modified copies of the input - * signal. Modifications may include (1) delay, (2) frequency shift, and - * (3) amplitude modulation. - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_CHORUSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for chorus */ - OMX_BU32 sDelay; /**< average delay in milliseconds */ - OMX_BU32 sModulationRate; /**< rate of modulation in millihertz */ - OMX_U32 nModulationDepth; /**< depth of modulation as a percentage of - delay (i.e. 0 to 100) */ - OMX_BU32 nFeedback; /**< Feedback from chorus output to input in percentage */ -} OMX_AUDIO_CONFIG_CHORUSTYPE; - - -/** Reverberation is part of the reflected sound that follows the early - * reflections. In a typical room, this consists of a dense succession of - * echoes whose energy decays exponentially. The reverberation effect structure - * as defined here includes both (early) reflections as well as (late) reverberations. - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_REVERBERATIONTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bEnable; /**< Enable/disable for reverberation control */ - OMX_BS32 sRoomLevel; /**< Intensity level for the whole room effect - (i.e. both early reflections and late - reverberation) in millibels */ - OMX_BS32 sRoomHighFreqLevel; /**< Attenuation at high frequencies - relative to the intensity at low - frequencies in millibels */ - OMX_BS32 sReflectionsLevel; /**< Intensity level of early reflections - (relative to room value), in millibels */ - OMX_BU32 sReflectionsDelay; /**< Delay time of the first reflection relative - to the direct path, in milliseconds */ - OMX_BS32 sReverbLevel; /**< Intensity level of late reverberation - relative to room level, in millibels */ - OMX_BU32 sReverbDelay; /**< Time delay from the first early reflection - to the beginning of the late reverberation - section, in milliseconds */ - OMX_BU32 sDecayTime; /**< Late reverberation decay time at low - frequencies, in milliseconds */ - OMX_BU32 nDecayHighFreqRatio; /**< Ratio of high frequency decay time relative - to low frequency decay time in percent */ - OMX_U32 nDensity; /**< Modal density in the late reverberation decay, - in percent (i.e. 0 - 100) */ - OMX_U32 nDiffusion; /**< Echo density in the late reverberation decay, - in percent (i.e. 0 - 100) */ - OMX_BU32 sReferenceHighFreq; /**< Reference high frequency in Hertz. This is - the frequency used as the reference for all - the high-frequency settings above */ - -} OMX_AUDIO_CONFIG_REVERBERATIONTYPE; - - -/** Possible settings for the Echo Cancelation structure to use - * @ingroup effects - */ -typedef enum OMX_AUDIO_ECHOCANTYPE { - OMX_AUDIO_EchoCanOff = 0, /**< Echo Cancellation is disabled */ - OMX_AUDIO_EchoCanNormal, /**< Echo Cancellation normal operation - - echo from plastics and face */ - OMX_AUDIO_EchoCanHFree, /**< Echo Cancellation optimized for - Hands Free operation */ - OMX_AUDIO_EchoCanCarKit, /**< Echo Cancellation optimized for - Car Kit (longer echo) */ - OMX_AUDIO_EchoCanKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_AUDIO_EchoCanVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_AUDIO_EchoCanMax = 0x7FFFFFFF -} OMX_AUDIO_ECHOCANTYPE; - - -/** Enable / Disable for echo cancelation, which removes undesired echo's - * from the audio - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_ECHOCANCELATIONTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_AUDIO_ECHOCANTYPE eEchoCancelation; /**< Echo cancelation settings */ -} OMX_AUDIO_CONFIG_ECHOCANCELATIONTYPE; - - -/** Enable / Disable for noise reduction, which undesired noise from - * the audio - * @ingroup effects - */ -typedef struct OMX_AUDIO_CONFIG_NOISEREDUCTIONTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BOOL bNoiseReduction; /**< Enable/disable for noise reduction */ -} OMX_AUDIO_CONFIG_NOISEREDUCTIONTYPE; - -/** @} */ - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_AudioExt.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_AudioExt.h deleted file mode 100644 index 2a1c3f2e1..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_AudioExt.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright (c) 2010 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_AudioExt.h - OpenMax IL version 1.1.2 - * The OMX_AudioExt header file contains extensions to the - * definitions used by both the application and the component to - * access video items. - */ - -#ifndef OMX_AudioExt_h -#define OMX_AudioExt_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/* Each OMX header shall include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ -#include - -#define OMX_AUDIO_AACToolAndroidSSBR (OMX_AUDIO_AACToolVendor << 0) /**< SSBR: MPEG-4 Single-rate (downsampled) Spectral Band Replication tool allowed or active */ -#define OMX_AUDIO_AACToolAndroidDSBR (OMX_AUDIO_AACToolVendor << 1) /**< DSBR: MPEG-4 Dual-rate Spectral Band Replication tool allowed or active */ - -typedef enum OMX_AUDIO_CODINGEXTTYPE { - OMX_AUDIO_CodingAndroidUnused = OMX_AUDIO_CodingKhronosExtensions + 0x00100000, - OMX_AUDIO_CodingAndroidAC3, /**< AC3 encoded data */ - OMX_AUDIO_CodingAndroidOPUS, /**< OPUS encoded data */ - OMX_AUDIO_CodingAndroidEAC3, /**< EAC3 encoded data */ -} OMX_AUDIO_CODINGEXTTYPE; - -typedef struct OMX_AUDIO_PARAM_ANDROID_AC3TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ -} OMX_AUDIO_PARAM_ANDROID_AC3TYPE; - -typedef struct OMX_AUDIO_PARAM_ANDROID_EAC3TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ -} OMX_AUDIO_PARAM_ANDROID_EAC3TYPE; - -typedef struct OMX_AUDIO_PARAM_ANDROID_OPUSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nChannels; /**< Number of channels */ - OMX_U32 nBitRate; /**< Bit rate of the encoded data data. Use 0 for variable - rate or unknown bit rates. Encoding is set to the - bitrate closest to specified value (in bps) */ - OMX_U32 nSampleRate; /**< Sampling rate of the source data. Use 0 for - variable or unknown sampling rate. */ - OMX_U32 nAudioBandWidth; /**< Audio band width (in Hz) to which an encoder should - limit the audio signal. Use 0 to let encoder decide */ -} OMX_AUDIO_PARAM_ANDROID_OPUSTYPE; - -typedef struct OMX_AUDIO_PARAM_ANDROID_AACPRESENTATIONTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_S32 nMaxOutputChannels; /**< Maximum channel count to be output, -1 if unspecified, 0 if downmixing disabled */ - OMX_S32 nDrcCut; /**< The DRC attenuation factor, between 0 and 127, -1 if unspecified */ - OMX_S32 nDrcBoost; /**< The DRC amplification factor, between 0 and 127, -1 if unspecified */ - OMX_S32 nHeavyCompression; /**< 0 for light compression, 1 for heavy compression, -1 if unspecified */ - OMX_S32 nTargetReferenceLevel; /**< Target reference level, between 0 and 127, -1 if unspecified */ - OMX_S32 nEncodedTargetLevel; /**< Target reference level assumed at the encoder, between 0 and 127, -1 if unspecified */ - OMX_S32 nPCMLimiterEnable; /**< Signal level limiting, 0 for disable, 1 for enable, -1 if unspecified */ -} OMX_AUDIO_PARAM_ANDROID_AACPRESENTATIONTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif /* OMX_AudioExt_h */ -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Component.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Component.h deleted file mode 100644 index 0dc2c7697..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Component.h +++ /dev/null @@ -1,596 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_Component.h - OpenMax IL version 1.1.2 - * The OMX_Component header file contains the definitions used to define - * the public interface of a component. This header file is intended to - * be used by both the application and the component. - */ - -#ifndef OMX_Component_h -#define OMX_Component_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - - - -/* Each OMX header must include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ - -#include -#include -#include -#include - -/** @ingroup comp */ -typedef enum OMX_PORTDOMAINTYPE { - OMX_PortDomainAudio, - OMX_PortDomainVideo, - OMX_PortDomainImage, - OMX_PortDomainOther, - OMX_PortDomainKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_PortDomainVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_PortDomainMax = 0x7ffffff -} OMX_PORTDOMAINTYPE; - -/** @ingroup comp */ -typedef struct OMX_PARAM_PORTDEFINITIONTYPE { - OMX_U32 nSize; /**< Size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Port number the structure applies to */ - OMX_DIRTYPE eDir; /**< Direction (input or output) of this port */ - OMX_U32 nBufferCountActual; /**< The actual number of buffers allocated on this port */ - OMX_U32 nBufferCountMin; /**< The minimum number of buffers this port requires */ - OMX_U32 nBufferSize; /**< Size, in bytes, for buffers to be used for this channel */ - OMX_BOOL bEnabled; /**< Ports default to enabled and are enabled/disabled by - OMX_CommandPortEnable/OMX_CommandPortDisable. - When disabled a port is unpopulated. A disabled port - is not populated with buffers on a transition to IDLE. */ - OMX_BOOL bPopulated; /**< Port is populated with all of its buffers as indicated by - nBufferCountActual. A disabled port is always unpopulated. - An enabled port is populated on a transition to OMX_StateIdle - and unpopulated on a transition to loaded. */ - OMX_PORTDOMAINTYPE eDomain; /**< Domain of the port. Determines the contents of metadata below. */ - union { - OMX_AUDIO_PORTDEFINITIONTYPE audio; - OMX_VIDEO_PORTDEFINITIONTYPE video; - OMX_IMAGE_PORTDEFINITIONTYPE image; - OMX_OTHER_PORTDEFINITIONTYPE other; - } format; - OMX_BOOL bBuffersContiguous; - OMX_U32 nBufferAlignment; -} OMX_PARAM_PORTDEFINITIONTYPE; - -/** @ingroup comp */ -typedef struct OMX_PARAM_U32TYPE { - OMX_U32 nSize; /**< Size of this structure, in Bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_U32 nU32; /**< U32 value */ -} OMX_PARAM_U32TYPE; - -/** @ingroup rpm */ -typedef enum OMX_SUSPENSIONPOLICYTYPE { - OMX_SuspensionDisabled, /**< No suspension; v1.0 behavior */ - OMX_SuspensionEnabled, /**< Suspension allowed */ - OMX_SuspensionPolicyKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_SuspensionPolicyStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_SuspensionPolicyMax = 0x7fffffff -} OMX_SUSPENSIONPOLICYTYPE; - -/** @ingroup rpm */ -typedef struct OMX_PARAM_SUSPENSIONPOLICYTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_SUSPENSIONPOLICYTYPE ePolicy; -} OMX_PARAM_SUSPENSIONPOLICYTYPE; - -/** @ingroup rpm */ -typedef enum OMX_SUSPENSIONTYPE { - OMX_NotSuspended, /**< component is not suspended */ - OMX_Suspended, /**< component is suspended */ - OMX_SuspensionKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_SuspensionVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_SuspendMax = 0x7FFFFFFF -} OMX_SUSPENSIONTYPE; - -/** @ingroup rpm */ -typedef struct OMX_PARAM_SUSPENSIONTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_SUSPENSIONTYPE eType; -} OMX_PARAM_SUSPENSIONTYPE ; - -typedef struct OMX_CONFIG_BOOLEANTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_BOOL bEnabled; -} OMX_CONFIG_BOOLEANTYPE; - -/* Parameter specifying the content uri to use. */ -/** @ingroup cp */ -typedef struct OMX_PARAM_CONTENTURITYPE -{ - OMX_U32 nSize; /**< size of the structure in bytes, including - actual URI name */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U8 contentURI[1]; /**< The URI name */ -} OMX_PARAM_CONTENTURITYPE; - -/* Parameter specifying the pipe to use. */ -/** @ingroup cp */ -typedef struct OMX_PARAM_CONTENTPIPETYPE -{ - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_HANDLETYPE hPipe; /**< The pipe handle*/ -} OMX_PARAM_CONTENTPIPETYPE; - -/** @ingroup rpm */ -typedef struct OMX_RESOURCECONCEALMENTTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_BOOL bResourceConcealmentForbidden; /**< disallow the use of resource concealment - methods (like degrading algorithm quality to - lower resource consumption or functional bypass) - on a component as a resolution to resource conflicts. */ -} OMX_RESOURCECONCEALMENTTYPE; - - -/** @ingroup metadata */ -typedef enum OMX_METADATACHARSETTYPE { - OMX_MetadataCharsetUnknown = 0, - OMX_MetadataCharsetASCII, - OMX_MetadataCharsetBinary, - OMX_MetadataCharsetCodePage1252, - OMX_MetadataCharsetUTF8, - OMX_MetadataCharsetJavaConformantUTF8, - OMX_MetadataCharsetUTF7, - OMX_MetadataCharsetImapUTF7, - OMX_MetadataCharsetUTF16LE, - OMX_MetadataCharsetUTF16BE, - OMX_MetadataCharsetGB12345, - OMX_MetadataCharsetHZGB2312, - OMX_MetadataCharsetGB2312, - OMX_MetadataCharsetGB18030, - OMX_MetadataCharsetGBK, - OMX_MetadataCharsetBig5, - OMX_MetadataCharsetISO88591, - OMX_MetadataCharsetISO88592, - OMX_MetadataCharsetISO88593, - OMX_MetadataCharsetISO88594, - OMX_MetadataCharsetISO88595, - OMX_MetadataCharsetISO88596, - OMX_MetadataCharsetISO88597, - OMX_MetadataCharsetISO88598, - OMX_MetadataCharsetISO88599, - OMX_MetadataCharsetISO885910, - OMX_MetadataCharsetISO885913, - OMX_MetadataCharsetISO885914, - OMX_MetadataCharsetISO885915, - OMX_MetadataCharsetShiftJIS, - OMX_MetadataCharsetISO2022JP, - OMX_MetadataCharsetISO2022JP1, - OMX_MetadataCharsetISOEUCJP, - OMX_MetadataCharsetSMS7Bit, - OMX_MetadataCharsetKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_MetadataCharsetVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_MetadataCharsetTypeMax= 0x7FFFFFFF -} OMX_METADATACHARSETTYPE; - -/** @ingroup metadata */ -typedef enum OMX_METADATASCOPETYPE -{ - OMX_MetadataScopeAllLevels, - OMX_MetadataScopeTopLevel, - OMX_MetadataScopePortLevel, - OMX_MetadataScopeNodeLevel, - OMX_MetadataScopeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_MetadataScopeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_MetadataScopeTypeMax = 0x7fffffff -} OMX_METADATASCOPETYPE; - -/** @ingroup metadata */ -typedef enum OMX_METADATASEARCHMODETYPE -{ - OMX_MetadataSearchValueSizeByIndex, - OMX_MetadataSearchItemByIndex, - OMX_MetadataSearchNextItemByKey, - OMX_MetadataSearchKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_MetadataSearchVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_MetadataSearchTypeMax = 0x7fffffff -} OMX_METADATASEARCHMODETYPE; -/** @ingroup metadata */ -typedef struct OMX_CONFIG_METADATAITEMCOUNTTYPE -{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_METADATASCOPETYPE eScopeMode; - OMX_U32 nScopeSpecifier; - OMX_U32 nMetadataItemCount; -} OMX_CONFIG_METADATAITEMCOUNTTYPE; - -/** @ingroup metadata */ -typedef struct OMX_CONFIG_METADATAITEMTYPE -{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_METADATASCOPETYPE eScopeMode; - OMX_U32 nScopeSpecifier; - OMX_U32 nMetadataItemIndex; - OMX_METADATASEARCHMODETYPE eSearchMode; - OMX_METADATACHARSETTYPE eKeyCharset; - OMX_U8 nKeySizeUsed; - OMX_U8 nKey[128]; - OMX_METADATACHARSETTYPE eValueCharset; - OMX_STRING sLanguageCountry; - OMX_U32 nValueMaxSize; - OMX_U32 nValueSizeUsed; - OMX_U8 nValue[1]; -} OMX_CONFIG_METADATAITEMTYPE; - -/* @ingroup metadata */ -typedef struct OMX_CONFIG_CONTAINERNODECOUNTTYPE -{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_BOOL bAllKeys; - OMX_U32 nParentNodeID; - OMX_U32 nNumNodes; -} OMX_CONFIG_CONTAINERNODECOUNTTYPE; - -/** @ingroup metadata */ -typedef struct OMX_CONFIG_CONTAINERNODEIDTYPE -{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_BOOL bAllKeys; - OMX_U32 nParentNodeID; - OMX_U32 nNodeIndex; - OMX_U32 nNodeID; - OMX_STRING cNodeName; - OMX_BOOL bIsLeafType; -} OMX_CONFIG_CONTAINERNODEIDTYPE; - -/** @ingroup metadata */ -typedef struct OMX_PARAM_METADATAFILTERTYPE -{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_BOOL bAllKeys; /* if true then this structure refers to all keys and - * the three key fields below are ignored */ - OMX_METADATACHARSETTYPE eKeyCharset; - OMX_U32 nKeySizeUsed; - OMX_U8 nKey [128]; - OMX_U32 nLanguageCountrySizeUsed; - OMX_U8 nLanguageCountry[128]; - OMX_BOOL bEnabled; /* if true then key is part of filter (e.g. - * retained for query later). If false then - * key is not part of filter */ -} OMX_PARAM_METADATAFILTERTYPE; - -/** The OMX_HANDLETYPE structure defines the component handle. The component - * handle is used to access all of the component's public methods and also - * contains pointers to the component's private data area. The component - * handle is initialized by the OMX core (with help from the component) - * during the process of loading the component. After the component is - * successfully loaded, the application can safely access any of the - * component's public functions (although some may return an error because - * the state is inappropriate for the access). - * - * @ingroup comp - */ -typedef struct OMX_COMPONENTTYPE -{ - /** The size of this structure, in bytes. It is the responsibility - of the allocator of this structure to fill in this value. Since - this structure is allocated by the GetHandle function, this - function will fill in this value. */ - OMX_U32 nSize; - - /** nVersion is the version of the OMX specification that the structure - is built against. It is the responsibility of the creator of this - structure to initialize this value and every user of this structure - should verify that it knows how to use the exact version of - this structure found herein. */ - OMX_VERSIONTYPE nVersion; - - /** pComponentPrivate is a pointer to the component private data area. - This member is allocated and initialized by the component when the - component is first loaded. The application should not access this - data area. */ - OMX_PTR pComponentPrivate; - - /** pApplicationPrivate is a pointer that is a parameter to the - OMX_GetHandle method, and contains an application private value - provided by the IL client. This application private data is - returned to the IL Client by OMX in all callbacks */ - OMX_PTR pApplicationPrivate; - - /** refer to OMX_GetComponentVersion in OMX_core.h or the OMX IL - specification for details on the GetComponentVersion method. - */ - OMX_ERRORTYPE (*GetComponentVersion)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_OUT OMX_STRING pComponentName, - OMX_OUT OMX_VERSIONTYPE* pComponentVersion, - OMX_OUT OMX_VERSIONTYPE* pSpecVersion, - OMX_OUT OMX_UUIDTYPE* pComponentUUID); - - /** refer to OMX_SendCommand in OMX_core.h or the OMX IL - specification for details on the SendCommand method. - */ - OMX_ERRORTYPE (*SendCommand)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_COMMANDTYPE Cmd, - OMX_IN OMX_U32 nParam1, - OMX_IN OMX_PTR pCmdData); - - /** refer to OMX_GetParameter in OMX_core.h or the OMX IL - specification for details on the GetParameter method. - */ - OMX_ERRORTYPE (*GetParameter)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_INDEXTYPE nParamIndex, - OMX_INOUT OMX_PTR pComponentParameterStructure); - - - /** refer to OMX_SetParameter in OMX_core.h or the OMX IL - specification for details on the SetParameter method. - */ - OMX_ERRORTYPE (*SetParameter)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_INDEXTYPE nIndex, - OMX_IN OMX_PTR pComponentParameterStructure); - - - /** refer to OMX_GetConfig in OMX_core.h or the OMX IL - specification for details on the GetConfig method. - */ - OMX_ERRORTYPE (*GetConfig)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_INDEXTYPE nIndex, - OMX_INOUT OMX_PTR pComponentConfigStructure); - - - /** refer to OMX_SetConfig in OMX_core.h or the OMX IL - specification for details on the SetConfig method. - */ - OMX_ERRORTYPE (*SetConfig)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_INDEXTYPE nIndex, - OMX_IN OMX_PTR pComponentConfigStructure); - - - /** refer to OMX_GetExtensionIndex in OMX_core.h or the OMX IL - specification for details on the GetExtensionIndex method. - */ - OMX_ERRORTYPE (*GetExtensionIndex)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_STRING cParameterName, - OMX_OUT OMX_INDEXTYPE* pIndexType); - - - /** refer to OMX_GetState in OMX_core.h or the OMX IL - specification for details on the GetState method. - */ - OMX_ERRORTYPE (*GetState)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_OUT OMX_STATETYPE* pState); - - - /** The ComponentTunnelRequest method will interact with another OMX - component to determine if tunneling is possible and to setup the - tunneling. The return codes for this method can be used to - determine if tunneling is not possible, or if tunneling is not - supported. - - Base profile components (i.e. non-interop) do not support this - method and should return OMX_ErrorNotImplemented - - The interop profile component MUST support tunneling to another - interop profile component with a compatible port parameters. - A component may also support proprietary communication. - - If proprietary communication is supported the negotiation of - proprietary communication is done outside of OMX in a vendor - specific way. It is only required that the proper result be - returned and the details of how the setup is done is left - to the component implementation. - - When this method is invoked when nPort in an output port, the - component will: - 1. Populate the pTunnelSetup structure with the output port's - requirements and constraints for the tunnel. - - When this method is invoked when nPort in an input port, the - component will: - 1. Query the necessary parameters from the output port to - determine if the ports are compatible for tunneling - 2. If the ports are compatible, the component should store - the tunnel step provided by the output port - 3. Determine which port (either input or output) is the buffer - supplier, and call OMX_SetParameter on the output port to - indicate this selection. - - The component will return from this call within 5 msec. - - @param [in] hComp - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle method. - @param [in] nPort - nPort is used to select the port on the component to be used - for tunneling. - @param [in] hTunneledComp - Handle of the component to tunnel with. This is the component - handle returned by the call to the OMX_GetHandle method. When - this parameter is 0x0 the component should setup the port for - communication with the application / IL Client. - @param [in] nPortOutput - nPortOutput is used indicate the port the component should - tunnel with. - @param [in] pTunnelSetup - Pointer to the tunnel setup structure. When nPort is an output port - the component should populate the fields of this structure. When - When nPort is an input port the component should review the setup - provided by the component with the output port. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup tun - */ - - OMX_ERRORTYPE (*ComponentTunnelRequest)( - OMX_IN OMX_HANDLETYPE hComp, - OMX_IN OMX_U32 nPort, - OMX_IN OMX_HANDLETYPE hTunneledComp, - OMX_IN OMX_U32 nTunneledPort, - OMX_INOUT OMX_TUNNELSETUPTYPE* pTunnelSetup); - - /** refer to OMX_UseBuffer in OMX_core.h or the OMX IL - specification for details on the UseBuffer method. - @ingroup buf - */ - OMX_ERRORTYPE (*UseBuffer)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_INOUT OMX_BUFFERHEADERTYPE** ppBufferHdr, - OMX_IN OMX_U32 nPortIndex, - OMX_IN OMX_PTR pAppPrivate, - OMX_IN OMX_U32 nSizeBytes, - OMX_IN OMX_U8* pBuffer); - - /** refer to OMX_AllocateBuffer in OMX_core.h or the OMX IL - specification for details on the AllocateBuffer method. - @ingroup buf - */ - OMX_ERRORTYPE (*AllocateBuffer)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_INOUT OMX_BUFFERHEADERTYPE** ppBuffer, - OMX_IN OMX_U32 nPortIndex, - OMX_IN OMX_PTR pAppPrivate, - OMX_IN OMX_U32 nSizeBytes); - - /** refer to OMX_FreeBuffer in OMX_core.h or the OMX IL - specification for details on the FreeBuffer method. - @ingroup buf - */ - OMX_ERRORTYPE (*FreeBuffer)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_U32 nPortIndex, - OMX_IN OMX_BUFFERHEADERTYPE* pBuffer); - - /** refer to OMX_EmptyThisBuffer in OMX_core.h or the OMX IL - specification for details on the EmptyThisBuffer method. - @ingroup buf - */ - OMX_ERRORTYPE (*EmptyThisBuffer)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_BUFFERHEADERTYPE* pBuffer); - - /** refer to OMX_FillThisBuffer in OMX_core.h or the OMX IL - specification for details on the FillThisBuffer method. - @ingroup buf - */ - OMX_ERRORTYPE (*FillThisBuffer)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_BUFFERHEADERTYPE* pBuffer); - - /** The SetCallbacks method is used by the core to specify the callback - structure from the application to the component. This is a blocking - call. The component will return from this call within 5 msec. - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the GetHandle function. - @param [in] pCallbacks - pointer to an OMX_CALLBACKTYPE structure used to provide the - callback information to the component - @param [in] pAppData - pointer to an application defined value. It is anticipated that - the application will pass a pointer to a data structure or a "this - pointer" in this area to allow the callback (in the application) - to determine the context of the call - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - */ - OMX_ERRORTYPE (*SetCallbacks)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_CALLBACKTYPE* pCallbacks, - OMX_IN OMX_PTR pAppData); - - /** ComponentDeInit method is used to deinitialize the component - providing a means to free any resources allocated at component - initialization. NOTE: After this call the component handle is - not valid for further use. - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the GetHandle function. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - */ - OMX_ERRORTYPE (*ComponentDeInit)( - OMX_IN OMX_HANDLETYPE hComponent); - - /** @ingroup buf */ - OMX_ERRORTYPE (*UseEGLImage)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_INOUT OMX_BUFFERHEADERTYPE** ppBufferHdr, - OMX_IN OMX_U32 nPortIndex, - OMX_IN OMX_PTR pAppPrivate, - OMX_IN void* eglImage); - - OMX_ERRORTYPE (*ComponentRoleEnum)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_OUT OMX_U8 *cRole, - OMX_IN OMX_U32 nIndex); - -} OMX_COMPONENTTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_ContentPipe.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_ContentPipe.h deleted file mode 100644 index 0224c8a2e..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_ContentPipe.h +++ /dev/null @@ -1,212 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_ContentPipe.h - OpenMax IL version 1.1.2 - * The OMX_ContentPipe header file contains the definitions used to define - * the public interface for content piples. This header file is intended to - * be used by the component. - */ - -#ifndef OMX_CONTENTPIPE_H -#define OMX_CONTENTPIPE_H - -#ifndef KD_EACCES -/* OpenKODE error codes. CPResult values may be zero (indicating success - or one of the following values) */ -#define KD_EACCES (1) -#define KD_EADDRINUSE (2) -#define KD_EAGAIN (5) -#define KD_EBADF (7) -#define KD_EBUSY (8) -#define KD_ECONNREFUSED (9) -#define KD_ECONNRESET (10) -#define KD_EDEADLK (11) -#define KD_EDESTADDRREQ (12) -#define KD_ERANGE (35) -#define KD_EEXIST (13) -#define KD_EFBIG (14) -#define KD_EHOSTUNREACH (15) -#define KD_EINVAL (17) -#define KD_EIO (18) -#define KD_EISCONN (20) -#define KD_EISDIR (21) -#define KD_EMFILE (22) -#define KD_ENAMETOOLONG (23) -#define KD_ENOENT (24) -#define KD_ENOMEM (25) -#define KD_ENOSPC (26) -#define KD_ENOSYS (27) -#define KD_ENOTCONN (28) -#define KD_EPERM (33) -#define KD_ETIMEDOUT (36) -#define KD_EILSEQ (19) -#endif - -/** Map types from OMX standard types only here so interface is as generic as possible. */ -typedef OMX_U32 CPresult; -typedef char * CPstring; -typedef void * CPhandle; -typedef OMX_U32 CPuint; -typedef OMX_S32 CPint; -typedef char CPbyte; -typedef OMX_BOOL CPbool; - -/** enumeration of origin types used in the CP_PIPETYPE's Seek function - * @ingroup cp - */ -typedef enum CP_ORIGINTYPE { - CP_OriginBegin, - CP_OriginCur, - CP_OriginEnd, - CP_OriginKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - CP_OriginVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - CP_OriginMax = 0X7FFFFFFF -} CP_ORIGINTYPE; - -/** enumeration of contact access types used in the CP_PIPETYPE's Open function - * @ingroup cp - */ -typedef enum CP_ACCESSTYPE { - CP_AccessRead, - CP_AccessWrite, - CP_AccessReadWrite, - CP_AccessKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - CP_AccessVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - CP_AccessMax = 0X7FFFFFFF -} CP_ACCESSTYPE; - -/** enumeration of results returned by the CP_PIPETYPE's CheckAvailableBytes function - * @ingroup cp - */ -typedef enum CP_CHECKBYTESRESULTTYPE -{ - CP_CheckBytesOk, /**< There are at least the request number - of bytes available */ - CP_CheckBytesNotReady, /**< The pipe is still retrieving bytes - and presently lacks sufficient bytes. - Client will be called when they are - sufficient bytes are available. */ - CP_CheckBytesInsufficientBytes, /**< The pipe has retrieved all bytes - but those available are less than those - requested */ - CP_CheckBytesAtEndOfStream, /**< The pipe has reached the end of stream - and no more bytes are available. */ - CP_CheckBytesOutOfBuffers, /**< All read/write buffers are currently in use. */ - CP_CheckBytesKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - CP_CheckBytesVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - CP_CheckBytesMax = 0X7FFFFFFF -} CP_CHECKBYTESRESULTTYPE; - -/** enumeration of content pipe events sent to the client callback. - * @ingroup cp - */ -typedef enum CP_EVENTTYPE{ - CP_BytesAvailable, /** bytes requested in a CheckAvailableBytes call are now available*/ - CP_Overflow, /** enumeration of content pipe events sent to the client callback*/ - CP_PipeDisconnected, /** enumeration of content pipe events sent to the client callback*/ - CP_EventKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - CP_EventVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - CP_EventMax = 0X7FFFFFFF -} CP_EVENTTYPE; - -/** content pipe definition - * @ingroup cp - */ -typedef struct CP_PIPETYPE -{ - /** Open a content stream for reading or writing. */ - CPresult (*Open)( CPhandle* hContent, CPstring szURI, CP_ACCESSTYPE eAccess ); - - /** Close a content stream. */ - CPresult (*Close)( CPhandle hContent ); - - /** Create a content source and open it for writing. */ - CPresult (*Create)( CPhandle *hContent, CPstring szURI ); - - /** Check the that specified number of bytes are available for reading or writing (depending on access type).*/ - CPresult (*CheckAvailableBytes)( CPhandle hContent, CPuint nBytesRequested, CP_CHECKBYTESRESULTTYPE *eResult ); - - /** Seek to certain position in the content relative to the specified origin. */ - CPresult (*SetPosition)( CPhandle hContent, CPint nOffset, CP_ORIGINTYPE eOrigin); - - /** Retrieve the current position relative to the start of the content. */ - CPresult (*GetPosition)( CPhandle hContent, CPuint *pPosition); - - /** Retrieve data of the specified size from the content stream (advance content pointer by size of data). - Note: pipe client provides pointer. This function is appropriate for small high frequency reads. */ - CPresult (*Read)( CPhandle hContent, CPbyte *pData, CPuint nSize); - - /** Retrieve a buffer allocated by the pipe that contains the requested number of bytes. - Buffer contains the next block of bytes, as specified by nSize, of the content. nSize also - returns the size of the block actually read. Content pointer advances the by the returned size. - Note: pipe provides pointer. This function is appropriate for large reads. The client must call - ReleaseReadBuffer when done with buffer. - - In some cases the requested block may not reside in contiguous memory within the - pipe implementation. For instance if the pipe leverages a circular buffer then the requested - block may straddle the boundary of the circular buffer. By default a pipe implementation - performs a copy in this case to provide the block to the pipe client in one contiguous buffer. - If, however, the client sets bForbidCopy, then the pipe returns only those bytes preceding the memory - boundary. Here the client may retrieve the data in segments over successive calls. */ - CPresult (*ReadBuffer)( CPhandle hContent, CPbyte **ppBuffer, CPuint *nSize, CPbool bForbidCopy); - - /** Release a buffer obtained by ReadBuffer back to the pipe. */ - CPresult (*ReleaseReadBuffer)(CPhandle hContent, CPbyte *pBuffer); - - /** Write data of the specified size to the content (advance content pointer by size of data). - Note: pipe client provides pointer. This function is appropriate for small high frequency writes. */ - CPresult (*Write)( CPhandle hContent, CPbyte *data, CPuint nSize); - - /** Retrieve a buffer allocated by the pipe used to write data to the content. - Client will fill buffer with output data. Note: pipe provides pointer. This function is appropriate - for large writes. The client must call WriteBuffer when done it has filled the buffer with data.*/ - CPresult (*GetWriteBuffer)( CPhandle hContent, CPbyte **ppBuffer, CPuint nSize); - - /** Deliver a buffer obtained via GetWriteBuffer to the pipe. Pipe will write the - the contents of the buffer to content and advance content pointer by the size of the buffer */ - CPresult (*WriteBuffer)( CPhandle hContent, CPbyte *pBuffer, CPuint nFilledSize); - - /** Register a per-handle client callback with the content pipe. */ - CPresult (*RegisterCallback)( CPhandle hContent, CPresult (*ClientCallback)(CP_EVENTTYPE eEvent, CPuint iParam)); - -} CP_PIPETYPE; - -#endif - diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Core.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Core.h deleted file mode 100644 index f746a69d2..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Core.h +++ /dev/null @@ -1,1464 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_Core.h - OpenMax IL version 1.1.2 - * The OMX_Core header file contains the definitions used by both the - * application and the component to access common items. - */ - -#ifndef OMX_Core_h -#define OMX_Core_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - - -/* Each OMX header shall include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ - -#include - - -/** The OMX_COMMANDTYPE enumeration is used to specify the action in the - * OMX_SendCommand macro. - * @ingroup core - */ -typedef enum OMX_COMMANDTYPE -{ - OMX_CommandStateSet, /**< Change the component state */ - OMX_CommandFlush, /**< Flush the data queue(s) of a component */ - OMX_CommandPortDisable, /**< Disable a port on a component. */ - OMX_CommandPortEnable, /**< Enable a port on a component. */ - OMX_CommandMarkBuffer, /**< Mark a component/buffer for observation */ - OMX_CommandKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_CommandVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_CommandMax = 0X7FFFFFFF -} OMX_COMMANDTYPE; - - - -/** The OMX_STATETYPE enumeration is used to indicate or change the component - * state. This enumeration reflects the current state of the component when - * used with the OMX_GetState macro or becomes the parameter in a state change - * command when used with the OMX_SendCommand macro. - * - * The component will be in the Loaded state after the component is initially - * loaded into memory. In the Loaded state, the component is not allowed to - * allocate or hold resources other than to build it's internal parameter - * and configuration tables. The application will send one or more - * SetParameters/GetParameters and SetConfig/GetConfig commands to the - * component and the component will record each of these parameter and - * configuration changes for use later. When the application sends the - * Idle command, the component will acquire the resources needed for the - * specified configuration and will transition to the idle state if the - * allocation is successful. If the component cannot successfully - * transition to the idle state for any reason, the state of the component - * shall be fully rolled back to the Loaded state (e.g. all allocated - * resources shall be released). When the component receives the command - * to go to the Executing state, it shall begin processing buffers by - * sending all input buffers it holds to the application. While - * the component is in the Idle state, the application may also send the - * Pause command. If the component receives the pause command while in the - * Idle state, the component shall send all input buffers it holds to the - * application, but shall not begin processing buffers. This will allow the - * application to prefill buffers. - * - * @ingroup comp - */ - -typedef enum OMX_STATETYPE -{ - OMX_StateInvalid, /**< component has detected that it's internal data - structures are corrupted to the point that - it cannot determine it's state properly */ - OMX_StateLoaded, /**< component has been loaded but has not completed - initialization. The OMX_SetParameter macro - and the OMX_GetParameter macro are the only - valid macros allowed to be sent to the - component in this state. */ - OMX_StateIdle, /**< component initialization has been completed - successfully and the component is ready to - to start. */ - OMX_StateExecuting, /**< component has accepted the start command and - is processing data (if data is available) */ - OMX_StatePause, /**< component has received pause command */ - OMX_StateWaitForResources, /**< component is waiting for resources, either after - preemption or before it gets the resources requested. - See specification for complete details. */ - OMX_StateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_StateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_StateMax = 0X7FFFFFFF -} OMX_STATETYPE; - -/** The OMX_ERRORTYPE enumeration defines the standard OMX Errors. These - * errors should cover most of the common failure cases. However, - * vendors are free to add additional error messages of their own as - * long as they follow these rules: - * 1. Vendor error messages shall be in the range of 0x90000000 to - * 0x9000FFFF. - * 2. Vendor error messages shall be defined in a header file provided - * with the component. No error messages are allowed that are - * not defined. - */ -typedef enum OMX_ERRORTYPE -{ - OMX_ErrorNone = 0, - - /** There were insufficient resources to perform the requested operation */ - OMX_ErrorInsufficientResources = (OMX_S32) 0x80001000, - - /** There was an error, but the cause of the error could not be determined */ - OMX_ErrorUndefined = (OMX_S32) 0x80001001, - - /** The component name string was not valid */ - OMX_ErrorInvalidComponentName = (OMX_S32) 0x80001002, - - /** No component with the specified name string was found */ - OMX_ErrorComponentNotFound = (OMX_S32) 0x80001003, - - /** The component specified did not have a "OMX_ComponentInit" or - "OMX_ComponentDeInit entry point */ - OMX_ErrorInvalidComponent = (OMX_S32) 0x80001004, - - /** One or more parameters were not valid */ - OMX_ErrorBadParameter = (OMX_S32) 0x80001005, - - /** The requested function is not implemented */ - OMX_ErrorNotImplemented = (OMX_S32) 0x80001006, - - /** The buffer was emptied before the next buffer was ready */ - OMX_ErrorUnderflow = (OMX_S32) 0x80001007, - - /** The buffer was not available when it was needed */ - OMX_ErrorOverflow = (OMX_S32) 0x80001008, - - /** The hardware failed to respond as expected */ - OMX_ErrorHardware = (OMX_S32) 0x80001009, - - /** The component is in the state OMX_StateInvalid */ - OMX_ErrorInvalidState = (OMX_S32) 0x8000100A, - - /** Stream is found to be corrupt */ - OMX_ErrorStreamCorrupt = (OMX_S32) 0x8000100B, - - /** Ports being connected are not compatible */ - OMX_ErrorPortsNotCompatible = (OMX_S32) 0x8000100C, - - /** Resources allocated to an idle component have been - lost resulting in the component returning to the loaded state */ - OMX_ErrorResourcesLost = (OMX_S32) 0x8000100D, - - /** No more indicies can be enumerated */ - OMX_ErrorNoMore = (OMX_S32) 0x8000100E, - - /** The component detected a version mismatch */ - OMX_ErrorVersionMismatch = (OMX_S32) 0x8000100F, - - /** The component is not ready to return data at this time */ - OMX_ErrorNotReady = (OMX_S32) 0x80001010, - - /** There was a timeout that occurred */ - OMX_ErrorTimeout = (OMX_S32) 0x80001011, - - /** This error occurs when trying to transition into the state you are already in */ - OMX_ErrorSameState = (OMX_S32) 0x80001012, - - /** Resources allocated to an executing or paused component have been - preempted, causing the component to return to the idle state */ - OMX_ErrorResourcesPreempted = (OMX_S32) 0x80001013, - - /** A non-supplier port sends this error to the IL client (via the EventHandler callback) - during the allocation of buffers (on a transition from the LOADED to the IDLE state or - on a port restart) when it deems that it has waited an unusually long time for the supplier - to send it an allocated buffer via a UseBuffer call. */ - OMX_ErrorPortUnresponsiveDuringAllocation = (OMX_S32) 0x80001014, - - /** A non-supplier port sends this error to the IL client (via the EventHandler callback) - during the deallocation of buffers (on a transition from the IDLE to LOADED state or - on a port stop) when it deems that it has waited an unusually long time for the supplier - to request the deallocation of a buffer header via a FreeBuffer call. */ - OMX_ErrorPortUnresponsiveDuringDeallocation = (OMX_S32) 0x80001015, - - /** A supplier port sends this error to the IL client (via the EventHandler callback) - during the stopping of a port (either on a transition from the IDLE to LOADED - state or a port stop) when it deems that it has waited an unusually long time for - the non-supplier to return a buffer via an EmptyThisBuffer or FillThisBuffer call. */ - OMX_ErrorPortUnresponsiveDuringStop = (OMX_S32) 0x80001016, - - /** Attempting a state transtion that is not allowed */ - OMX_ErrorIncorrectStateTransition = (OMX_S32) 0x80001017, - - /* Attempting a command that is not allowed during the present state. */ - OMX_ErrorIncorrectStateOperation = (OMX_S32) 0x80001018, - - /** The values encapsulated in the parameter or config structure are not supported. */ - OMX_ErrorUnsupportedSetting = (OMX_S32) 0x80001019, - - /** The parameter or config indicated by the given index is not supported. */ - OMX_ErrorUnsupportedIndex = (OMX_S32) 0x8000101A, - - /** The port index supplied is incorrect. */ - OMX_ErrorBadPortIndex = (OMX_S32) 0x8000101B, - - /** The port has lost one or more of its buffers and it thus unpopulated. */ - OMX_ErrorPortUnpopulated = (OMX_S32) 0x8000101C, - - /** Component suspended due to temporary loss of resources */ - OMX_ErrorComponentSuspended = (OMX_S32) 0x8000101D, - - /** Component suspended due to an inability to acquire dynamic resources */ - OMX_ErrorDynamicResourcesUnavailable = (OMX_S32) 0x8000101E, - - /** When the macroblock error reporting is enabled the component returns new error - for every frame that has errors */ - OMX_ErrorMbErrorsInFrame = (OMX_S32) 0x8000101F, - - /** A component reports this error when it cannot parse or determine the format of an input stream. */ - OMX_ErrorFormatNotDetected = (OMX_S32) 0x80001020, - - /** The content open operation failed. */ - OMX_ErrorContentPipeOpenFailed = (OMX_S32) 0x80001021, - - /** The content creation operation failed. */ - OMX_ErrorContentPipeCreationFailed = (OMX_S32) 0x80001022, - - /** Separate table information is being used */ - OMX_ErrorSeperateTablesUsed = (OMX_S32) 0x80001023, - - /** Tunneling is unsupported by the component*/ - OMX_ErrorTunnelingUnsupported = (OMX_S32) 0x80001024, - - OMX_ErrorKhronosExtensions = (OMX_S32)0x8F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_ErrorVendorStartUnused = (OMX_S32)0x90000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_ErrorMax = 0x7FFFFFFF -} OMX_ERRORTYPE; - -/** @ingroup core */ -typedef OMX_ERRORTYPE (* OMX_COMPONENTINITTYPE)(OMX_IN OMX_HANDLETYPE hComponent); - -/** @ingroup core */ -typedef struct OMX_COMPONENTREGISTERTYPE -{ - const char * pName; /* Component name, 128 byte limit (including '\0') applies */ - OMX_COMPONENTINITTYPE pInitialize; /* Component instance initialization function */ -} OMX_COMPONENTREGISTERTYPE; - -/** @ingroup core */ -extern OMX_COMPONENTREGISTERTYPE OMX_ComponentRegistered[]; - -/** @ingroup rpm */ -typedef struct OMX_PRIORITYMGMTTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nGroupPriority; /**< Priority of the component group */ - OMX_U32 nGroupID; /**< ID of the component group */ -} OMX_PRIORITYMGMTTYPE; - -/* Component name and Role names are limited to 128 characters including the terminating '\0'. */ -#define OMX_MAX_STRINGNAME_SIZE 128 - -/** @ingroup comp */ -typedef struct OMX_PARAM_COMPONENTROLETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U8 cRole[OMX_MAX_STRINGNAME_SIZE]; /**< name of standard component which defines component role */ -} OMX_PARAM_COMPONENTROLETYPE; - -/** End of Stream Buffer Flag: - * - * A component sets EOS when it has no more data to emit on a particular - * output port. Thus an output port shall set EOS on the last buffer it - * emits. A component's determination of when an output port should - * cease sending data is implemenation specific. - * @ingroup buf - */ - -#define OMX_BUFFERFLAG_EOS 0x00000001 - -/** Start Time Buffer Flag: - * - * The source of a stream (e.g. a demux component) sets the STARTTIME - * flag on the buffer that contains the starting timestamp for the - * stream. The starting timestamp corresponds to the first data that - * should be displayed at startup or after a seek. - * The first timestamp of the stream is not necessarily the start time. - * For instance, in the case of a seek to a particular video frame, - * the target frame may be an interframe. Thus the first buffer of - * the stream will be the intra-frame preceding the target frame and - * the starttime will occur with the target frame (with any other - * required frames required to reconstruct the target intervening). - * - * The STARTTIME flag is directly associated with the buffer's - * timestamp ' thus its association to buffer data and its - * propagation is identical to the timestamp's. - * - * When a Sync Component client receives a buffer with the - * STARTTIME flag it shall perform a SetConfig on its sync port - * using OMX_ConfigTimeClientStartTime and passing the buffer's - * timestamp. - * - * @ingroup buf - */ - -#define OMX_BUFFERFLAG_STARTTIME 0x00000002 - - - -/** Decode Only Buffer Flag: - * - * The source of a stream (e.g. a demux component) sets the DECODEONLY - * flag on any buffer that should shall be decoded but should not be - * displayed. This flag is used, for instance, when a source seeks to - * a target interframe that requires the decode of frames preceding the - * target to facilitate the target's reconstruction. In this case the - * source would emit the frames preceding the target downstream - * but mark them as decode only. - * - * The DECODEONLY is associated with buffer data and propagated in a - * manner identical to the buffer timestamp. - * - * A component that renders data should ignore all buffers with - * the DECODEONLY flag set. - * - * @ingroup buf - */ - -#define OMX_BUFFERFLAG_DECODEONLY 0x00000004 - - -/* Data Corrupt Flag: This flag is set when the IL client believes the data in the associated buffer is corrupt - * @ingroup buf - */ - -#define OMX_BUFFERFLAG_DATACORRUPT 0x00000008 - -/* End of Frame: The buffer contains exactly one end of frame and no data - * occurs after the end of frame. This flag is an optional hint. The absence - * of this flag does not imply the absence of an end of frame within the buffer. - * @ingroup buf -*/ -#define OMX_BUFFERFLAG_ENDOFFRAME 0x00000010 - -/* Sync Frame Flag: This flag is set when the buffer content contains a coded sync frame ' - * a frame that has no dependency on any other frame information - * @ingroup buf - */ -#define OMX_BUFFERFLAG_SYNCFRAME 0x00000020 - -/* Extra data present flag: there is extra data appended to the data stream - * residing in the buffer - * @ingroup buf - */ -#define OMX_BUFFERFLAG_EXTRADATA 0x00000040 - -/** Codec Config Buffer Flag: -* OMX_BUFFERFLAG_CODECCONFIG is an optional flag that is set by an -* output port when all bytes in the buffer form part or all of a set of -* codec specific configuration data. Examples include SPS/PPS nal units -* for OMX_VIDEO_CodingAVC or AudioSpecificConfig data for -* OMX_AUDIO_CodingAAC. Any component that for a given stream sets -* OMX_BUFFERFLAG_CODECCONFIG shall not mix codec configuration bytes -* with frame data in the same buffer, and shall send all buffers -* containing codec configuration bytes before any buffers containing -* frame data that those configurations bytes describe. -* If the stream format for a particular codec has a frame specific -* header at the start of each frame, for example OMX_AUDIO_CodingMP3 or -* OMX_AUDIO_CodingAAC in ADTS mode, then these shall be presented as -* normal without setting OMX_BUFFERFLAG_CODECCONFIG. - * @ingroup buf - */ -#define OMX_BUFFERFLAG_CODECCONFIG 0x00000080 - - - -/** @ingroup buf */ -typedef struct OMX_BUFFERHEADERTYPE -{ - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U8* pBuffer; /**< Pointer to actual block of memory - that is acting as the buffer */ - OMX_U32 nAllocLen; /**< size of the buffer allocated, in bytes */ - OMX_U32 nFilledLen; /**< number of bytes currently in the - buffer */ - OMX_U32 nOffset; /**< start offset of valid data in bytes from - the start of the buffer */ - OMX_PTR pAppPrivate; /**< pointer to any data the application - wants to associate with this buffer */ - OMX_PTR pPlatformPrivate; /**< pointer to any data the platform - wants to associate with this buffer */ - OMX_PTR pInputPortPrivate; /**< pointer to any data the input port - wants to associate with this buffer */ - OMX_PTR pOutputPortPrivate; /**< pointer to any data the output port - wants to associate with this buffer */ - OMX_HANDLETYPE hMarkTargetComponent; /**< The component that will generate a - mark event upon processing this buffer. */ - OMX_PTR pMarkData; /**< Application specific data associated with - the mark sent on a mark event to disambiguate - this mark from others. */ - OMX_U32 nTickCount; /**< Optional entry that the component and - application can update with a tick count - when they access the component. This - value should be in microseconds. Since - this is a value relative to an arbitrary - starting point, this value cannot be used - to determine absolute time. This is an - optional entry and not all components - will update it.*/ - OMX_TICKS nTimeStamp; /**< Timestamp corresponding to the sample - starting at the first logical sample - boundary in the buffer. Timestamps of - successive samples within the buffer may - be inferred by adding the duration of the - of the preceding buffer to the timestamp - of the preceding buffer.*/ - OMX_U32 nFlags; /**< buffer specific flags */ - OMX_U32 nOutputPortIndex; /**< The index of the output port (if any) using - this buffer */ - OMX_U32 nInputPortIndex; /**< The index of the input port (if any) using - this buffer */ -} OMX_BUFFERHEADERTYPE; - -/** The OMX_EXTRADATATYPE enumeration is used to define the - * possible extra data payload types. - * NB: this enum is binary backwards compatible with the previous - * OMX_EXTRADATA_QUANT define. This should be replaced with - * OMX_ExtraDataQuantization. - */ -typedef enum OMX_EXTRADATATYPE -{ - OMX_ExtraDataNone = 0, /**< Indicates that no more extra data sections follow */ - OMX_ExtraDataQuantization, /**< The data payload contains quantization data */ - OMX_ExtraDataKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_ExtraDataVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_ExtraDataMax = 0x7FFFFFFF -} OMX_EXTRADATATYPE; - - -typedef struct OMX_OTHER_EXTRADATATYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_EXTRADATATYPE eType; /* Extra Data type */ - OMX_U32 nDataSize; /* Size of the supporting data to follow */ - OMX_U8 data[1]; /* Supporting data hint */ -} OMX_OTHER_EXTRADATATYPE; - -/** @ingroup comp */ -typedef struct OMX_PORT_PARAM_TYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPorts; /**< The number of ports for this component */ - OMX_U32 nStartPortNumber; /** first port number for this type of port */ -} OMX_PORT_PARAM_TYPE; - -/** @ingroup comp */ -typedef enum OMX_EVENTTYPE -{ - OMX_EventCmdComplete, /**< component has sucessfully completed a command */ - OMX_EventError, /**< component has detected an error condition */ - OMX_EventMark, /**< component has detected a buffer mark */ - OMX_EventPortSettingsChanged, /**< component is reported a port settings change */ - OMX_EventBufferFlag, /**< component has detected an EOS */ - OMX_EventResourcesAcquired, /**< component has been granted resources and is - automatically starting the state change from - OMX_StateWaitForResources to OMX_StateIdle. */ - OMX_EventComponentResumed, /**< Component resumed due to reacquisition of resources */ - OMX_EventDynamicResourcesAvailable, /**< Component has acquired previously unavailable dynamic resources */ - OMX_EventPortFormatDetected, /**< Component has detected a supported format. */ - OMX_EventKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_EventVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - - /** Event when tunneled decoder has rendered an output or reached EOS - * nData1 must contain the number of timestamps returned - * pEventData must point to an array of the OMX_VIDEO_RENDEREVENTTYPE structs containing the - * render-timestamps of each frame. Component may batch rendered timestamps using this event, - * but must signal the event no more than 40ms after the first frame in the batch. The frames - * must be ordered by system timestamp inside and across batches. - * - * If component is doing frame-rate conversion, it must signal the render time of each - * converted frame, and must interpolate media timestamps for in-between frames. - * - * When the component reached EOS, it must signal an EOS timestamp using the same mechanism. - * This is in addition to the timestamp of the last rendered frame, and should follow that - * frame. - */ - OMX_EventOutputRendered = 0x7F000001, - OMX_EventMax = 0x7FFFFFFF -} OMX_EVENTTYPE; - -typedef struct OMX_CALLBACKTYPE -{ - /** The EventHandler method is used to notify the application when an - event of interest occurs. Events are defined in the OMX_EVENTTYPE - enumeration. Please see that enumeration for details of what will - be returned for each type of event. Callbacks should not return - an error to the component, so if an error occurs, the application - shall handle it internally. This is a blocking call. - - The application should return from this call within 5 msec to avoid - blocking the component for an excessively long period of time. - - @param hComponent - handle of the component to access. This is the component - handle returned by the call to the GetHandle function. - @param pAppData - pointer to an application defined value that was provided in the - pAppData parameter to the OMX_GetHandle method for the component. - This application defined value is provided so that the application - can have a component specific context when receiving the callback. - @param eEvent - Event that the component wants to notify the application about. - @param nData1 - nData will be the OMX_ERRORTYPE for an error event and will be - an OMX_COMMANDTYPE for a command complete event and OMX_INDEXTYPE for a OMX_PortSettingsChanged event. - @param nData2 - nData2 will hold further information related to the event. Can be OMX_STATETYPE for - a OMX_CommandStateSet command or port index for a OMX_PortSettingsChanged event. - Default value is 0 if not used. ) - @param pEventData - Pointer to additional event-specific data (see spec for meaning). - */ - - OMX_ERRORTYPE (*EventHandler)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_PTR pAppData, - OMX_IN OMX_EVENTTYPE eEvent, - OMX_IN OMX_U32 nData1, - OMX_IN OMX_U32 nData2, - OMX_IN OMX_PTR pEventData); - - /** The EmptyBufferDone method is used to return emptied buffers from an - input port back to the application for reuse. This is a blocking call - so the application should not attempt to refill the buffers during this - call, but should queue them and refill them in another thread. There - is no error return, so the application shall handle any errors generated - internally. - - The application should return from this call within 5 msec. - - @param hComponent - handle of the component to access. This is the component - handle returned by the call to the GetHandle function. - @param pAppData - pointer to an application defined value that was provided in the - pAppData parameter to the OMX_GetHandle method for the component. - This application defined value is provided so that the application - can have a component specific context when receiving the callback. - @param pBuffer - pointer to an OMX_BUFFERHEADERTYPE structure allocated with UseBuffer - or AllocateBuffer indicating the buffer that was emptied. - @ingroup buf - */ - OMX_ERRORTYPE (*EmptyBufferDone)( - OMX_IN OMX_HANDLETYPE hComponent, - OMX_IN OMX_PTR pAppData, - OMX_IN OMX_BUFFERHEADERTYPE* pBuffer); - - /** The FillBufferDone method is used to return filled buffers from an - output port back to the application for emptying and then reuse. - This is a blocking call so the application should not attempt to - empty the buffers during this call, but should queue the buffers - and empty them in another thread. There is no error return, so - the application shall handle any errors generated internally. The - application shall also update the buffer header to indicate the - number of bytes placed into the buffer. - - The application should return from this call within 5 msec. - - @param hComponent - handle of the component to access. This is the component - handle returned by the call to the GetHandle function. - @param pAppData - pointer to an application defined value that was provided in the - pAppData parameter to the OMX_GetHandle method for the component. - This application defined value is provided so that the application - can have a component specific context when receiving the callback. - @param pBuffer - pointer to an OMX_BUFFERHEADERTYPE structure allocated with UseBuffer - or AllocateBuffer indicating the buffer that was filled. - @ingroup buf - */ - OMX_ERRORTYPE (*FillBufferDone)( - OMX_OUT OMX_HANDLETYPE hComponent, - OMX_OUT OMX_PTR pAppData, - OMX_OUT OMX_BUFFERHEADERTYPE* pBuffer); - -} OMX_CALLBACKTYPE; - -/** The OMX_BUFFERSUPPLIERTYPE enumeration is used to dictate port supplier - preference when tunneling between two ports. - @ingroup tun buf -*/ -typedef enum OMX_BUFFERSUPPLIERTYPE -{ - OMX_BufferSupplyUnspecified = 0x0, /**< port supplying the buffers is unspecified, - or don't care */ - OMX_BufferSupplyInput, /**< input port supplies the buffers */ - OMX_BufferSupplyOutput, /**< output port supplies the buffers */ - OMX_BufferSupplyKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_BufferSupplyVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_BufferSupplyMax = 0x7FFFFFFF -} OMX_BUFFERSUPPLIERTYPE; - - -/** buffer supplier parameter - * @ingroup tun - */ -typedef struct OMX_PARAM_BUFFERSUPPLIERTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_BUFFERSUPPLIERTYPE eBufferSupplier; /**< buffer supplier */ -} OMX_PARAM_BUFFERSUPPLIERTYPE; - - -/**< indicates that buffers received by an input port of a tunnel - may not modify the data in the buffers - @ingroup tun - */ -#define OMX_PORTTUNNELFLAG_READONLY 0x00000001 - - -/** The OMX_TUNNELSETUPTYPE structure is used to pass data from an output - port to an input port as part the two ComponentTunnelRequest calls - resulting from a OMX_SetupTunnel call from the IL Client. - @ingroup tun - */ -typedef struct OMX_TUNNELSETUPTYPE -{ - OMX_U32 nTunnelFlags; /**< bit flags for tunneling */ - OMX_BUFFERSUPPLIERTYPE eSupplier; /**< supplier preference */ -} OMX_TUNNELSETUPTYPE; - -/* OMX Component headers is included to enable the core to use - macros for functions into the component for OMX release 1.0. - Developers should not access any structures or data from within - the component header directly */ -/* TO BE REMOVED - #include */ - -/** GetComponentVersion will return information about the component. - This is a blocking call. This macro will go directly from the - application to the component (via a core macro). The - component will return from this call within 5 msec. - @param [in] hComponent - handle of component to execute the command - @param [out] pComponentName - pointer to an empty string of length 128 bytes. The component - will write its name into this string. The name will be - terminated by a single zero byte. The name of a component will - be 127 bytes or less to leave room for the trailing zero byte. - An example of a valid component name is "OMX.ABC.ChannelMixer\0". - @param [out] pComponentVersion - pointer to an OMX Version structure that the component will fill - in. The component will fill in a value that indicates the - component version. NOTE: the component version is NOT the same - as the OMX Specification version (found in all structures). The - component version is defined by the vendor of the component and - its value is entirely up to the component vendor. - @param [out] pSpecVersion - pointer to an OMX Version structure that the component will fill - in. The SpecVersion is the version of the specification that the - component was built against. Please note that this value may or - may not match the structure's version. For example, if the - component was built against the 2.0 specification, but the - application (which creates the structure is built against the - 1.0 specification the versions would be different. - @param [out] pComponentUUID - pointer to the UUID of the component which will be filled in by - the component. The UUID is a unique identifier that is set at - RUN time for the component and is unique to each instantion of - the component. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_GetComponentVersion( \ - hComponent, \ - pComponentName, \ - pComponentVersion, \ - pSpecVersion, \ - pComponentUUID) \ - ((OMX_COMPONENTTYPE*)hComponent)->GetComponentVersion( \ - hComponent, \ - pComponentName, \ - pComponentVersion, \ - pSpecVersion, \ - pComponentUUID) /* Macro End */ - - -/** Send a command to the component. This call is a non-blocking call. - The component should check the parameters and then queue the command - to the component thread to be executed. The component thread shall - send the EventHandler() callback at the conclusion of the command. - This macro will go directly from the application to the component (via - a core macro). The component will return from this call within 5 msec. - - When the command is "OMX_CommandStateSet" the component will queue a - state transition to the new state idenfied in nParam. - - When the command is "OMX_CommandFlush", to flush a port's buffer queues, - the command will force the component to return all buffers NOT CURRENTLY - BEING PROCESSED to the application, in the order in which the buffers - were received. - - When the command is "OMX_CommandPortDisable" or - "OMX_CommandPortEnable", the component's port (given by the value of - nParam) will be stopped or restarted. - - When the command "OMX_CommandMarkBuffer" is used to mark a buffer, the - pCmdData will point to a OMX_MARKTYPE structure containing the component - handle of the component to examine the buffer chain for the mark. nParam1 - contains the index of the port on which the buffer mark is applied. - - Specification text for more details. - - @param [in] hComponent - handle of component to execute the command - @param [in] Cmd - Command for the component to execute - @param [in] nParam - Parameter for the command to be executed. When Cmd has the value - OMX_CommandStateSet, value is a member of OMX_STATETYPE. When Cmd has - the value OMX_CommandFlush, value of nParam indicates which port(s) - to flush. -1 is used to flush all ports a single port index will - only flush that port. When Cmd has the value "OMX_CommandPortDisable" - or "OMX_CommandPortEnable", the component's port is given by - the value of nParam. When Cmd has the value "OMX_CommandMarkBuffer" - the components pot is given by the value of nParam. - @param [in] pCmdData - Parameter pointing to the OMX_MARKTYPE structure when Cmd has the value - "OMX_CommandMarkBuffer". - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_SendCommand( \ - hComponent, \ - Cmd, \ - nParam, \ - pCmdData) \ - ((OMX_COMPONENTTYPE*)hComponent)->SendCommand( \ - hComponent, \ - Cmd, \ - nParam, \ - pCmdData) /* Macro End */ - - -/** The OMX_GetParameter macro will get one of the current parameter - settings from the component. This macro cannot only be invoked when - the component is in the OMX_StateInvalid state. The nParamIndex - parameter is used to indicate which structure is being requested from - the component. The application shall allocate the correct structure - and shall fill in the structure size and version information before - invoking this macro. When the parameter applies to a port, the - caller shall fill in the appropriate nPortIndex value indicating the - port on which the parameter applies. If the component has not had - any settings changed, then the component should return a set of - valid DEFAULT parameters for the component. This is a blocking - call. - - The component should return from this call within 20 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] nParamIndex - Index of the structure to be filled. This value is from the - OMX_INDEXTYPE enumeration. - @param [in,out] pComponentParameterStructure - Pointer to application allocated structure to be filled by the - component. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_GetParameter( \ - hComponent, \ - nParamIndex, \ - pComponentParameterStructure) \ - ((OMX_COMPONENTTYPE*)hComponent)->GetParameter( \ - hComponent, \ - nParamIndex, \ - pComponentParameterStructure) /* Macro End */ - - -/** The OMX_SetParameter macro will send an initialization parameter - structure to a component. Each structure shall be sent one at a time, - in a separate invocation of the macro. This macro can only be - invoked when the component is in the OMX_StateLoaded state, or the - port is disabled (when the parameter applies to a port). The - nParamIndex parameter is used to indicate which structure is being - passed to the component. The application shall allocate the - correct structure and shall fill in the structure size and version - information (as well as the actual data) before invoking this macro. - The application is free to dispose of this structure after the call - as the component is required to copy any data it shall retain. This - is a blocking call. - - The component should return from this call within 20 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] nIndex - Index of the structure to be sent. This value is from the - OMX_INDEXTYPE enumeration. - @param [in] pComponentParameterStructure - pointer to application allocated structure to be used for - initialization by the component. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_SetParameter( \ - hComponent, \ - nParamIndex, \ - pComponentParameterStructure) \ - ((OMX_COMPONENTTYPE*)hComponent)->SetParameter( \ - hComponent, \ - nParamIndex, \ - pComponentParameterStructure) /* Macro End */ - - -/** The OMX_GetConfig macro will get one of the configuration structures - from a component. This macro can be invoked anytime after the - component has been loaded. The nParamIndex call parameter is used to - indicate which structure is being requested from the component. The - application shall allocate the correct structure and shall fill in the - structure size and version information before invoking this macro. - If the component has not had this configuration parameter sent before, - then the component should return a set of valid DEFAULT values for the - component. This is a blocking call. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] nIndex - Index of the structure to be filled. This value is from the - OMX_INDEXTYPE enumeration. - @param [in,out] pComponentConfigStructure - pointer to application allocated structure to be filled by the - component. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp -*/ -#define OMX_GetConfig( \ - hComponent, \ - nConfigIndex, \ - pComponentConfigStructure) \ - ((OMX_COMPONENTTYPE*)hComponent)->GetConfig( \ - hComponent, \ - nConfigIndex, \ - pComponentConfigStructure) /* Macro End */ - - -/** The OMX_SetConfig macro will send one of the configuration - structures to a component. Each structure shall be sent one at a time, - each in a separate invocation of the macro. This macro can be invoked - anytime after the component has been loaded. The application shall - allocate the correct structure and shall fill in the structure size - and version information (as well as the actual data) before invoking - this macro. The application is free to dispose of this structure after - the call as the component is required to copy any data it shall retain. - This is a blocking call. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] nConfigIndex - Index of the structure to be sent. This value is from the - OMX_INDEXTYPE enumeration above. - @param [in] pComponentConfigStructure - pointer to application allocated structure to be used for - initialization by the component. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_SetConfig( \ - hComponent, \ - nConfigIndex, \ - pComponentConfigStructure) \ - ((OMX_COMPONENTTYPE*)hComponent)->SetConfig( \ - hComponent, \ - nConfigIndex, \ - pComponentConfigStructure) /* Macro End */ - - -/** The OMX_GetExtensionIndex macro will invoke a component to translate - a vendor specific configuration or parameter string into an OMX - structure index. There is no requirement for the vendor to support - this command for the indexes already found in the OMX_INDEXTYPE - enumeration (this is done to save space in small components). The - component shall support all vendor supplied extension indexes not found - in the master OMX_INDEXTYPE enumeration. This is a blocking call. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the GetHandle function. - @param [in] cParameterName - OMX_STRING that shall be less than 128 characters long including - the trailing null byte. This is the string that will get - translated by the component into a configuration index. - @param [out] pIndexType - a pointer to a OMX_INDEXTYPE to receive the index value. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_GetExtensionIndex( \ - hComponent, \ - cParameterName, \ - pIndexType) \ - ((OMX_COMPONENTTYPE*)hComponent)->GetExtensionIndex( \ - hComponent, \ - cParameterName, \ - pIndexType) /* Macro End */ - - -/** The OMX_GetState macro will invoke the component to get the current - state of the component and place the state value into the location - pointed to by pState. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [out] pState - pointer to the location to receive the state. The value returned - is one of the OMX_STATETYPE members - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp - */ -#define OMX_GetState( \ - hComponent, \ - pState) \ - ((OMX_COMPONENTTYPE*)hComponent)->GetState( \ - hComponent, \ - pState) /* Macro End */ - - -/** The OMX_UseBuffer macro will request that the component use - a buffer (and allocate its own buffer header) already allocated - by another component, or by the IL Client. This is a blocking - call. - - The component should return from this call within 20 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [out] ppBuffer - pointer to an OMX_BUFFERHEADERTYPE structure used to receive the - pointer to the buffer header - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ - -#define OMX_UseBuffer( \ - hComponent, \ - ppBufferHdr, \ - nPortIndex, \ - pAppPrivate, \ - nSizeBytes, \ - pBuffer) \ - ((OMX_COMPONENTTYPE*)hComponent)->UseBuffer( \ - hComponent, \ - ppBufferHdr, \ - nPortIndex, \ - pAppPrivate, \ - nSizeBytes, \ - pBuffer) - - -/** The OMX_AllocateBuffer macro will request that the component allocate - a new buffer and buffer header. The component will allocate the - buffer and the buffer header and return a pointer to the buffer - header. This is a blocking call. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [out] ppBuffer - pointer to an OMX_BUFFERHEADERTYPE structure used to receive - the pointer to the buffer header - @param [in] nPortIndex - nPortIndex is used to select the port on the component the buffer will - be used with. The port can be found by using the nPortIndex - value as an index into the Port Definition array of the component. - @param [in] pAppPrivate - pAppPrivate is used to initialize the pAppPrivate member of the - buffer header structure. - @param [in] nSizeBytes - size of the buffer to allocate. Used when bAllocateNew is true. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ -#define OMX_AllocateBuffer( \ - hComponent, \ - ppBuffer, \ - nPortIndex, \ - pAppPrivate, \ - nSizeBytes) \ - ((OMX_COMPONENTTYPE*)hComponent)->AllocateBuffer( \ - hComponent, \ - ppBuffer, \ - nPortIndex, \ - pAppPrivate, \ - nSizeBytes) /* Macro End */ - - -/** The OMX_FreeBuffer macro will release a buffer header from the component - which was allocated using either OMX_AllocateBuffer or OMX_UseBuffer. If - the component allocated the buffer (see the OMX_UseBuffer macro) then - the component shall free the buffer and buffer header. This is a - blocking call. - - The component should return from this call within 20 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] nPortIndex - nPortIndex is used to select the port on the component the buffer will - be used with. - @param [in] pBuffer - pointer to an OMX_BUFFERHEADERTYPE structure allocated with UseBuffer - or AllocateBuffer. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ -#define OMX_FreeBuffer( \ - hComponent, \ - nPortIndex, \ - pBuffer) \ - ((OMX_COMPONENTTYPE*)hComponent)->FreeBuffer( \ - hComponent, \ - nPortIndex, \ - pBuffer) /* Macro End */ - - -/** The OMX_EmptyThisBuffer macro will send a buffer full of data to an - input port of a component. The buffer will be emptied by the component - and returned to the application via the EmptyBufferDone call back. - This is a non-blocking call in that the component will record the buffer - and return immediately and then empty the buffer, later, at the proper - time. As expected, this macro may be invoked only while the component - is in the OMX_StateExecuting. If nPortIndex does not specify an input - port, the component shall return an error. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] pBuffer - pointer to an OMX_BUFFERHEADERTYPE structure allocated with UseBuffer - or AllocateBuffer. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ -#define OMX_EmptyThisBuffer( \ - hComponent, \ - pBuffer) \ - ((OMX_COMPONENTTYPE*)hComponent)->EmptyThisBuffer( \ - hComponent, \ - pBuffer) /* Macro End */ - - -/** The OMX_FillThisBuffer macro will send an empty buffer to an - output port of a component. The buffer will be filled by the component - and returned to the application via the FillBufferDone call back. - This is a non-blocking call in that the component will record the buffer - and return immediately and then fill the buffer, later, at the proper - time. As expected, this macro may be invoked only while the component - is in the OMX_ExecutingState. If nPortIndex does not specify an output - port, the component shall return an error. - - The component should return from this call within 5 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [in] pBuffer - pointer to an OMX_BUFFERHEADERTYPE structure allocated with UseBuffer - or AllocateBuffer. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ -#define OMX_FillThisBuffer( \ - hComponent, \ - pBuffer) \ - ((OMX_COMPONENTTYPE*)hComponent)->FillThisBuffer( \ - hComponent, \ - pBuffer) /* Macro End */ - - - -/** The OMX_UseEGLImage macro will request that the component use - a EGLImage provided by EGL (and allocate its own buffer header) - This is a blocking call. - - The component should return from this call within 20 msec. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the OMX_GetHandle function. - @param [out] ppBuffer - pointer to an OMX_BUFFERHEADERTYPE structure used to receive the - pointer to the buffer header. Note that the memory location used - for this buffer is NOT visible to the IL Client. - @param [in] nPortIndex - nPortIndex is used to select the port on the component the buffer will - be used with. The port can be found by using the nPortIndex - value as an index into the Port Definition array of the component. - @param [in] pAppPrivate - pAppPrivate is used to initialize the pAppPrivate member of the - buffer header structure. - @param [in] eglImage - eglImage contains the handle of the EGLImage to use as a buffer on the - specified port. The component is expected to validate properties of - the EGLImage against the configuration of the port to ensure the component - can use the EGLImage as a buffer. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup comp buf - */ -#define OMX_UseEGLImage( \ - hComponent, \ - ppBufferHdr, \ - nPortIndex, \ - pAppPrivate, \ - eglImage) \ - ((OMX_COMPONENTTYPE*)hComponent)->UseEGLImage( \ - hComponent, \ - ppBufferHdr, \ - nPortIndex, \ - pAppPrivate, \ - eglImage) - -/** The OMX_Init method is used to initialize the OMX core. It shall be the - first call made into OMX and it should only be executed one time without - an interviening OMX_Deinit call. - - The core should return from this call within 20 msec. - - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_Init(void); - - -/** The OMX_Deinit method is used to deinitialize the OMX core. It shall be - the last call made into OMX. In the event that the core determines that - thare are components loaded when this call is made, the core may return - with an error rather than try to unload the components. - - The core should return from this call within 20 msec. - - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_Deinit(void); - - -/** The OMX_ComponentNameEnum method will enumerate through all the names of - recognised valid components in the system. This function is provided - as a means to detect all the components in the system run-time. There is - no strict ordering to the enumeration order of component names, although - each name will only be enumerated once. If the OMX core supports run-time - installation of new components, it is only requried to detect newly - installed components when the first call to enumerate component names - is made (i.e. when nIndex is 0x0). - - The core should return from this call in 20 msec. - - @param [out] cComponentName - pointer to a null terminated string with the component name. The - names of the components are strings less than 127 bytes in length - plus the trailing null for a maximum size of 128 bytes. An example - of a valid component name is "OMX.TI.AUDIO.DSP.MIXER\0". Names are - assigned by the vendor, but shall start with "OMX." and then have - the Vendor designation next. - @param [in] nNameLength - number of characters in the cComponentName string. With all - component name strings restricted to less than 128 characters - (including the trailing null) it is recomended that the caller - provide a input string for the cComponentName of 128 characters. - @param [in] nIndex - number containing the enumeration index for the component. - Multiple calls to OMX_ComponentNameEnum with increasing values - of nIndex will enumerate through the component names in the - system until OMX_ErrorNoMore is returned. The value of nIndex - is 0 to (N-1), where N is the number of valid installed components - in the system. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. When the value of nIndex exceeds the number of - components in the system minus 1, OMX_ErrorNoMore will be - returned. Otherwise the appropriate OMX error will be returned. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_ComponentNameEnum( - OMX_OUT OMX_STRING cComponentName, - OMX_IN OMX_U32 nNameLength, - OMX_IN OMX_U32 nIndex); - - -/** The OMX_GetHandle method will locate the component specified by the - component name given, load that component into memory and then invoke - the component's methods to create an instance of the component. - - The core should return from this call within 20 msec. - - @param [out] pHandle - pointer to an OMX_HANDLETYPE pointer to be filled in by this method. - @param [in] cComponentName - pointer to a null terminated string with the component name. The - names of the components are strings less than 127 bytes in length - plus the trailing null for a maximum size of 128 bytes. An example - of a valid component name is "OMX.TI.AUDIO.DSP.MIXER\0". Names are - assigned by the vendor, but shall start with "OMX." and then have - the Vendor designation next. - @param [in] pAppData - pointer to an application defined value that will be returned - during callbacks so that the application can identify the source - of the callback. - @param [in] pCallBacks - pointer to a OMX_CALLBACKTYPE structure that will be passed to the - component to initialize it with. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_GetHandle( - OMX_OUT OMX_HANDLETYPE* pHandle, - OMX_IN OMX_STRING cComponentName, - OMX_IN OMX_PTR pAppData, - OMX_IN OMX_CALLBACKTYPE* pCallBacks); - - -/** The OMX_FreeHandle method will free a handle allocated by the OMX_GetHandle - method. If the component reference count goes to zero, the component will - be unloaded from memory. - - The core should return from this call within 20 msec when the component is - in the OMX_StateLoaded state. - - @param [in] hComponent - Handle of the component to be accessed. This is the component - handle returned by the call to the GetHandle function. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_FreeHandle( - OMX_IN OMX_HANDLETYPE hComponent); - - - -/** The OMX_SetupTunnel method will handle the necessary calls to the components - to setup the specified tunnel the two components. NOTE: This is - an actual method (not a #define macro). This method will make calls into - the component ComponentTunnelRequest method to do the actual tunnel - connection. - - The ComponentTunnelRequest method on both components will be called. - This method shall not be called unless the component is in the - OMX_StateLoaded state except when the ports used for the tunnel are - disabled. In this case, the component may be in the OMX_StateExecuting, - OMX_StatePause, or OMX_StateIdle states. - - The core should return from this call within 20 msec. - - @param [in] hOutput - Handle of the component to be accessed. Also this is the handle - of the component whose port, specified in the nPortOutput parameter - will be used the source for the tunnel. This is the component handle - returned by the call to the OMX_GetHandle function. There is a - requirement that hOutput be the source for the data when - tunelling (i.e. nPortOutput is an output port). If 0x0, the component - specified in hInput will have it's port specified in nPortInput - setup for communication with the application / IL client. - @param [in] nPortOutput - nPortOutput is used to select the source port on component to be - used in the tunnel. - @param [in] hInput - This is the component to setup the tunnel with. This is the handle - of the component whose port, specified in the nPortInput parameter - will be used the destination for the tunnel. This is the component handle - returned by the call to the OMX_GetHandle function. There is a - requirement that hInput be the destination for the data when - tunelling (i.e. nPortInut is an input port). If 0x0, the component - specified in hOutput will have it's port specified in nPortPOutput - setup for communication with the application / IL client. - @param [in] nPortInput - nPortInput is used to select the destination port on component to be - used in the tunnel. - @return OMX_ERRORTYPE - If the command successfully executes, the return code will be - OMX_ErrorNone. Otherwise the appropriate OMX error will be returned. - When OMX_ErrorNotImplemented is returned, one or both components is - a non-interop component and does not support tunneling. - - On failure, the ports of both components are setup for communication - with the application / IL Client. - @ingroup core tun - */ -OMX_API OMX_ERRORTYPE OMX_APIENTRY OMX_SetupTunnel( - OMX_IN OMX_HANDLETYPE hOutput, - OMX_IN OMX_U32 nPortOutput, - OMX_IN OMX_HANDLETYPE hInput, - OMX_IN OMX_U32 nPortInput); - -/** @ingroup cp */ -OMX_API OMX_ERRORTYPE OMX_GetContentPipe( - OMX_OUT OMX_HANDLETYPE *hPipe, - OMX_IN OMX_STRING szURI); - -/** The OMX_GetComponentsOfRole method will return the number of components that support the given - role and (if the compNames field is non-NULL) the names of those components. The call will fail if - an insufficiently sized array of names is supplied. To ensure the array is sufficiently sized the - client should: - * first call this function with the compNames field NULL to determine the number of component names - * second call this function with the compNames field pointing to an array of names allocated - according to the number returned by the first call. - - The core should return from this call within 5 msec. - - @param [in] role - This is generic standard component name consisting only of component class - name and the type within that class (e.g. 'audio_decoder.aac'). - @param [inout] pNumComps - This is used both as input and output. - - If compNames is NULL, the input is ignored and the output specifies how many components support - the given role. - - If compNames is not NULL, on input it bounds the size of the input structure and - on output, it specifies the number of components string names listed within the compNames parameter. - @param [inout] compNames - If NULL this field is ignored. If non-NULL this points to an array of 128-byte strings which accepts - a list of the names of all physical components that implement the specified standard component name. - Each name is NULL terminated. numComps indicates the number of names. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_GetComponentsOfRole ( - OMX_IN OMX_STRING role, - OMX_INOUT OMX_U32 *pNumComps, - OMX_INOUT OMX_U8 **compNames); - -/** The OMX_GetRolesOfComponent method will return the number of roles supported by the given - component and (if the roles field is non-NULL) the names of those roles. The call will fail if - an insufficiently sized array of names is supplied. To ensure the array is sufficiently sized the - client should: - * first call this function with the roles field NULL to determine the number of role names - * second call this function with the roles field pointing to an array of names allocated - according to the number returned by the first call. - - The core should return from this call within 5 msec. - - @param [in] compName - This is the name of the component being queried about. - @param [inout] pNumRoles - This is used both as input and output. - - If roles is NULL, the input is ignored and the output specifies how many roles the component supports. - - If compNames is not NULL, on input it bounds the size of the input structure and - on output, it specifies the number of roles string names listed within the roles parameter. - @param [out] roles - If NULL this field is ignored. If non-NULL this points to an array of 128-byte strings - which accepts a list of the names of all standard components roles implemented on the - specified component name. numComps indicates the number of names. - @ingroup core - */ -OMX_API OMX_ERRORTYPE OMX_GetRolesOfComponent ( - OMX_IN OMX_STRING compName, - OMX_INOUT OMX_U32 *pNumRoles, - OMX_OUT OMX_U8 **roles); - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ - diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_IVCommon.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_IVCommon.h deleted file mode 100644 index f9b6f4b0f..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_IVCommon.h +++ /dev/null @@ -1,958 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/** - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** - * @file OMX_IVCommon.h - OpenMax IL version 1.1.2 - * The structures needed by Video and Image components to exchange - * parameters and configuration data with the components. - */ -#ifndef OMX_IVCommon_h -#define OMX_IVCommon_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/** - * Each OMX header must include all required header files to allow the header - * to compile without errors. The includes below are required for this header - * file to compile successfully - */ - -#include - -/** @defgroup iv OpenMAX IL Imaging and Video Domain - * Common structures for OpenMAX IL Imaging and Video domains - * @{ - */ - - -/** - * Enumeration defining possible uncompressed image/video formats. - * - * ENUMS: - * Unused : Placeholder value when format is N/A - * Monochrome : black and white - * 8bitRGB332 : Red 7:5, Green 4:2, Blue 1:0 - * 12bitRGB444 : Red 11:8, Green 7:4, Blue 3:0 - * 16bitARGB4444 : Alpha 15:12, Red 11:8, Green 7:4, Blue 3:0 - * 16bitARGB1555 : Alpha 15, Red 14:10, Green 9:5, Blue 4:0 - * 16bitRGB565 : Red 15:11, Green 10:5, Blue 4:0 - * 16bitBGR565 : Blue 15:11, Green 10:5, Red 4:0 - * 18bitRGB666 : Red 17:12, Green 11:6, Blue 5:0 - * 18bitARGB1665 : Alpha 17, Red 16:11, Green 10:5, Blue 4:0 - * 19bitARGB1666 : Alpha 18, Red 17:12, Green 11:6, Blue 5:0 - * 24bitRGB888 : Red 24:16, Green 15:8, Blue 7:0 - * 24bitBGR888 : Blue 24:16, Green 15:8, Red 7:0 - * 24bitARGB1887 : Alpha 23, Red 22:15, Green 14:7, Blue 6:0 - * 25bitARGB1888 : Alpha 24, Red 23:16, Green 15:8, Blue 7:0 - * 32bitBGRA8888 : Blue 31:24, Green 23:16, Red 15:8, Alpha 7:0 - * 32bitARGB8888 : Alpha 31:24, Red 23:16, Green 15:8, Blue 7:0 - * YUV411Planar : U,Y are subsampled by a factor of 4 horizontally - * YUV411PackedPlanar : packed per payload in planar slices - * YUV420Planar : Three arrays Y,U,V. - * YUV420PackedPlanar : packed per payload in planar slices - * YUV420SemiPlanar : Two arrays, one is all Y, the other is U and V - * YUV422Planar : Three arrays Y,U,V. - * YUV422PackedPlanar : packed per payload in planar slices - * YUV422SemiPlanar : Two arrays, one is all Y, the other is U and V - * YCbYCr : Organized as 16bit YUYV (i.e. YCbYCr) - * YCrYCb : Organized as 16bit YVYU (i.e. YCrYCb) - * CbYCrY : Organized as 16bit UYVY (i.e. CbYCrY) - * CrYCbY : Organized as 16bit VYUY (i.e. CrYCbY) - * YUV444Interleaved : Each pixel contains equal parts YUV - * RawBayer8bit : SMIA camera output format - * RawBayer10bit : SMIA camera output format - * RawBayer8bitcompressed : SMIA camera output format - */ -typedef enum OMX_COLOR_FORMATTYPE { - OMX_COLOR_FormatUnused, - OMX_COLOR_FormatMonochrome, - OMX_COLOR_Format8bitRGB332, - OMX_COLOR_Format12bitRGB444, - OMX_COLOR_Format16bitARGB4444, - OMX_COLOR_Format16bitARGB1555, - OMX_COLOR_Format16bitRGB565, - OMX_COLOR_Format16bitBGR565, - OMX_COLOR_Format18bitRGB666, - OMX_COLOR_Format18bitARGB1665, - OMX_COLOR_Format19bitARGB1666, - OMX_COLOR_Format24bitRGB888, - OMX_COLOR_Format24bitBGR888, - OMX_COLOR_Format24bitARGB1887, - OMX_COLOR_Format25bitARGB1888, - OMX_COLOR_Format32bitBGRA8888, - OMX_COLOR_Format32bitARGB8888, - OMX_COLOR_FormatYUV411Planar, - OMX_COLOR_FormatYUV411PackedPlanar, - OMX_COLOR_FormatYUV420Planar, - OMX_COLOR_FormatYUV420PackedPlanar, - OMX_COLOR_FormatYUV420SemiPlanar, - OMX_COLOR_FormatYUV422Planar, - OMX_COLOR_FormatYUV422PackedPlanar, - OMX_COLOR_FormatYUV422SemiPlanar, - OMX_COLOR_FormatYCbYCr, - OMX_COLOR_FormatYCrYCb, - OMX_COLOR_FormatCbYCrY, - OMX_COLOR_FormatCrYCbY, - OMX_COLOR_FormatYUV444Interleaved, - OMX_COLOR_FormatRawBayer8bit, - OMX_COLOR_FormatRawBayer10bit, - OMX_COLOR_FormatRawBayer8bitcompressed, - OMX_COLOR_FormatL2, - OMX_COLOR_FormatL4, - OMX_COLOR_FormatL8, - OMX_COLOR_FormatL16, - OMX_COLOR_FormatL24, - OMX_COLOR_FormatL32, - OMX_COLOR_FormatYUV420PackedSemiPlanar, - OMX_COLOR_FormatYUV422PackedSemiPlanar, - OMX_COLOR_Format18BitBGR666, - OMX_COLOR_Format24BitARGB6666, - OMX_COLOR_Format24BitABGR6666, - OMX_COLOR_FormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_COLOR_FormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - /** - -/** @defgroup imaging OpenMAX IL Imaging Domain - * @ingroup iv - * Structures for OpenMAX IL Imaging domain - * @{ - */ - -/** - * Enumeration used to define the possible image compression coding. - */ -typedef enum OMX_IMAGE_CODINGTYPE { - OMX_IMAGE_CodingUnused, /**< Value when format is N/A */ - OMX_IMAGE_CodingAutoDetect, /**< Auto detection of image format */ - OMX_IMAGE_CodingJPEG, /**< JPEG/JFIF image format */ - OMX_IMAGE_CodingJPEG2K, /**< JPEG 2000 image format */ - OMX_IMAGE_CodingEXIF, /**< EXIF image format */ - OMX_IMAGE_CodingTIFF, /**< TIFF image format */ - OMX_IMAGE_CodingGIF, /**< Graphics image format */ - OMX_IMAGE_CodingPNG, /**< PNG image format */ - OMX_IMAGE_CodingLZW, /**< LZW image format */ - OMX_IMAGE_CodingBMP, /**< Windows Bitmap format */ - OMX_IMAGE_CodingKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_IMAGE_CodingVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_IMAGE_CodingMax = 0x7FFFFFFF -} OMX_IMAGE_CODINGTYPE; - - -/** - * Data structure used to define an image path. The number of image paths - * for input and output will vary by type of the image component. - * - * Input (aka Source) : Zero Inputs, one Output, - * Splitter : One Input, 2 or more Outputs, - * Processing Element : One Input, one output, - * Mixer : 2 or more inputs, one output, - * Output (aka Sink) : One Input, zero outputs. - * - * The PortDefinition structure is used to define all of the parameters - * necessary for the compliant component to setup an input or an output - * image path. If additional vendor specific data is required, it should - * be transmitted to the component using the CustomCommand function. - * Compliant components will prepopulate this structure with optimal - * values during the OMX_GetParameter() command. - * - * STRUCT MEMBERS: - * cMIMEType : MIME type of data for the port - * pNativeRender : Platform specific reference for a display if a - * sync, otherwise this field is 0 - * nFrameWidth : Width of frame to be used on port if - * uncompressed format is used. Use 0 for - * unknown, don't care or variable - * nFrameHeight : Height of frame to be used on port if - * uncompressed format is used. Use 0 for - * unknown, don't care or variable - * nStride : Number of bytes per span of an image (i.e. - * indicates the number of bytes to get from - * span N to span N+1, where negative stride - * indicates the image is bottom up - * nSliceHeight : Height used when encoding in slices - * bFlagErrorConcealment : Turns on error concealment if it is supported by - * the OMX component - * eCompressionFormat : Compression format used in this instance of - * the component. When OMX_IMAGE_CodingUnused is - * specified, eColorFormat is valid - * eColorFormat : Decompressed format used by this component - * pNativeWindow : Platform specific reference for a window object if a - * display sink , otherwise this field is 0x0. - */ -typedef struct OMX_IMAGE_PORTDEFINITIONTYPE { - OMX_STRING cMIMEType; - OMX_NATIVE_DEVICETYPE pNativeRender; - OMX_U32 nFrameWidth; - OMX_U32 nFrameHeight; - OMX_S32 nStride; - OMX_U32 nSliceHeight; - OMX_BOOL bFlagErrorConcealment; - OMX_IMAGE_CODINGTYPE eCompressionFormat; - OMX_COLOR_FORMATTYPE eColorFormat; - OMX_NATIVE_WINDOWTYPE pNativeWindow; -} OMX_IMAGE_PORTDEFINITIONTYPE; - - -/** - * Port format parameter. This structure is used to enumerate the various - * data input/output format supported by the port. - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Indicates which port to set - * nIndex : Indicates the enumeration index for the format from - * 0x0 to N-1 - * eCompressionFormat : Compression format used in this instance of the - * component. When OMX_IMAGE_CodingUnused is specified, - * eColorFormat is valid - * eColorFormat : Decompressed format used by this component - */ -typedef struct OMX_IMAGE_PARAM_PORTFORMATTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nIndex; - OMX_IMAGE_CODINGTYPE eCompressionFormat; - OMX_COLOR_FORMATTYPE eColorFormat; -} OMX_IMAGE_PARAM_PORTFORMATTYPE; - - -/** - * Flash control type - * - * ENUMS - * Torch : Flash forced constantly on - */ -typedef enum OMX_IMAGE_FLASHCONTROLTYPE { - OMX_IMAGE_FlashControlOn = 0, - OMX_IMAGE_FlashControlOff, - OMX_IMAGE_FlashControlAuto, - OMX_IMAGE_FlashControlRedEyeReduction, - OMX_IMAGE_FlashControlFillin, - OMX_IMAGE_FlashControlTorch, - OMX_IMAGE_FlashControlKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_IMAGE_FlashControlVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_IMAGE_FlashControlMax = 0x7FFFFFFF -} OMX_IMAGE_FLASHCONTROLTYPE; - - -/** - * Flash control configuration - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eFlashControl : Flash control type - */ -typedef struct OMX_IMAGE_PARAM_FLASHCONTROLTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_IMAGE_FLASHCONTROLTYPE eFlashControl; -} OMX_IMAGE_PARAM_FLASHCONTROLTYPE; - - -/** - * Focus control type - */ -typedef enum OMX_IMAGE_FOCUSCONTROLTYPE { - OMX_IMAGE_FocusControlOn = 0, - OMX_IMAGE_FocusControlOff, - OMX_IMAGE_FocusControlAuto, - OMX_IMAGE_FocusControlAutoLock, - OMX_IMAGE_FocusControlKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_IMAGE_FocusControlVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_IMAGE_FocusControlMax = 0x7FFFFFFF -} OMX_IMAGE_FOCUSCONTROLTYPE; - - -/** - * Focus control configuration - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eFocusControl : Focus control - * nFocusSteps : Focus can take on values from 0 mm to infinity. - * Interest is only in number of steps over this range. - * nFocusStepIndex : Current focus step index - */ -typedef struct OMX_IMAGE_CONFIG_FOCUSCONTROLTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_IMAGE_FOCUSCONTROLTYPE eFocusControl; - OMX_U32 nFocusSteps; - OMX_U32 nFocusStepIndex; -} OMX_IMAGE_CONFIG_FOCUSCONTROLTYPE; - - -/** - * Q Factor for JPEG compression, which controls the tradeoff between image - * quality and size. Q Factor provides a more simple means of controlling - * JPEG compression quality, without directly programming Quantization - * tables for chroma and luma - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nQFactor : JPEG Q factor value in the range of 1-100. A factor of 1 - * produces the smallest, worst quality images, and a factor - * of 100 produces the largest, best quality images. A - * typical default is 75 for small good quality images - */ -typedef struct OMX_IMAGE_PARAM_QFACTORTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nQFactor; -} OMX_IMAGE_PARAM_QFACTORTYPE; - -/** - * Quantization table type - */ - -typedef enum OMX_IMAGE_QUANTIZATIONTABLETYPE { - OMX_IMAGE_QuantizationTableLuma = 0, - OMX_IMAGE_QuantizationTableChroma, - OMX_IMAGE_QuantizationTableChromaCb, - OMX_IMAGE_QuantizationTableChromaCr, - OMX_IMAGE_QuantizationTableKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_IMAGE_QuantizationTableVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_IMAGE_QuantizationTableMax = 0x7FFFFFFF -} OMX_IMAGE_QUANTIZATIONTABLETYPE; - -/** - * JPEG quantization tables are used to determine DCT compression for - * YUV data, as an alternative to specifying Q factor, providing exact - * control of compression - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eQuantizationTable : Quantization table type - * nQuantizationMatrix[64] : JPEG quantization table of coefficients stored - * in increasing columns then by rows of data (i.e. - * row 1, ... row 8). Quantization values are in - * the range 0-255 and stored in linear order - * (i.e. the component will zig-zag the - * quantization table data if required internally) - */ -typedef struct OMX_IMAGE_PARAM_QUANTIZATIONTABLETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_IMAGE_QUANTIZATIONTABLETYPE eQuantizationTable; - OMX_U8 nQuantizationMatrix[64]; -} OMX_IMAGE_PARAM_QUANTIZATIONTABLETYPE; - - -/** - * Huffman table type, the same Huffman table is applied for chroma and - * luma component - */ -typedef enum OMX_IMAGE_HUFFMANTABLETYPE { - OMX_IMAGE_HuffmanTableAC = 0, - OMX_IMAGE_HuffmanTableDC, - OMX_IMAGE_HuffmanTableACLuma, - OMX_IMAGE_HuffmanTableACChroma, - OMX_IMAGE_HuffmanTableDCLuma, - OMX_IMAGE_HuffmanTableDCChroma, - OMX_IMAGE_HuffmanTableKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_IMAGE_HuffmanTableVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_IMAGE_HuffmanTableMax = 0x7FFFFFFF -} OMX_IMAGE_HUFFMANTABLETYPE; - -/** - * JPEG Huffman table - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eHuffmanTable : Huffman table type - * nNumberOfHuffmanCodeOfLength[16] : 0-16, number of Huffman codes of each - * possible length - * nHuffmanTable[256] : 0-255, the size used for AC and DC - * HuffmanTable are 16 and 162 - */ -typedef struct OMX_IMAGE_PARAM_HUFFMANTTABLETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_IMAGE_HUFFMANTABLETYPE eHuffmanTable; - OMX_U8 nNumberOfHuffmanCodeOfLength[16]; - OMX_U8 nHuffmanTable[256]; -}OMX_IMAGE_PARAM_HUFFMANTTABLETYPE; - -/** @} */ -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Index.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Index.h deleted file mode 100644 index 1a2a548e8..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Index.h +++ /dev/null @@ -1,274 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** @file OMX_Index.h - OpenMax IL version 1.1.2 - * The OMX_Index header file contains the definitions for both applications - * and components . - */ - - -#ifndef OMX_Index_h -#define OMX_Index_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/* Each OMX header must include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ -#include - -/** The OMX_INDEXTYPE enumeration is used to select a structure when either - * getting or setting parameters and/or configuration data. Each entry in - * this enumeration maps to an OMX specified structure. When the - * OMX_GetParameter, OMX_SetParameter, OMX_GetConfig or OMX_SetConfig methods - * are used, the second parameter will always be an entry from this enumeration - * and the third entry will be the structure shown in the comments for the entry. - * For example, if the application is initializing a cropping function, the - * OMX_SetConfig command would have OMX_IndexConfigCommonInputCrop as the second parameter - * and would send a pointer to an initialized OMX_RECTTYPE structure as the - * third parameter. - * - * The enumeration entries named with the OMX_Config prefix are sent using - * the OMX_SetConfig command and the enumeration entries named with the - * OMX_PARAM_ prefix are sent using the OMX_SetParameter command. - */ -typedef enum OMX_INDEXTYPE { - - OMX_IndexComponentStartUnused = 0x01000000, - OMX_IndexParamPriorityMgmt, /**< reference: OMX_PRIORITYMGMTTYPE */ - OMX_IndexParamAudioInit, /**< reference: OMX_PORT_PARAM_TYPE */ - OMX_IndexParamImageInit, /**< reference: OMX_PORT_PARAM_TYPE */ - OMX_IndexParamVideoInit, /**< reference: OMX_PORT_PARAM_TYPE */ - OMX_IndexParamOtherInit, /**< reference: OMX_PORT_PARAM_TYPE */ - OMX_IndexParamNumAvailableStreams, /**< reference: OMX_PARAM_U32TYPE */ - OMX_IndexParamActiveStream, /**< reference: OMX_PARAM_U32TYPE */ - OMX_IndexParamSuspensionPolicy, /**< reference: OMX_PARAM_SUSPENSIONPOLICYTYPE */ - OMX_IndexParamComponentSuspended, /**< reference: OMX_PARAM_SUSPENSIONTYPE */ - OMX_IndexConfigCapturing, /**< reference: OMX_CONFIG_BOOLEANTYPE */ - OMX_IndexConfigCaptureMode, /**< reference: OMX_CONFIG_CAPTUREMODETYPE */ - OMX_IndexAutoPauseAfterCapture, /**< reference: OMX_CONFIG_BOOLEANTYPE */ - OMX_IndexParamContentURI, /**< reference: OMX_PARAM_CONTENTURITYPE */ - OMX_IndexParamCustomContentPipe, /**< reference: OMX_PARAM_CONTENTPIPETYPE */ - OMX_IndexParamDisableResourceConcealment, /**< reference: OMX_RESOURCECONCEALMENTTYPE */ - OMX_IndexConfigMetadataItemCount, /**< reference: OMX_CONFIG_METADATAITEMCOUNTTYPE */ - OMX_IndexConfigContainerNodeCount, /**< reference: OMX_CONFIG_CONTAINERNODECOUNTTYPE */ - OMX_IndexConfigMetadataItem, /**< reference: OMX_CONFIG_METADATAITEMTYPE */ - OMX_IndexConfigCounterNodeID, /**< reference: OMX_CONFIG_CONTAINERNODEIDTYPE */ - OMX_IndexParamMetadataFilterType, /**< reference: OMX_PARAM_METADATAFILTERTYPE */ - OMX_IndexParamMetadataKeyFilter, /**< reference: OMX_PARAM_METADATAFILTERTYPE */ - OMX_IndexConfigPriorityMgmt, /**< reference: OMX_PRIORITYMGMTTYPE */ - OMX_IndexParamStandardComponentRole, /**< reference: OMX_PARAM_COMPONENTROLETYPE */ - - OMX_IndexPortStartUnused = 0x02000000, - OMX_IndexParamPortDefinition, /**< reference: OMX_PARAM_PORTDEFINITIONTYPE */ - OMX_IndexParamCompBufferSupplier, /**< reference: OMX_PARAM_BUFFERSUPPLIERTYPE */ - OMX_IndexReservedStartUnused = 0x03000000, - - /* Audio parameters and configurations */ - OMX_IndexAudioStartUnused = 0x04000000, - OMX_IndexParamAudioPortFormat, /**< reference: OMX_AUDIO_PARAM_PORTFORMATTYPE */ - OMX_IndexParamAudioPcm, /**< reference: OMX_AUDIO_PARAM_PCMMODETYPE */ - OMX_IndexParamAudioAac, /**< reference: OMX_AUDIO_PARAM_AACPROFILETYPE */ - OMX_IndexParamAudioRa, /**< reference: OMX_AUDIO_PARAM_RATYPE */ - OMX_IndexParamAudioMp3, /**< reference: OMX_AUDIO_PARAM_MP3TYPE */ - OMX_IndexParamAudioAdpcm, /**< reference: OMX_AUDIO_PARAM_ADPCMTYPE */ - OMX_IndexParamAudioG723, /**< reference: OMX_AUDIO_PARAM_G723TYPE */ - OMX_IndexParamAudioG729, /**< reference: OMX_AUDIO_PARAM_G729TYPE */ - OMX_IndexParamAudioAmr, /**< reference: OMX_AUDIO_PARAM_AMRTYPE */ - OMX_IndexParamAudioWma, /**< reference: OMX_AUDIO_PARAM_WMATYPE */ - OMX_IndexParamAudioSbc, /**< reference: OMX_AUDIO_PARAM_SBCTYPE */ - OMX_IndexParamAudioMidi, /**< reference: OMX_AUDIO_PARAM_MIDITYPE */ - OMX_IndexParamAudioGsm_FR, /**< reference: OMX_AUDIO_PARAM_GSMFRTYPE */ - OMX_IndexParamAudioMidiLoadUserSound, /**< reference: OMX_AUDIO_PARAM_MIDILOADUSERSOUNDTYPE */ - OMX_IndexParamAudioG726, /**< reference: OMX_AUDIO_PARAM_G726TYPE */ - OMX_IndexParamAudioGsm_EFR, /**< reference: OMX_AUDIO_PARAM_GSMEFRTYPE */ - OMX_IndexParamAudioGsm_HR, /**< reference: OMX_AUDIO_PARAM_GSMHRTYPE */ - OMX_IndexParamAudioPdc_FR, /**< reference: OMX_AUDIO_PARAM_PDCFRTYPE */ - OMX_IndexParamAudioPdc_EFR, /**< reference: OMX_AUDIO_PARAM_PDCEFRTYPE */ - OMX_IndexParamAudioPdc_HR, /**< reference: OMX_AUDIO_PARAM_PDCHRTYPE */ - OMX_IndexParamAudioTdma_FR, /**< reference: OMX_AUDIO_PARAM_TDMAFRTYPE */ - OMX_IndexParamAudioTdma_EFR, /**< reference: OMX_AUDIO_PARAM_TDMAEFRTYPE */ - OMX_IndexParamAudioQcelp8, /**< reference: OMX_AUDIO_PARAM_QCELP8TYPE */ - OMX_IndexParamAudioQcelp13, /**< reference: OMX_AUDIO_PARAM_QCELP13TYPE */ - OMX_IndexParamAudioEvrc, /**< reference: OMX_AUDIO_PARAM_EVRCTYPE */ - OMX_IndexParamAudioSmv, /**< reference: OMX_AUDIO_PARAM_SMVTYPE */ - OMX_IndexParamAudioVorbis, /**< reference: OMX_AUDIO_PARAM_VORBISTYPE */ - OMX_IndexParamAudioFlac, /**< reference: OMX_AUDIO_PARAM_FLACTYPE */ - - OMX_IndexConfigAudioMidiImmediateEvent, /**< reference: OMX_AUDIO_CONFIG_MIDIIMMEDIATEEVENTTYPE */ - OMX_IndexConfigAudioMidiControl, /**< reference: OMX_AUDIO_CONFIG_MIDICONTROLTYPE */ - OMX_IndexConfigAudioMidiSoundBankProgram, /**< reference: OMX_AUDIO_CONFIG_MIDISOUNDBANKPROGRAMTYPE */ - OMX_IndexConfigAudioMidiStatus, /**< reference: OMX_AUDIO_CONFIG_MIDISTATUSTYPE */ - OMX_IndexConfigAudioMidiMetaEvent, /**< reference: OMX_AUDIO_CONFIG_MIDIMETAEVENTTYPE */ - OMX_IndexConfigAudioMidiMetaEventData, /**< reference: OMX_AUDIO_CONFIG_MIDIMETAEVENTDATATYPE */ - OMX_IndexConfigAudioVolume, /**< reference: OMX_AUDIO_CONFIG_VOLUMETYPE */ - OMX_IndexConfigAudioBalance, /**< reference: OMX_AUDIO_CONFIG_BALANCETYPE */ - OMX_IndexConfigAudioChannelMute, /**< reference: OMX_AUDIO_CONFIG_CHANNELMUTETYPE */ - OMX_IndexConfigAudioMute, /**< reference: OMX_AUDIO_CONFIG_MUTETYPE */ - OMX_IndexConfigAudioLoudness, /**< reference: OMX_AUDIO_CONFIG_LOUDNESSTYPE */ - OMX_IndexConfigAudioEchoCancelation, /**< reference: OMX_AUDIO_CONFIG_ECHOCANCELATIONTYPE */ - OMX_IndexConfigAudioNoiseReduction, /**< reference: OMX_AUDIO_CONFIG_NOISEREDUCTIONTYPE */ - OMX_IndexConfigAudioBass, /**< reference: OMX_AUDIO_CONFIG_BASSTYPE */ - OMX_IndexConfigAudioTreble, /**< reference: OMX_AUDIO_CONFIG_TREBLETYPE */ - OMX_IndexConfigAudioStereoWidening, /**< reference: OMX_AUDIO_CONFIG_STEREOWIDENINGTYPE */ - OMX_IndexConfigAudioChorus, /**< reference: OMX_AUDIO_CONFIG_CHORUSTYPE */ - OMX_IndexConfigAudioEqualizer, /**< reference: OMX_AUDIO_CONFIG_EQUALIZERTYPE */ - OMX_IndexConfigAudioReverberation, /**< reference: OMX_AUDIO_CONFIG_REVERBERATIONTYPE */ - OMX_IndexConfigAudioChannelVolume, /**< reference: OMX_AUDIO_CONFIG_CHANNELVOLUMETYPE */ - - /* Image specific parameters and configurations */ - OMX_IndexImageStartUnused = 0x05000000, - OMX_IndexParamImagePortFormat, /**< reference: OMX_IMAGE_PARAM_PORTFORMATTYPE */ - OMX_IndexParamFlashControl, /**< reference: OMX_IMAGE_PARAM_FLASHCONTROLTYPE */ - OMX_IndexConfigFocusControl, /**< reference: OMX_IMAGE_CONFIG_FOCUSCONTROLTYPE */ - OMX_IndexParamQFactor, /**< reference: OMX_IMAGE_PARAM_QFACTORTYPE */ - OMX_IndexParamQuantizationTable, /**< reference: OMX_IMAGE_PARAM_QUANTIZATIONTABLETYPE */ - OMX_IndexParamHuffmanTable, /**< reference: OMX_IMAGE_PARAM_HUFFMANTTABLETYPE */ - OMX_IndexConfigFlashControl, /**< reference: OMX_IMAGE_PARAM_FLASHCONTROLTYPE */ - - /* Video specific parameters and configurations */ - OMX_IndexVideoStartUnused = 0x06000000, - OMX_IndexParamVideoPortFormat, /**< reference: OMX_VIDEO_PARAM_PORTFORMATTYPE */ - OMX_IndexParamVideoQuantization, /**< reference: OMX_VIDEO_PARAM_QUANTIZATIONTYPE */ - OMX_IndexParamVideoFastUpdate, /**< reference: OMX_VIDEO_PARAM_VIDEOFASTUPDATETYPE */ - OMX_IndexParamVideoBitrate, /**< reference: OMX_VIDEO_PARAM_BITRATETYPE */ - OMX_IndexParamVideoMotionVector, /**< reference: OMX_VIDEO_PARAM_MOTIONVECTORTYPE */ - OMX_IndexParamVideoIntraRefresh, /**< reference: OMX_VIDEO_PARAM_INTRAREFRESHTYPE */ - OMX_IndexParamVideoErrorCorrection, /**< reference: OMX_VIDEO_PARAM_ERRORCORRECTIONTYPE */ - OMX_IndexParamVideoVBSMC, /**< reference: OMX_VIDEO_PARAM_VBSMCTYPE */ - OMX_IndexParamVideoMpeg2, /**< reference: OMX_VIDEO_PARAM_MPEG2TYPE */ - OMX_IndexParamVideoMpeg4, /**< reference: OMX_VIDEO_PARAM_MPEG4TYPE */ - OMX_IndexParamVideoWmv, /**< reference: OMX_VIDEO_PARAM_WMVTYPE */ - OMX_IndexParamVideoRv, /**< reference: OMX_VIDEO_PARAM_RVTYPE */ - OMX_IndexParamVideoAvc, /**< reference: OMX_VIDEO_PARAM_AVCTYPE */ - OMX_IndexParamVideoH263, /**< reference: OMX_VIDEO_PARAM_H263TYPE */ - OMX_IndexParamVideoProfileLevelQuerySupported, /**< reference: OMX_VIDEO_PARAM_PROFILELEVELTYPE */ - OMX_IndexParamVideoProfileLevelCurrent, /**< reference: OMX_VIDEO_PARAM_PROFILELEVELTYPE */ - OMX_IndexConfigVideoBitrate, /**< reference: OMX_VIDEO_CONFIG_BITRATETYPE */ - OMX_IndexConfigVideoFramerate, /**< reference: OMX_CONFIG_FRAMERATETYPE */ - OMX_IndexConfigVideoIntraVOPRefresh, /**< reference: OMX_CONFIG_INTRAREFRESHVOPTYPE */ - OMX_IndexConfigVideoIntraMBRefresh, /**< reference: OMX_CONFIG_MACROBLOCKERRORMAPTYPE */ - OMX_IndexConfigVideoMBErrorReporting, /**< reference: OMX_CONFIG_MBERRORREPORTINGTYPE */ - OMX_IndexParamVideoMacroblocksPerFrame, /**< reference: OMX_PARAM_MACROBLOCKSTYPE */ - OMX_IndexConfigVideoMacroBlockErrorMap, /**< reference: OMX_CONFIG_MACROBLOCKERRORMAPTYPE */ - OMX_IndexParamVideoSliceFMO, /**< reference: OMX_VIDEO_PARAM_AVCSLICEFMO */ - OMX_IndexConfigVideoAVCIntraPeriod, /**< reference: OMX_VIDEO_CONFIG_AVCINTRAPERIOD */ - OMX_IndexConfigVideoNalSize, /**< reference: OMX_VIDEO_CONFIG_NALSIZE */ - - /* Image & Video common Configurations */ - OMX_IndexCommonStartUnused = 0x07000000, - OMX_IndexParamCommonDeblocking, /**< reference: OMX_PARAM_DEBLOCKINGTYPE */ - OMX_IndexParamCommonSensorMode, /**< reference: OMX_PARAM_SENSORMODETYPE */ - OMX_IndexParamCommonInterleave, /**< reference: OMX_PARAM_INTERLEAVETYPE */ - OMX_IndexConfigCommonColorFormatConversion, /**< reference: OMX_CONFIG_COLORCONVERSIONTYPE */ - OMX_IndexConfigCommonScale, /**< reference: OMX_CONFIG_SCALEFACTORTYPE */ - OMX_IndexConfigCommonImageFilter, /**< reference: OMX_CONFIG_IMAGEFILTERTYPE */ - OMX_IndexConfigCommonColorEnhancement, /**< reference: OMX_CONFIG_COLORENHANCEMENTTYPE */ - OMX_IndexConfigCommonColorKey, /**< reference: OMX_CONFIG_COLORKEYTYPE */ - OMX_IndexConfigCommonColorBlend, /**< reference: OMX_CONFIG_COLORBLENDTYPE */ - OMX_IndexConfigCommonFrameStabilisation,/**< reference: OMX_CONFIG_FRAMESTABTYPE */ - OMX_IndexConfigCommonRotate, /**< reference: OMX_CONFIG_ROTATIONTYPE */ - OMX_IndexConfigCommonMirror, /**< reference: OMX_CONFIG_MIRRORTYPE */ - OMX_IndexConfigCommonOutputPosition, /**< reference: OMX_CONFIG_POINTTYPE */ - OMX_IndexConfigCommonInputCrop, /**< reference: OMX_CONFIG_RECTTYPE */ - OMX_IndexConfigCommonOutputCrop, /**< reference: OMX_CONFIG_RECTTYPE */ - OMX_IndexConfigCommonDigitalZoom, /**< reference: OMX_CONFIG_SCALEFACTORTYPE */ - OMX_IndexConfigCommonOpticalZoom, /**< reference: OMX_CONFIG_SCALEFACTORTYPE*/ - OMX_IndexConfigCommonWhiteBalance, /**< reference: OMX_CONFIG_WHITEBALCONTROLTYPE */ - OMX_IndexConfigCommonExposure, /**< reference: OMX_CONFIG_EXPOSURECONTROLTYPE */ - OMX_IndexConfigCommonContrast, /**< reference: OMX_CONFIG_CONTRASTTYPE */ - OMX_IndexConfigCommonBrightness, /**< reference: OMX_CONFIG_BRIGHTNESSTYPE */ - OMX_IndexConfigCommonBacklight, /**< reference: OMX_CONFIG_BACKLIGHTTYPE */ - OMX_IndexConfigCommonGamma, /**< reference: OMX_CONFIG_GAMMATYPE */ - OMX_IndexConfigCommonSaturation, /**< reference: OMX_CONFIG_SATURATIONTYPE */ - OMX_IndexConfigCommonLightness, /**< reference: OMX_CONFIG_LIGHTNESSTYPE */ - OMX_IndexConfigCommonExclusionRect, /**< reference: OMX_CONFIG_RECTTYPE */ - OMX_IndexConfigCommonDithering, /**< reference: OMX_CONFIG_DITHERTYPE */ - OMX_IndexConfigCommonPlaneBlend, /**< reference: OMX_CONFIG_PLANEBLENDTYPE */ - OMX_IndexConfigCommonExposureValue, /**< reference: OMX_CONFIG_EXPOSUREVALUETYPE */ - OMX_IndexConfigCommonOutputSize, /**< reference: OMX_FRAMESIZETYPE */ - OMX_IndexParamCommonExtraQuantData, /**< reference: OMX_OTHER_EXTRADATATYPE */ - OMX_IndexConfigCommonFocusRegion, /**< reference: OMX_CONFIG_FOCUSREGIONTYPE */ - OMX_IndexConfigCommonFocusStatus, /**< reference: OMX_PARAM_FOCUSSTATUSTYPE */ - OMX_IndexConfigCommonTransitionEffect, /**< reference: OMX_CONFIG_TRANSITIONEFFECTTYPE */ - - /* Reserved Configuration range */ - OMX_IndexOtherStartUnused = 0x08000000, - OMX_IndexParamOtherPortFormat, /**< reference: OMX_OTHER_PARAM_PORTFORMATTYPE */ - OMX_IndexConfigOtherPower, /**< reference: OMX_OTHER_CONFIG_POWERTYPE */ - OMX_IndexConfigOtherStats, /**< reference: OMX_OTHER_CONFIG_STATSTYPE */ - - - /* Reserved Time range */ - OMX_IndexTimeStartUnused = 0x09000000, - OMX_IndexConfigTimeScale, /**< reference: OMX_TIME_CONFIG_SCALETYPE */ - OMX_IndexConfigTimeClockState, /**< reference: OMX_TIME_CONFIG_CLOCKSTATETYPE */ - OMX_IndexConfigTimeActiveRefClock, /**< reference: OMX_TIME_CONFIG_ACTIVEREFCLOCKTYPE */ - OMX_IndexConfigTimeCurrentMediaTime, /**< reference: OMX_TIME_CONFIG_TIMESTAMPTYPE (read only) */ - OMX_IndexConfigTimeCurrentWallTime, /**< reference: OMX_TIME_CONFIG_TIMESTAMPTYPE (read only) */ - OMX_IndexConfigTimeCurrentAudioReference, /**< reference: OMX_TIME_CONFIG_TIMESTAMPTYPE (write only) */ - OMX_IndexConfigTimeCurrentVideoReference, /**< reference: OMX_TIME_CONFIG_TIMESTAMPTYPE (write only) */ - OMX_IndexConfigTimeMediaTimeRequest, /**< reference: OMX_TIME_CONFIG_MEDIATIMEREQUESTTYPE (write only) */ - OMX_IndexConfigTimeClientStartTime, /** - - -/** Khronos standard extension indices. - -This enum lists the current Khronos extension indices to OpenMAX IL. -*/ -typedef enum OMX_INDEXEXTTYPE { - - /* Component parameters and configurations */ - OMX_IndexExtComponentStartUnused = OMX_IndexKhronosExtensions + 0x00100000, - OMX_IndexConfigCallbackRequest, /**< reference: OMX_CONFIG_CALLBACKREQUESTTYPE */ - OMX_IndexConfigCommitMode, /**< reference: OMX_CONFIG_COMMITMODETYPE */ - OMX_IndexConfigCommit, /**< reference: OMX_CONFIG_COMMITTYPE */ - - /* Port parameters and configurations */ - OMX_IndexExtPortStartUnused = OMX_IndexKhronosExtensions + 0x00200000, - - /* Audio parameters and configurations */ - OMX_IndexExtAudioStartUnused = OMX_IndexKhronosExtensions + 0x00400000, - OMX_IndexParamAudioAndroidAc3, /**< reference: OMX_AUDIO_PARAM_ANDROID_AC3TYPE */ - OMX_IndexParamAudioAndroidOpus, /**< reference: OMX_AUDIO_PARAM_ANDROID_OPUSTYPE */ - OMX_IndexParamAudioAndroidAacPresentation, /**< reference: OMX_AUDIO_PARAM_ANDROID_AACPRESENTATIONTYPE */ - OMX_IndexParamAudioAndroidEac3, /**< reference: OMX_AUDIO_PARAM_ANDROID_EAC3TYPE */ - - /* Image parameters and configurations */ - OMX_IndexExtImageStartUnused = OMX_IndexKhronosExtensions + 0x00500000, - - /* Video parameters and configurations */ - OMX_IndexExtVideoStartUnused = OMX_IndexKhronosExtensions + 0x00600000, - OMX_IndexParamNalStreamFormatSupported, /**< reference: OMX_NALSTREAMFORMATTYPE */ - OMX_IndexParamNalStreamFormat, /**< reference: OMX_NALSTREAMFORMATTYPE */ - OMX_IndexParamNalStreamFormatSelect, /**< reference: OMX_NALSTREAMFORMATTYPE */ - OMX_IndexParamVideoVp8, /**< reference: OMX_VIDEO_PARAM_VP8TYPE */ - OMX_IndexConfigVideoVp8ReferenceFrame, /**< reference: OMX_VIDEO_VP8REFERENCEFRAMETYPE */ - OMX_IndexConfigVideoVp8ReferenceFrameType, /**< reference: OMX_VIDEO_VP8REFERENCEFRAMEINFOTYPE */ - OMX_IndexParamVideoAndroidVp8Encoder, /**< reference: OMX_VIDEO_PARAM_ANDROID_VP8ENCODERTYPE */ - OMX_IndexParamVideoHevc, /**< reference: OMX_VIDEO_PARAM_HEVCTYPE */ - OMX_IndexParamSliceSegments, /**< reference: OMX_VIDEO_SLICESEGMENTSTYPE */ - - /* Image & Video common configurations */ - OMX_IndexExtCommonStartUnused = OMX_IndexKhronosExtensions + 0x00700000, - - /* Other configurations */ - OMX_IndexExtOtherStartUnused = OMX_IndexKhronosExtensions + 0x00800000, - OMX_IndexConfigAutoFramerateConversion, /**< reference: OMX_CONFIG_BOOLEANTYPE */ - OMX_IndexConfigPriority, /**< reference: OMX_PARAM_U32TYPE */ - OMX_IndexConfigOperatingRate, /**< reference: OMX_PARAM_U32TYPE in Q16 format for video and in Hz for audio */ - OMX_IndexParamConsumerUsageBits, /**< reference: OMX_PARAM_U32TYPE */ - - /* Time configurations */ - OMX_IndexExtTimeStartUnused = OMX_IndexKhronosExtensions + 0x00900000, - - OMX_IndexExtMax = 0x7FFFFFFF -} OMX_INDEXEXTTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif /* OMX_IndexExt_h */ -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Other.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Other.h deleted file mode 100644 index 6072ef62c..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Other.h +++ /dev/null @@ -1,354 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** @file OMX_Other.h - OpenMax IL version 1.1.2 - * The structures needed by Other components to exchange - * parameters and configuration data with the components. - */ - -#ifndef OMX_Other_h -#define OMX_Other_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - - -/* Each OMX header must include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ - -#include - - -/** - * Enumeration of possible data types which match to multiple domains or no - * domain at all. For types which are vendor specific, a value above - * OMX_OTHER_VENDORTSTART should be used. - */ -typedef enum OMX_OTHER_FORMATTYPE { - OMX_OTHER_FormatTime = 0, /**< Transmission of various timestamps, elapsed time, - time deltas, etc */ - OMX_OTHER_FormatPower, /**< Perhaps used for enabling/disabling power - management, setting clocks? */ - OMX_OTHER_FormatStats, /**< Could be things such as frame rate, frames - dropped, etc */ - OMX_OTHER_FormatBinary, /**< Arbitrary binary data */ - OMX_OTHER_FormatVendorReserved = 1000, /**< Starting value for vendor specific - formats */ - - OMX_OTHER_FormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_OTHER_FormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_OTHER_FormatMax = 0x7FFFFFFF -} OMX_OTHER_FORMATTYPE; - -/** - * Enumeration of seek modes. - */ -typedef enum OMX_TIME_SEEKMODETYPE { - OMX_TIME_SeekModeFast = 0, /**< Prefer seeking to an approximation - * of the requested seek position over - * the actual seek position if it - * results in a faster seek. */ - OMX_TIME_SeekModeAccurate, /**< Prefer seeking to the actual seek - * position over an approximation - * of the requested seek position even - * if it results in a slower seek. */ - OMX_TIME_SeekModeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_TIME_SeekModeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_TIME_SeekModeMax = 0x7FFFFFFF -} OMX_TIME_SEEKMODETYPE; - -/* Structure representing the seekmode of the component */ -typedef struct OMX_TIME_CONFIG_SEEKMODETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_TIME_SEEKMODETYPE eType; /**< The seek mode */ -} OMX_TIME_CONFIG_SEEKMODETYPE; - -/** Structure representing a time stamp used with the following configs - * on the Clock Component (CC): - * - * OMX_IndexConfigTimeCurrentWallTime: query of the CC's current wall - * time - * OMX_IndexConfigTimeCurrentMediaTime: query of the CC's current media - * time - * OMX_IndexConfigTimeCurrentAudioReference and - * OMX_IndexConfigTimeCurrentVideoReference: audio/video reference - * clock sending SC its reference time - * OMX_IndexConfigTimeClientStartTime: a Clock Component client sends - * this structure to the Clock Component via a SetConfig on its - * client port when it receives a buffer with - * OMX_BUFFERFLAG_STARTTIME set. It must use the timestamp - * specified by that buffer for nStartTimestamp. - * - * It's also used with the following config on components in general: - * - * OMX_IndexConfigTimePosition: IL client querying component position - * (GetConfig) or commanding a component to seek to the given location - * (SetConfig) - */ -typedef struct OMX_TIME_CONFIG_TIMESTAMPTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version - * information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_TICKS nTimestamp; /**< timestamp .*/ -} OMX_TIME_CONFIG_TIMESTAMPTYPE; - -/** Enumeration of possible reference clocks to the media time. */ -typedef enum OMX_TIME_UPDATETYPE { - OMX_TIME_UpdateRequestFulfillment, /**< Update is the fulfillment of a media time request. */ - OMX_TIME_UpdateScaleChanged, /**< Update was generated because the scale chagned. */ - OMX_TIME_UpdateClockStateChanged, /**< Update was generated because the clock state changed. */ - OMX_TIME_UpdateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_TIME_UpdateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_TIME_UpdateMax = 0x7FFFFFFF -} OMX_TIME_UPDATETYPE; - -/** Enumeration of possible reference clocks to the media time. */ -typedef enum OMX_TIME_REFCLOCKTYPE { - OMX_TIME_RefClockNone, /**< Use no references. */ - OMX_TIME_RefClockAudio, /**< Use references sent through OMX_IndexConfigTimeCurrentAudioReference */ - OMX_TIME_RefClockVideo, /**< Use references sent through OMX_IndexConfigTimeCurrentVideoReference */ - OMX_TIME_RefClockKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_TIME_RefClockVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_TIME_RefClockMax = 0x7FFFFFFF -} OMX_TIME_REFCLOCKTYPE; - -/** Enumeration of clock states. */ -typedef enum OMX_TIME_CLOCKSTATE { - OMX_TIME_ClockStateRunning, /**< Clock running. */ - OMX_TIME_ClockStateWaitingForStartTime, /**< Clock waiting until the - * prescribed clients emit their - * start time. */ - OMX_TIME_ClockStateStopped, /**< Clock stopped. */ - OMX_TIME_ClockStateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_TIME_ClockStateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_TIME_ClockStateMax = 0x7FFFFFFF -} OMX_TIME_CLOCKSTATE; - -/** Structure representing a media time request to the clock component. - * - * A client component sends this structure to the Clock Component via a SetConfig - * on its client port to specify a media timestamp the Clock Component - * should emit. The Clock Component should fulfill the request by sending a - * OMX_TIME_MEDIATIMETYPE when its media clock matches the requested - * timestamp. - * - * The client may require a media time request be fulfilled slightly - * earlier than the media time specified. In this case the client specifies - * an offset which is equal to the difference between wall time corresponding - * to the requested media time and the wall time when it will be - * fulfilled. - * - * A client component may uses these requests and the OMX_TIME_MEDIATIMETYPE to - * time events according to timestamps. If a client must perform an operation O at - * a time T (e.g. deliver a video frame at its corresponding timestamp), it makes a - * media time request at T (perhaps specifying an offset to ensure the request fulfillment - * is a little early). When the clock component passes the resulting OMX_TIME_MEDIATIMETYPE - * structure back to the client component, the client may perform operation O (perhaps having - * to wait a slight amount more time itself as specified by the return values). - */ - -typedef struct OMX_TIME_CONFIG_MEDIATIMEREQUESTTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< port that this structure applies to */ - OMX_PTR pClientPrivate; /**< Client private data to disabiguate this media time - * from others (e.g. the number of the frame to deliver). - * Duplicated in the media time structure that fulfills - * this request. A value of zero is reserved for time scale - * updates. */ - OMX_TICKS nMediaTimestamp; /**< Media timestamp requested.*/ - OMX_TICKS nOffset; /**< Amount of wall clock time by which this - * request should be fulfilled early */ -} OMX_TIME_CONFIG_MEDIATIMEREQUESTTYPE; - -/**< Structure sent from the clock component client either when fulfilling - * a media time request or when the time scale has changed. - * - * In the former case the Clock Component fills this structure and times its emission - * to a client component (via the client port) according to the corresponding media - * time request sent by the client. The Clock Component should time the emission to occur - * when the requested timestamp matches the Clock Component's media time but also the - * prescribed offset early. - * - * Upon scale changes the clock component clears the nClientPrivate data, sends the current - * media time and sets the nScale to the new scale via the client port. It emits a - * OMX_TIME_MEDIATIMETYPE to all clients independent of any requests. This allows clients to - * alter processing to accomodate scaling. For instance a video component might skip inter-frames - * in the case of extreme fastforward. Likewise an audio component might add or remove samples - * from an audio frame to scale audio data. - * - * It is expected that some clock components may not be able to fulfill requests - * at exactly the prescribed time. This is acceptable so long as the request is - * fulfilled at least as early as described and not later. This structure provides - * fields the client may use to wait for the remaining time. - * - * The client may use either the nOffset or nWallTimeAtMedia fields to determine the - * wall time until the nMediaTimestamp actually occurs. In the latter case the - * client can get a more accurate value for offset by getting the current wall - * from the cloc component and subtracting it from nWallTimeAtMedia. - */ - -typedef struct OMX_TIME_MEDIATIMETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nClientPrivate; /**< Client private data to disabiguate this media time - * from others. Copied from the media time request. - * A value of zero is reserved for time scale updates. */ - OMX_TIME_UPDATETYPE eUpdateType; /**< Reason for the update */ - OMX_TICKS nMediaTimestamp; /**< Media time requested. If no media time was - * requested then this is the current media time. */ - OMX_TICKS nOffset; /**< Amount of wall clock time by which this - * request was actually fulfilled early */ - - OMX_TICKS nWallTimeAtMediaTime; /**< Wall time corresponding to nMediaTimeStamp. - * A client may compare this value to current - * media time obtained from the Clock Component to determine - * the wall time until the media timestamp is really - * current. */ - OMX_S32 xScale; /**< Current media time scale in Q16 format. */ - OMX_TIME_CLOCKSTATE eState; /* Seeking Change. Added 7/12.*/ - /**< State of the media time. */ -} OMX_TIME_MEDIATIMETYPE; - -/** Structure representing the current media time scale factor. Applicable only to clock - * component, other components see scale changes via OMX_TIME_MEDIATIMETYPE buffers sent via - * the clock component client ports. Upon recieving this config the clock component changes - * the rate by which the media time increases or decreases effectively implementing trick modes. - */ -typedef struct OMX_TIME_CONFIG_SCALETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_S32 xScale; /**< This is a value in Q16 format which is used for - * scaling the media time */ -} OMX_TIME_CONFIG_SCALETYPE; - -/** Bits used to identify a clock port. Used in OMX_TIME_CONFIG_CLOCKSTATETYPE's nWaitMask field */ -#define OMX_CLOCKPORT0 0x00000001 -#define OMX_CLOCKPORT1 0x00000002 -#define OMX_CLOCKPORT2 0x00000004 -#define OMX_CLOCKPORT3 0x00000008 -#define OMX_CLOCKPORT4 0x00000010 -#define OMX_CLOCKPORT5 0x00000020 -#define OMX_CLOCKPORT6 0x00000040 -#define OMX_CLOCKPORT7 0x00000080 - -/** Structure representing the current mode of the media clock. - * IL Client uses this config to change or query the mode of the - * media clock of the clock component. Applicable only to clock - * component. - * - * On a SetConfig if eState is OMX_TIME_ClockStateRunning media time - * starts immediately at the prescribed start time. If - * OMX_TIME_ClockStateWaitingForStartTime the Clock Component ignores - * the given nStartTime and waits for all clients specified in the - * nWaitMask to send starttimes (via - * OMX_IndexConfigTimeClientStartTime). The Clock Component then starts - * the media clock using the earliest start time supplied. */ -typedef struct OMX_TIME_CONFIG_CLOCKSTATETYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version - * information */ - OMX_TIME_CLOCKSTATE eState; /**< State of the media time. */ - OMX_TICKS nStartTime; /**< Start time of the media time. */ - OMX_TICKS nOffset; /**< Time to offset the media time by - * (e.g. preroll). Media time will be - * reported to be nOffset ticks earlier. - */ - OMX_U32 nWaitMask; /**< Mask of OMX_CLOCKPORT values. */ -} OMX_TIME_CONFIG_CLOCKSTATETYPE; - -/** Structure representing the reference clock currently being used to - * compute media time. IL client uses this config to change or query the - * clock component's active reference clock */ -typedef struct OMX_TIME_CONFIG_ACTIVEREFCLOCKTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_TIME_REFCLOCKTYPE eClock; /**< Reference clock used to compute media time */ -} OMX_TIME_CONFIG_ACTIVEREFCLOCKTYPE; - -/** Descriptor for setting specifics of power type. - * Note: this structure is listed for backwards compatibility. */ -typedef struct OMX_OTHER_CONFIG_POWERTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_BOOL bEnablePM; /**< Flag to enable Power Management */ -} OMX_OTHER_CONFIG_POWERTYPE; - - -/** Descriptor for setting specifics of stats type. - * Note: this structure is listed for backwards compatibility. */ -typedef struct OMX_OTHER_CONFIG_STATSTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - /* what goes here */ -} OMX_OTHER_CONFIG_STATSTYPE; - - -/** - * The PortDefinition structure is used to define all of the parameters - * necessary for the compliant component to setup an input or an output other - * path. - */ -typedef struct OMX_OTHER_PORTDEFINITIONTYPE { - OMX_OTHER_FORMATTYPE eFormat; /**< Type of data expected for this channel */ -} OMX_OTHER_PORTDEFINITIONTYPE; - -/** Port format parameter. This structure is used to enumerate - * the various data input/output format supported by the port. - */ -typedef struct OMX_OTHER_PARAM_PORTFORMATTYPE { - OMX_U32 nSize; /**< size of the structure in bytes */ - OMX_VERSIONTYPE nVersion; /**< OMX specification version information */ - OMX_U32 nPortIndex; /**< Indicates which port to set */ - OMX_U32 nIndex; /**< Indicates the enumeration index for the format from 0x0 to N-1 */ - OMX_OTHER_FORMATTYPE eFormat; /**< Type of data expected for this channel */ -} OMX_OTHER_PARAM_PORTFORMATTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Types.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Types.h deleted file mode 100644 index 5afaba057..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Types.h +++ /dev/null @@ -1,387 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/* - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_Types.h - OpenMax IL version 1.1.2 - * The OMX_Types header file contains the primitive type definitions used by - * the core, the application and the component. This file may need to be - * modified to be used on systems that do not have "char" set to 8 bits, - * "short" set to 16 bits and "long" set to 32 bits. - */ - -#ifndef OMX_Types_h -#define OMX_Types_h - -#include - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/** The OMX_API and OMX_APIENTRY are platform specific definitions used - * to declare OMX function prototypes. They are modified to meet the - * requirements for a particular platform */ -#ifdef __SYMBIAN32__ -# ifdef __OMX_EXPORTS -# define OMX_API __declspec(dllexport) -# else -# ifdef _WIN32 -# define OMX_API __declspec(dllexport) -# else -# define OMX_API __declspec(dllimport) -# endif -# endif -#else -# ifdef _WIN32 -# ifdef __OMX_EXPORTS -# define OMX_API __declspec(dllexport) -# else -//# define OMX_API __declspec(dllimport) -#define OMX_API -# endif -# else -# ifdef __OMX_EXPORTS -# define OMX_API -# else -# define OMX_API extern -# endif -# endif -#endif - -#ifndef OMX_APIENTRY -#define OMX_APIENTRY -#endif - -/** OMX_IN is used to identify inputs to an OMX function. This designation - will also be used in the case of a pointer that points to a parameter - that is used as an output. */ -#ifndef OMX_IN -#define OMX_IN -#endif - -/** OMX_OUT is used to identify outputs from an OMX function. This - designation will also be used in the case of a pointer that points - to a parameter that is used as an input. */ -#ifndef OMX_OUT -#define OMX_OUT -#endif - - -/** OMX_INOUT is used to identify parameters that may be either inputs or - outputs from an OMX function at the same time. This designation will - also be used in the case of a pointer that points to a parameter that - is used both as an input and an output. */ -#ifndef OMX_INOUT -#define OMX_INOUT -#endif - -/** OMX_ALL is used to as a wildcard to select all entities of the same type - * when specifying the index, or referring to a object by an index. (i.e. - * use OMX_ALL to indicate all N channels). When used as a port index - * for a config or parameter this OMX_ALL denotes that the config or - * parameter applies to the entire component not just one port. */ -#define OMX_ALL 0xFFFFFFFF - -/** In the following we define groups that help building doxygen documentation */ - -/** @defgroup core OpenMAX IL core - * Functions and structure related to the OMX IL core - */ - - /** @defgroup comp OpenMAX IL component - * Functions and structure related to the OMX IL component - */ - -/** @defgroup rpm Resource and Policy Management - * Structures for resource and policy management of components - */ - -/** @defgroup buf Buffer Management - * Buffer handling functions and structures - */ - -/** @defgroup tun Tunneling - * @ingroup core comp - * Structures and functions to manage tunnels among component ports - */ - -/** @defgroup cp Content Pipes - * @ingroup core - */ - - /** @defgroup metadata Metadata handling - * - */ - -/** OMX_U8 is an 8 bit unsigned quantity that is byte aligned */ -typedef unsigned char OMX_U8; - -/** OMX_S8 is an 8 bit signed quantity that is byte aligned */ -typedef signed char OMX_S8; - -/** OMX_U16 is a 16 bit unsigned quantity that is 16 bit word aligned */ -typedef unsigned short OMX_U16; - -/** OMX_S16 is a 16 bit signed quantity that is 16 bit word aligned */ -typedef signed short OMX_S16; - -/** OMX_U32 is a 32 bit unsigned quantity that is 32 bit word aligned */ -typedef uint32_t OMX_U32; - -/** OMX_S32 is a 32 bit signed quantity that is 32 bit word aligned */ -typedef int32_t OMX_S32; - - -/* Users with compilers that cannot accept the "long long" designation should - define the OMX_SKIP64BIT macro. It should be noted that this may cause - some components to fail to compile if the component was written to require - 64 bit integral types. However, these components would NOT compile anyway - since the compiler does not support the way the component was written. -*/ -#ifndef OMX_SKIP64BIT -#ifdef __SYMBIAN32__ -/** OMX_U64 is a 64 bit unsigned quantity that is 64 bit word aligned */ -typedef unsigned long long OMX_U64; - -/** OMX_S64 is a 64 bit signed quantity that is 64 bit word aligned */ -typedef signed long long OMX_S64; - -#elif defined(WIN32) - -/** OMX_U64 is a 64 bit unsigned quantity that is 64 bit word aligned */ -typedef unsigned __int64 OMX_U64; - -/** OMX_S64 is a 64 bit signed quantity that is 64 bit word aligned */ -typedef signed __int64 OMX_S64; - -#else /* WIN32 */ - -/** OMX_U64 is a 64 bit unsigned quantity that is 64 bit word aligned */ -typedef unsigned long long OMX_U64; - -/** OMX_S64 is a 64 bit signed quantity that is 64 bit word aligned */ -typedef signed long long OMX_S64; - -#endif /* WIN32 */ -#endif - - -/** The OMX_BOOL type is intended to be used to represent a true or a false - value when passing parameters to and from the OMX core and components. The - OMX_BOOL is a 32 bit quantity and is aligned on a 32 bit word boundary. - */ -typedef enum OMX_BOOL { - OMX_FALSE = 0, - OMX_TRUE = !OMX_FALSE, - OMX_BOOL_MAX = 0x7FFFFFFF -} OMX_BOOL; - -/* - * Temporary Android 64 bit modification - * - * #define OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS - * overrides all OMX pointer types to be uint32_t. - * - * After this change, OMX codecs will work in 32 bit only, so 64 bit processes - * must communicate to a remote 32 bit process for OMX to work. - */ - -#ifdef OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS - -typedef uint32_t OMX_PTR; -typedef OMX_PTR OMX_STRING; -typedef OMX_PTR OMX_BYTE; - -#else /* OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS */ - -/** The OMX_PTR type is intended to be used to pass pointers between the OMX - applications and the OMX Core and components. This is a 32 bit pointer and - is aligned on a 32 bit boundary. - */ -typedef void* OMX_PTR; - -/** The OMX_STRING type is intended to be used to pass "C" type strings between - the application and the core and component. The OMX_STRING type is a 32 - bit pointer to a zero terminated string. The pointer is word aligned and - the string is byte aligned. - */ -typedef char* OMX_STRING; - -/** The OMX_BYTE type is intended to be used to pass arrays of bytes such as - buffers between the application and the component and core. The OMX_BYTE - type is a 32 bit pointer to a zero terminated string. The pointer is word - aligned and the string is byte aligned. - */ -typedef unsigned char* OMX_BYTE; - -#endif /* OMX_ANDROID_COMPILE_AS_32BIT_ON_64BIT_PLATFORMS */ - -/** OMX_UUIDTYPE is a very long unique identifier to uniquely identify - at runtime. This identifier should be generated by a component in a way - that guarantees that every instance of the identifier running on the system - is unique. */ -typedef unsigned char OMX_UUIDTYPE[128]; - -/** The OMX_DIRTYPE enumeration is used to indicate if a port is an input or - an output port. This enumeration is common across all component types. - */ -typedef enum OMX_DIRTYPE -{ - OMX_DirInput, /**< Port is an input port */ - OMX_DirOutput, /**< Port is an output port */ - OMX_DirMax = 0x7FFFFFFF -} OMX_DIRTYPE; - -/** The OMX_ENDIANTYPE enumeration is used to indicate the bit ordering - for numerical data (i.e. big endian, or little endian). - */ -typedef enum OMX_ENDIANTYPE -{ - OMX_EndianBig, /**< big endian */ - OMX_EndianLittle, /**< little endian */ - OMX_EndianMax = 0x7FFFFFFF -} OMX_ENDIANTYPE; - - -/** The OMX_NUMERICALDATATYPE enumeration is used to indicate if data - is signed or unsigned - */ -typedef enum OMX_NUMERICALDATATYPE -{ - OMX_NumericalDataSigned, /**< signed data */ - OMX_NumericalDataUnsigned, /**< unsigned data */ - OMX_NumercialDataMax = 0x7FFFFFFF -} OMX_NUMERICALDATATYPE; - - -/** Unsigned bounded value type */ -typedef struct OMX_BU32 { - OMX_U32 nValue; /**< actual value */ - OMX_U32 nMin; /**< minimum for value (i.e. nValue >= nMin) */ - OMX_U32 nMax; /**< maximum for value (i.e. nValue <= nMax) */ -} OMX_BU32; - - -/** Signed bounded value type */ -typedef struct OMX_BS32 { - OMX_S32 nValue; /**< actual value */ - OMX_S32 nMin; /**< minimum for value (i.e. nValue >= nMin) */ - OMX_S32 nMax; /**< maximum for value (i.e. nValue <= nMax) */ -} OMX_BS32; - - -/** Structure representing some time or duration in microseconds. This structure - * must be interpreted as a signed 64 bit value. The quantity is signed to accommodate - * negative deltas and preroll scenarios. The quantity is represented in microseconds - * to accomodate high resolution timestamps (e.g. DVD presentation timestamps based - * on a 90kHz clock) and to allow more accurate and synchronized delivery (e.g. - * individual audio samples delivered at 192 kHz). The quantity is 64 bit to - * accommodate a large dynamic range (signed 32 bit values would allow only for plus - * or minus 35 minutes). - * - * Implementations with limited precision may convert the signed 64 bit value to - * a signed 32 bit value internally but risk loss of precision. - */ -#ifndef OMX_SKIP64BIT -typedef OMX_S64 OMX_TICKS; -#else -typedef struct OMX_TICKS -{ - OMX_U32 nLowPart; /** low bits of the signed 64 bit tick value */ - OMX_U32 nHighPart; /** high bits of the signed 64 bit tick value */ -} OMX_TICKS; -#endif -#define OMX_TICKS_PER_SECOND 1000000 - -/** Define the public interface for the OMX Handle. The core will not use - this value internally, but the application should only use this value. - */ -typedef OMX_PTR OMX_HANDLETYPE; - -typedef struct OMX_MARKTYPE -{ - OMX_HANDLETYPE hMarkTargetComponent; /**< The component that will - generate a mark event upon - processing the mark. */ - OMX_PTR pMarkData; /**< Application specific data associated with - the mark sent on a mark event to disambiguate - this mark from others. */ -} OMX_MARKTYPE; - - -/** OMX_NATIVE_DEVICETYPE is used to map a OMX video port to the - * platform & operating specific object used to reference the display - * or can be used by a audio port for native audio rendering */ -typedef OMX_PTR OMX_NATIVE_DEVICETYPE; - -/** OMX_NATIVE_WINDOWTYPE is used to map a OMX video port to the - * platform & operating specific object used to reference the window */ -typedef OMX_PTR OMX_NATIVE_WINDOWTYPE; - -/** The OMX_VERSIONTYPE union is used to specify the version for - a structure or component. For a component, the version is entirely - specified by the component vendor. Components doing the same function - from different vendors may or may not have the same version. For - structures, the version shall be set by the entity that allocates the - structure. For structures specified in the OMX 1.1 specification, the - value of the version shall be set to 1.1.0.0 in all cases. Access to the - OMX_VERSIONTYPE can be by a single 32 bit access (e.g. by nVersion) or - by accessing one of the structure elements to, for example, check only - the Major revision. - */ -typedef union OMX_VERSIONTYPE -{ - struct - { - OMX_U8 nVersionMajor; /**< Major version accessor element */ - OMX_U8 nVersionMinor; /**< Minor version accessor element */ - OMX_U8 nRevision; /**< Revision version accessor element */ - OMX_U8 nStep; /**< Step version accessor element */ - } s; - OMX_U32 nVersion; /**< 32 bit value to make accessing the - version easily done in a single word - size copy/compare operation */ -} OMX_VERSIONTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Video.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_Video.h deleted file mode 100644 index decc410ba..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_Video.h +++ /dev/null @@ -1,1081 +0,0 @@ -/* ------------------------------------------------------------------ - * Copyright (C) 1998-2009 PacketVideo - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either - * express or implied. - * See the License for the specific language governing permissions - * and limitations under the License. - * ------------------------------------------------------------------- - */ -/** - * Copyright (c) 2008 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** - * @file OMX_Video.h - OpenMax IL version 1.1.2 - * The structures is needed by Video components to exchange parameters - * and configuration data with OMX components. - */ -#ifndef OMX_Video_h -#define OMX_Video_h - -/** @defgroup video OpenMAX IL Video Domain - * @ingroup iv - * Structures for OpenMAX IL Video domain - * @{ - */ - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - - -/** - * Each OMX header must include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ - -#include - - -/** - * Enumeration used to define the possible video compression codings. - * NOTE: This essentially refers to file extensions. If the coding is - * being used to specify the ENCODE type, then additional work - * must be done to configure the exact flavor of the compression - * to be used. For decode cases where the user application can - * not differentiate between MPEG-4 and H.264 bit streams, it is - * up to the codec to handle this. - */ -typedef enum OMX_VIDEO_CODINGTYPE { - OMX_VIDEO_CodingUnused, /**< Value when coding is N/A */ - OMX_VIDEO_CodingAutoDetect, /**< Autodetection of coding type */ - OMX_VIDEO_CodingMPEG2, /**< AKA: H.262 */ - OMX_VIDEO_CodingH263, /**< H.263 */ - OMX_VIDEO_CodingMPEG4, /**< MPEG-4 */ - OMX_VIDEO_CodingWMV, /**< all versions of Windows Media Video */ - OMX_VIDEO_CodingRV, /**< all versions of Real Video */ - OMX_VIDEO_CodingAVC, /**< H.264/AVC */ - OMX_VIDEO_CodingMJPEG, /**< Motion JPEG */ - OMX_VIDEO_CodingVP8, /**< Google VP8, formerly known as On2 VP8 */ - OMX_VIDEO_CodingVP9, /**< Google VP9 */ - OMX_VIDEO_CodingHEVC, /**< ITU H.265/HEVC */ - OMX_VIDEO_CodingKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_CodingVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_CodingMax = 0x7FFFFFFF -} OMX_VIDEO_CODINGTYPE; - - -/** - * Data structure used to define a video path. The number of Video paths for - * input and output will vary by type of the Video component. - * - * Input (aka Source) : zero Inputs, one Output, - * Splitter : one Input, 2 or more Outputs, - * Processing Element : one Input, one output, - * Mixer : 2 or more inputs, one output, - * Output (aka Sink) : one Input, zero outputs. - * - * The PortDefinition structure is used to define all of the parameters - * necessary for the compliant component to setup an input or an output video - * path. If additional vendor specific data is required, it should be - * transmitted to the component using the CustomCommand function. Compliant - * components will prepopulate this structure with optimal values during the - * GetDefaultInitParams command. - * - * STRUCT MEMBERS: - * cMIMEType : MIME type of data for the port - * pNativeRender : Platform specific reference for a display if a - * sync, otherwise this field is 0 - * nFrameWidth : Width of frame to be used on channel if - * uncompressed format is used. Use 0 for unknown, - * don't care or variable - * nFrameHeight : Height of frame to be used on channel if - * uncompressed format is used. Use 0 for unknown, - * don't care or variable - * nStride : Number of bytes per span of an image - * (i.e. indicates the number of bytes to get - * from span N to span N+1, where negative stride - * indicates the image is bottom up - * nSliceHeight : Height used when encoding in slices - * nBitrate : Bit rate of frame to be used on channel if - * compressed format is used. Use 0 for unknown, - * don't care or variable - * xFramerate : Frame rate to be used on channel if uncompressed - * format is used. Use 0 for unknown, don't care or - * variable. Units are Q16 frames per second. - * bFlagErrorConcealment : Turns on error concealment if it is supported by - * the OMX component - * eCompressionFormat : Compression format used in this instance of the - * component. When OMX_VIDEO_CodingUnused is - * specified, eColorFormat is used - * eColorFormat : Decompressed format used by this component - * pNativeWindow : Platform specific reference for a window object if a - * display sink , otherwise this field is 0x0. - */ -typedef struct OMX_VIDEO_PORTDEFINITIONTYPE { - OMX_STRING cMIMEType; - OMX_NATIVE_DEVICETYPE pNativeRender; - OMX_U32 nFrameWidth; - OMX_U32 nFrameHeight; - OMX_S32 nStride; - OMX_U32 nSliceHeight; - OMX_U32 nBitrate; - OMX_U32 xFramerate; - OMX_BOOL bFlagErrorConcealment; - OMX_VIDEO_CODINGTYPE eCompressionFormat; - OMX_COLOR_FORMATTYPE eColorFormat; - OMX_NATIVE_WINDOWTYPE pNativeWindow; -} OMX_VIDEO_PORTDEFINITIONTYPE; - -/** - * Port format parameter. This structure is used to enumerate the various - * data input/output format supported by the port. - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Indicates which port to set - * nIndex : Indicates the enumeration index for the format from - * 0x0 to N-1 - * eCompressionFormat : Compression format used in this instance of the - * component. When OMX_VIDEO_CodingUnused is specified, - * eColorFormat is used - * eColorFormat : Decompressed format used by this component - * xFrameRate : Indicates the video frame rate in Q16 format - */ -typedef struct OMX_VIDEO_PARAM_PORTFORMATTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nIndex; - OMX_VIDEO_CODINGTYPE eCompressionFormat; - OMX_COLOR_FORMATTYPE eColorFormat; - OMX_U32 xFramerate; -} OMX_VIDEO_PARAM_PORTFORMATTYPE; - - -/** - * This is a structure for configuring video compression quantization - * parameter values. Codecs may support different QP values for different - * frame types. - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version info - * nPortIndex : Port that this structure applies to - * nQpI : QP value to use for index frames - * nQpP : QP value to use for P frames - * nQpB : QP values to use for bidirectional frames - */ -typedef struct OMX_VIDEO_PARAM_QUANTIZATIONTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nQpI; - OMX_U32 nQpP; - OMX_U32 nQpB; -} OMX_VIDEO_PARAM_QUANTIZATIONTYPE; - - -/** - * Structure for configuration of video fast update parameters. - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version info - * nPortIndex : Port that this structure applies to - * bEnableVFU : Enable/Disable video fast update - * nFirstGOB : Specifies the number of the first macroblock row - * nFirstMB : specifies the first MB relative to the specified first GOB - * nNumMBs : Specifies the number of MBs to be refreshed from nFirstGOB - * and nFirstMB - */ -typedef struct OMX_VIDEO_PARAM_VIDEOFASTUPDATETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bEnableVFU; - OMX_U32 nFirstGOB; - OMX_U32 nFirstMB; - OMX_U32 nNumMBs; -} OMX_VIDEO_PARAM_VIDEOFASTUPDATETYPE; - - -/** - * Enumeration of possible bitrate control types - */ -typedef enum OMX_VIDEO_CONTROLRATETYPE { - OMX_Video_ControlRateDisable, - OMX_Video_ControlRateVariable, - OMX_Video_ControlRateConstant, - OMX_Video_ControlRateVariableSkipFrames, - OMX_Video_ControlRateConstantSkipFrames, - OMX_Video_ControlRateKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_Video_ControlRateVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_Video_ControlRateMax = 0x7FFFFFFF -} OMX_VIDEO_CONTROLRATETYPE; - - -/** - * Structure for configuring bitrate mode of a codec. - * - * STRUCT MEMBERS: - * nSize : Size of the struct in bytes - * nVersion : OMX spec version info - * nPortIndex : Port that this struct applies to - * eControlRate : Control rate type enum - * nTargetBitrate : Target bitrate to encode with - */ -typedef struct OMX_VIDEO_PARAM_BITRATETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_CONTROLRATETYPE eControlRate; - OMX_U32 nTargetBitrate; -} OMX_VIDEO_PARAM_BITRATETYPE; - - -/** - * Enumeration of possible motion vector (MV) types - */ -typedef enum OMX_VIDEO_MOTIONVECTORTYPE { - OMX_Video_MotionVectorPixel, - OMX_Video_MotionVectorHalfPel, - OMX_Video_MotionVectorQuarterPel, - OMX_Video_MotionVectorEighthPel, - OMX_Video_MotionVectorKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_Video_MotionVectorVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_Video_MotionVectorMax = 0x7FFFFFFF -} OMX_VIDEO_MOTIONVECTORTYPE; - - -/** - * Structure for configuring the number of motion vectors used as well - * as their accuracy. - * - * STRUCT MEMBERS: - * nSize : Size of the struct in bytes - * nVersion : OMX spec version info - * nPortIndex : port that this structure applies to - * eAccuracy : Enumerated MV accuracy - * bUnrestrictedMVs : Allow unrestricted MVs - * bFourMV : Allow use of 4 MVs - * sXSearchRange : Search range in horizontal direction for MVs - * sYSearchRange : Search range in vertical direction for MVs - */ -typedef struct OMX_VIDEO_PARAM_MOTIONVECTORTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_MOTIONVECTORTYPE eAccuracy; - OMX_BOOL bUnrestrictedMVs; - OMX_BOOL bFourMV; - OMX_S32 sXSearchRange; - OMX_S32 sYSearchRange; -} OMX_VIDEO_PARAM_MOTIONVECTORTYPE; - - -/** - * Enumeration of possible methods to use for Intra Refresh - */ -typedef enum OMX_VIDEO_INTRAREFRESHTYPE { - OMX_VIDEO_IntraRefreshCyclic, - OMX_VIDEO_IntraRefreshAdaptive, - OMX_VIDEO_IntraRefreshBoth, - OMX_VIDEO_IntraRefreshKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_IntraRefreshVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_IntraRefreshMax = 0x7FFFFFFF -} OMX_VIDEO_INTRAREFRESHTYPE; - - -/** - * Structure for configuring intra refresh mode - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eRefreshMode : Cyclic, Adaptive, or Both - * nAirMBs : Number of intra macroblocks to refresh in a frame when - * AIR is enabled - * nAirRef : Number of times a motion marked macroblock has to be - * intra coded - * nCirMBs : Number of consecutive macroblocks to be coded as "intra" - * when CIR is enabled - */ -typedef struct OMX_VIDEO_PARAM_INTRAREFRESHTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_INTRAREFRESHTYPE eRefreshMode; - OMX_U32 nAirMBs; - OMX_U32 nAirRef; - OMX_U32 nCirMBs; -} OMX_VIDEO_PARAM_INTRAREFRESHTYPE; - - -/** - * Structure for enabling various error correction methods for video - * compression. - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * bEnableHEC : Enable/disable header extension codes (HEC) - * bEnableResync : Enable/disable resynchronization markers - * nResynchMarkerSpacing : Resynch markers interval (in bits) to be - * applied in the stream - * bEnableDataPartitioning : Enable/disable data partitioning - * bEnableRVLC : Enable/disable reversible variable length - * coding - */ -typedef struct OMX_VIDEO_PARAM_ERRORCORRECTIONTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bEnableHEC; - OMX_BOOL bEnableResync; - OMX_U32 nResynchMarkerSpacing; - OMX_BOOL bEnableDataPartitioning; - OMX_BOOL bEnableRVLC; -} OMX_VIDEO_PARAM_ERRORCORRECTIONTYPE; - - -/** - * Configuration of variable block-size motion compensation (VBSMC) - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * b16x16 : Enable inter block search 16x16 - * b16x8 : Enable inter block search 16x8 - * b8x16 : Enable inter block search 8x16 - * b8x8 : Enable inter block search 8x8 - * b8x4 : Enable inter block search 8x4 - * b4x8 : Enable inter block search 4x8 - * b4x4 : Enable inter block search 4x4 - */ -typedef struct OMX_VIDEO_PARAM_VBSMCTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL b16x16; - OMX_BOOL b16x8; - OMX_BOOL b8x16; - OMX_BOOL b8x8; - OMX_BOOL b8x4; - OMX_BOOL b4x8; - OMX_BOOL b4x4; -} OMX_VIDEO_PARAM_VBSMCTYPE; - - -/** - * H.263 profile types, each profile indicates support for various - * performance bounds and different annexes. - * - * ENUMS: - * Baseline : Baseline Profile: H.263 (V1), no optional modes - * H320 Coding : H.320 Coding Efficiency Backward Compatibility - * Profile: H.263+ (V2), includes annexes I, J, L.4 - * and T - * BackwardCompatible : Backward Compatibility Profile: H.263 (V1), - * includes annex F - * ISWV2 : Interactive Streaming Wireless Profile: H.263+ - * (V2), includes annexes I, J, K and T - * ISWV3 : Interactive Streaming Wireless Profile: H.263++ - * (V3), includes profile 3 and annexes V and W.6.3.8 - * HighCompression : Conversational High Compression Profile: H.263++ - * (V3), includes profiles 1 & 2 and annexes D and U - * Internet : Conversational Internet Profile: H.263++ (V3), - * includes profile 5 and annex K - * Interlace : Conversational Interlace Profile: H.263++ (V3), - * includes profile 5 and annex W.6.3.11 - * HighLatency : High Latency Profile: H.263++ (V3), includes - * profile 6 and annexes O.1 and P.5 - */ -typedef enum OMX_VIDEO_H263PROFILETYPE { - OMX_VIDEO_H263ProfileBaseline = 0x01, - OMX_VIDEO_H263ProfileH320Coding = 0x02, - OMX_VIDEO_H263ProfileBackwardCompatible = 0x04, - OMX_VIDEO_H263ProfileISWV2 = 0x08, - OMX_VIDEO_H263ProfileISWV3 = 0x10, - OMX_VIDEO_H263ProfileHighCompression = 0x20, - OMX_VIDEO_H263ProfileInternet = 0x40, - OMX_VIDEO_H263ProfileInterlace = 0x80, - OMX_VIDEO_H263ProfileHighLatency = 0x100, - OMX_VIDEO_H263ProfileKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_H263ProfileVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_H263ProfileMax = 0x7FFFFFFF -} OMX_VIDEO_H263PROFILETYPE; - - -/** - * H.263 level types, each level indicates support for various frame sizes, - * bit rates, decoder frame rates. - */ -typedef enum OMX_VIDEO_H263LEVELTYPE { - OMX_VIDEO_H263Level10 = 0x01, - OMX_VIDEO_H263Level20 = 0x02, - OMX_VIDEO_H263Level30 = 0x04, - OMX_VIDEO_H263Level40 = 0x08, - OMX_VIDEO_H263Level45 = 0x10, - OMX_VIDEO_H263Level50 = 0x20, - OMX_VIDEO_H263Level60 = 0x40, - OMX_VIDEO_H263Level70 = 0x80, - OMX_VIDEO_H263LevelKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_H263LevelVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_H263LevelMax = 0x7FFFFFFF -} OMX_VIDEO_H263LEVELTYPE; - - -/** - * Specifies the picture type. These values should be OR'd to signal all - * pictures types which are allowed. - * - * ENUMS: - * Generic Picture Types: I, P and B - * H.263 Specific Picture Types: SI and SP - * H.264 Specific Picture Types: EI and EP - * MPEG-4 Specific Picture Types: S - */ -typedef enum OMX_VIDEO_PICTURETYPE { - OMX_VIDEO_PictureTypeI = 0x01, - OMX_VIDEO_PictureTypeP = 0x02, - OMX_VIDEO_PictureTypeB = 0x04, - OMX_VIDEO_PictureTypeSI = 0x08, - OMX_VIDEO_PictureTypeSP = 0x10, - OMX_VIDEO_PictureTypeEI = 0x11, - OMX_VIDEO_PictureTypeEP = 0x12, - OMX_VIDEO_PictureTypeS = 0x14, - OMX_VIDEO_PictureTypeKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_PictureTypeVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_PictureTypeMax = 0x7FFFFFFF -} OMX_VIDEO_PICTURETYPE; - - -/** - * H.263 Params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nPFrames : Number of P frames between each I frame - * nBFrames : Number of B frames between each I frame - * eProfile : H.263 profile(s) to use - * eLevel : H.263 level(s) to use - * bPLUSPTYPEAllowed : Indicating that it is allowed to use PLUSPTYPE - * (specified in the 1998 version of H.263) to - * indicate custom picture sizes or clock - * frequencies - * nAllowedPictureTypes : Specifies the picture types allowed in the - * bitstream - * bForceRoundingTypeToZero : value of the RTYPE bit (bit 6 of MPPTYPE) is - * not constrained. It is recommended to change - * the value of the RTYPE bit for each reference - * picture in error-free communication - * nPictureHeaderRepetition : Specifies the frequency of picture header - * repetition - * nGOBHeaderInterval : Specifies the interval of non-empty GOB - * headers in units of GOBs - */ -typedef struct OMX_VIDEO_PARAM_H263TYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nPFrames; - OMX_U32 nBFrames; - OMX_VIDEO_H263PROFILETYPE eProfile; - OMX_VIDEO_H263LEVELTYPE eLevel; - OMX_BOOL bPLUSPTYPEAllowed; - OMX_U32 nAllowedPictureTypes; - OMX_BOOL bForceRoundingTypeToZero; - OMX_U32 nPictureHeaderRepetition; - OMX_U32 nGOBHeaderInterval; -} OMX_VIDEO_PARAM_H263TYPE; - - -/** - * MPEG-2 profile types, each profile indicates support for various - * performance bounds and different annexes. - */ -typedef enum OMX_VIDEO_MPEG2PROFILETYPE { - OMX_VIDEO_MPEG2ProfileSimple = 0, /**< Simple Profile */ - OMX_VIDEO_MPEG2ProfileMain, /**< Main Profile */ - OMX_VIDEO_MPEG2Profile422, /**< 4:2:2 Profile */ - OMX_VIDEO_MPEG2ProfileSNR, /**< SNR Profile */ - OMX_VIDEO_MPEG2ProfileSpatial, /**< Spatial Profile */ - OMX_VIDEO_MPEG2ProfileHigh, /**< High Profile */ - OMX_VIDEO_MPEG2ProfileKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_MPEG2ProfileVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_MPEG2ProfileMax = 0x7FFFFFFF -} OMX_VIDEO_MPEG2PROFILETYPE; - - -/** - * MPEG-2 level types, each level indicates support for various frame - * sizes, bit rates, decoder frame rates. No need - */ -typedef enum OMX_VIDEO_MPEG2LEVELTYPE { - OMX_VIDEO_MPEG2LevelLL = 0, /**< Low Level */ - OMX_VIDEO_MPEG2LevelML, /**< Main Level */ - OMX_VIDEO_MPEG2LevelH14, /**< High 1440 */ - OMX_VIDEO_MPEG2LevelHL, /**< High Level */ - OMX_VIDEO_MPEG2LevelKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_MPEG2LevelVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_MPEG2LevelMax = 0x7FFFFFFF -} OMX_VIDEO_MPEG2LEVELTYPE; - - -/** - * MPEG-2 params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nPFrames : Number of P frames between each I frame - * nBFrames : Number of B frames between each I frame - * eProfile : MPEG-2 profile(s) to use - * eLevel : MPEG-2 levels(s) to use - */ -typedef struct OMX_VIDEO_PARAM_MPEG2TYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nPFrames; - OMX_U32 nBFrames; - OMX_VIDEO_MPEG2PROFILETYPE eProfile; - OMX_VIDEO_MPEG2LEVELTYPE eLevel; -} OMX_VIDEO_PARAM_MPEG2TYPE; - - -/** - * MPEG-4 profile types, each profile indicates support for various - * performance bounds and different annexes. - * - * ENUMS: - * - Simple Profile, Levels 1-3 - * - Simple Scalable Profile, Levels 1-2 - * - Core Profile, Levels 1-2 - * - Main Profile, Levels 2-4 - * - N-bit Profile, Level 2 - * - Scalable Texture Profile, Level 1 - * - Simple Face Animation Profile, Levels 1-2 - * - Simple Face and Body Animation (FBA) Profile, Levels 1-2 - * - Basic Animated Texture Profile, Levels 1-2 - * - Hybrid Profile, Levels 1-2 - * - Advanced Real Time Simple Profiles, Levels 1-4 - * - Core Scalable Profile, Levels 1-3 - * - Advanced Coding Efficiency Profile, Levels 1-4 - * - Advanced Core Profile, Levels 1-2 - * - Advanced Scalable Texture, Levels 2-3 - */ -typedef enum OMX_VIDEO_MPEG4PROFILETYPE { - OMX_VIDEO_MPEG4ProfileSimple = 0x01, - OMX_VIDEO_MPEG4ProfileSimpleScalable = 0x02, - OMX_VIDEO_MPEG4ProfileCore = 0x04, - OMX_VIDEO_MPEG4ProfileMain = 0x08, - OMX_VIDEO_MPEG4ProfileNbit = 0x10, - OMX_VIDEO_MPEG4ProfileScalableTexture = 0x20, - OMX_VIDEO_MPEG4ProfileSimpleFace = 0x40, - OMX_VIDEO_MPEG4ProfileSimpleFBA = 0x80, - OMX_VIDEO_MPEG4ProfileBasicAnimated = 0x100, - OMX_VIDEO_MPEG4ProfileHybrid = 0x200, - OMX_VIDEO_MPEG4ProfileAdvancedRealTime = 0x400, - OMX_VIDEO_MPEG4ProfileCoreScalable = 0x800, - OMX_VIDEO_MPEG4ProfileAdvancedCoding = 0x1000, - OMX_VIDEO_MPEG4ProfileAdvancedCore = 0x2000, - OMX_VIDEO_MPEG4ProfileAdvancedScalable = 0x4000, - OMX_VIDEO_MPEG4ProfileAdvancedSimple = 0x8000, - OMX_VIDEO_MPEG4ProfileKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_MPEG4ProfileVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_MPEG4ProfileMax = 0x7FFFFFFF -} OMX_VIDEO_MPEG4PROFILETYPE; - - -/** - * MPEG-4 level types, each level indicates support for various frame - * sizes, bit rates, decoder frame rates. No need - */ -typedef enum OMX_VIDEO_MPEG4LEVELTYPE { - OMX_VIDEO_MPEG4Level0 = 0x01, /**< Level 0 */ - OMX_VIDEO_MPEG4Level0b = 0x02, /**< Level 0b */ - OMX_VIDEO_MPEG4Level1 = 0x04, /**< Level 1 */ - OMX_VIDEO_MPEG4Level2 = 0x08, /**< Level 2 */ - OMX_VIDEO_MPEG4Level3 = 0x10, /**< Level 3 */ - OMX_VIDEO_MPEG4Level4 = 0x20, /**< Level 4 */ - OMX_VIDEO_MPEG4Level4a = 0x40, /**< Level 4a */ - OMX_VIDEO_MPEG4Level5 = 0x80, /**< Level 5 */ - OMX_VIDEO_MPEG4LevelKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_MPEG4LevelVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_MPEG4LevelMax = 0x7FFFFFFF -} OMX_VIDEO_MPEG4LEVELTYPE; - - -/** - * MPEG-4 configuration. This structure handles configuration options - * which are specific to MPEG4 algorithms - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nSliceHeaderSpacing : Number of macroblocks between slice header (H263+ - * Annex K). Put zero if not used - * bSVH : Enable Short Video Header mode - * bGov : Flag to enable GOV - * nPFrames : Number of P frames between each I frame (also called - * GOV period) - * nBFrames : Number of B frames between each I frame - * nIDCVLCThreshold : Value of intra DC VLC threshold - * bACPred : Flag to use ac prediction - * nMaxPacketSize : Maximum size of packet in bytes. - * nTimeIncRes : Used to pass VOP time increment resolution for MPEG4. - * Interpreted as described in MPEG4 standard. - * eProfile : MPEG-4 profile(s) to use. - * eLevel : MPEG-4 level(s) to use. - * nAllowedPictureTypes : Specifies the picture types allowed in the bitstream - * nHeaderExtension : Specifies the number of consecutive video packet - * headers within a VOP - * bReversibleVLC : Specifies whether reversible variable length coding - * is in use - */ -typedef struct OMX_VIDEO_PARAM_MPEG4TYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nSliceHeaderSpacing; - OMX_BOOL bSVH; - OMX_BOOL bGov; - OMX_U32 nPFrames; - OMX_U32 nBFrames; - OMX_U32 nIDCVLCThreshold; - OMX_BOOL bACPred; - OMX_U32 nMaxPacketSize; - OMX_U32 nTimeIncRes; - OMX_VIDEO_MPEG4PROFILETYPE eProfile; - OMX_VIDEO_MPEG4LEVELTYPE eLevel; - OMX_U32 nAllowedPictureTypes; - OMX_U32 nHeaderExtension; - OMX_BOOL bReversibleVLC; -} OMX_VIDEO_PARAM_MPEG4TYPE; - - -/** - * WMV Versions - */ -typedef enum OMX_VIDEO_WMVFORMATTYPE { - OMX_VIDEO_WMVFormatUnused = 0x01, /**< Format unused or unknown */ - OMX_VIDEO_WMVFormat7 = 0x02, /**< Windows Media Video format 7 */ - OMX_VIDEO_WMVFormat8 = 0x04, /**< Windows Media Video format 8 */ - OMX_VIDEO_WMVFormat9 = 0x08, /**< Windows Media Video format 9 */ - OMX_VIDEO_WMFFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_WMFFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_WMVFormatMax = 0x7FFFFFFF -} OMX_VIDEO_WMVFORMATTYPE; - - -/** - * WMV Params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eFormat : Version of WMV stream / data - */ -typedef struct OMX_VIDEO_PARAM_WMVTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_WMVFORMATTYPE eFormat; -} OMX_VIDEO_PARAM_WMVTYPE; - - -/** - * Real Video Version - */ -typedef enum OMX_VIDEO_RVFORMATTYPE { - OMX_VIDEO_RVFormatUnused = 0, /**< Format unused or unknown */ - OMX_VIDEO_RVFormat8, /**< Real Video format 8 */ - OMX_VIDEO_RVFormat9, /**< Real Video format 9 */ - OMX_VIDEO_RVFormatG2, /**< Real Video Format G2 */ - OMX_VIDEO_RVFormatKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_RVFormatVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_RVFormatMax = 0x7FFFFFFF -} OMX_VIDEO_RVFORMATTYPE; - - -/** - * Real Video Params - * - * STUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * eFormat : Version of RV stream / data - * nBitsPerPixel : Bits per pixel coded in the frame - * nPaddedWidth : Padded width in pixel of a video frame - * nPaddedHeight : Padded Height in pixels of a video frame - * nFrameRate : Rate of video in frames per second - * nBitstreamFlags : Flags which internal information about the bitstream - * nBitstreamVersion : Bitstream version - * nMaxEncodeFrameSize: Max encoded frame size - * bEnablePostFilter : Turn on/off post filter - * bEnableTemporalInterpolation : Turn on/off temporal interpolation - * bEnableLatencyMode : When enabled, the decoder does not display a decoded - * frame until it has detected that no enhancement layer - * frames or dependent B frames will be coming. This - * detection usually occurs when a subsequent non-B - * frame is encountered - */ -typedef struct OMX_VIDEO_PARAM_RVTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_RVFORMATTYPE eFormat; - OMX_U16 nBitsPerPixel; - OMX_U16 nPaddedWidth; - OMX_U16 nPaddedHeight; - OMX_U32 nFrameRate; - OMX_U32 nBitstreamFlags; - OMX_U32 nBitstreamVersion; - OMX_U32 nMaxEncodeFrameSize; - OMX_BOOL bEnablePostFilter; - OMX_BOOL bEnableTemporalInterpolation; - OMX_BOOL bEnableLatencyMode; -} OMX_VIDEO_PARAM_RVTYPE; - - -/** - * AVC profile types, each profile indicates support for various - * performance bounds and different annexes. - */ -typedef enum OMX_VIDEO_AVCPROFILETYPE { - OMX_VIDEO_AVCProfileBaseline = 0x01, /**< Baseline profile */ - OMX_VIDEO_AVCProfileMain = 0x02, /**< Main profile */ - OMX_VIDEO_AVCProfileExtended = 0x04, /**< Extended profile */ - OMX_VIDEO_AVCProfileHigh = 0x08, /**< High profile */ - OMX_VIDEO_AVCProfileHigh10 = 0x10, /**< High 10 profile */ - OMX_VIDEO_AVCProfileHigh422 = 0x20, /**< High 4:2:2 profile */ - OMX_VIDEO_AVCProfileHigh444 = 0x40, /**< High 4:4:4 profile */ - OMX_VIDEO_AVCProfileKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_AVCProfileVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_AVCProfileMax = 0x7FFFFFFF -} OMX_VIDEO_AVCPROFILETYPE; - - -/** - * AVC level types, each level indicates support for various frame sizes, - * bit rates, decoder frame rates. No need - */ -typedef enum OMX_VIDEO_AVCLEVELTYPE { - OMX_VIDEO_AVCLevel1 = 0x01, /**< Level 1 */ - OMX_VIDEO_AVCLevel1b = 0x02, /**< Level 1b */ - OMX_VIDEO_AVCLevel11 = 0x04, /**< Level 1.1 */ - OMX_VIDEO_AVCLevel12 = 0x08, /**< Level 1.2 */ - OMX_VIDEO_AVCLevel13 = 0x10, /**< Level 1.3 */ - OMX_VIDEO_AVCLevel2 = 0x20, /**< Level 2 */ - OMX_VIDEO_AVCLevel21 = 0x40, /**< Level 2.1 */ - OMX_VIDEO_AVCLevel22 = 0x80, /**< Level 2.2 */ - OMX_VIDEO_AVCLevel3 = 0x100, /**< Level 3 */ - OMX_VIDEO_AVCLevel31 = 0x200, /**< Level 3.1 */ - OMX_VIDEO_AVCLevel32 = 0x400, /**< Level 3.2 */ - OMX_VIDEO_AVCLevel4 = 0x800, /**< Level 4 */ - OMX_VIDEO_AVCLevel41 = 0x1000, /**< Level 4.1 */ - OMX_VIDEO_AVCLevel42 = 0x2000, /**< Level 4.2 */ - OMX_VIDEO_AVCLevel5 = 0x4000, /**< Level 5 */ - OMX_VIDEO_AVCLevel51 = 0x8000, /**< Level 5.1 */ - OMX_VIDEO_AVCLevel52 = 0x10000, /**< Level 5.2 */ - OMX_VIDEO_AVCLevelKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_AVCLevelVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_AVCLevelMax = 0x7FFFFFFF -} OMX_VIDEO_AVCLEVELTYPE; - - -/** - * AVC loop filter modes - * - * OMX_VIDEO_AVCLoopFilterEnable : Enable - * OMX_VIDEO_AVCLoopFilterDisable : Disable - * OMX_VIDEO_AVCLoopFilterDisableSliceBoundary : Disabled on slice boundaries - */ -typedef enum OMX_VIDEO_AVCLOOPFILTERTYPE { - OMX_VIDEO_AVCLoopFilterEnable = 0, - OMX_VIDEO_AVCLoopFilterDisable, - OMX_VIDEO_AVCLoopFilterDisableSliceBoundary, - OMX_VIDEO_AVCLoopFilterKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_AVCLoopFilterVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_AVCLoopFilterMax = 0x7FFFFFFF -} OMX_VIDEO_AVCLOOPFILTERTYPE; - - -/** - * AVC params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nSliceHeaderSpacing : Number of macroblocks between slice header, put - * zero if not used - * nPFrames : Number of P frames between each I frame - * nBFrames : Number of B frames between each I frame - * bUseHadamard : Enable/disable Hadamard transform - * nRefFrames : Max number of reference frames to use for inter - * motion search (1-16) - * nRefIdxTrailing : Pic param set ref frame index (index into ref - * frame buffer of trailing frames list), B frame - * support - * nRefIdxForward : Pic param set ref frame index (index into ref - * frame buffer of forward frames list), B frame - * support - * bEnableUEP : Enable/disable unequal error protection. This - * is only valid of data partitioning is enabled. - * bEnableFMO : Enable/disable flexible macroblock ordering - * bEnableASO : Enable/disable arbitrary slice ordering - * bEnableRS : Enable/disable sending of redundant slices - * eProfile : AVC profile(s) to use - * eLevel : AVC level(s) to use - * nAllowedPictureTypes : Specifies the picture types allowed in the - * bitstream - * bFrameMBsOnly : specifies that every coded picture of the - * coded video sequence is a coded frame - * containing only frame macroblocks - * bMBAFF : Enable/disable switching between frame and - * field macroblocks within a picture - * bEntropyCodingCABAC : Entropy decoding method to be applied for the - * syntax elements for which two descriptors appear - * in the syntax tables - * bWeightedPPrediction : Enable/disable weighted prediction shall not - * be applied to P and SP slices - * nWeightedBipredicitonMode : Default weighted prediction is applied to B - * slices - * bconstIpred : Enable/disable intra prediction - * bDirect8x8Inference : Specifies the method used in the derivation - * process for luma motion vectors for B_Skip, - * B_Direct_16x16 and B_Direct_8x8 as specified - * in subclause 8.4.1.2 of the AVC spec - * bDirectSpatialTemporal : Flag indicating spatial or temporal direct - * mode used in B slice coding (related to - * bDirect8x8Inference) . Spatial direct mode is - * more common and should be the default. - * nCabacInitIdx : Index used to init CABAC contexts - * eLoopFilterMode : Enable/disable loop filter - */ -typedef struct OMX_VIDEO_PARAM_AVCTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nSliceHeaderSpacing; - OMX_U32 nPFrames; - OMX_U32 nBFrames; - OMX_BOOL bUseHadamard; - OMX_U32 nRefFrames; - OMX_U32 nRefIdx10ActiveMinus1; - OMX_U32 nRefIdx11ActiveMinus1; - OMX_BOOL bEnableUEP; - OMX_BOOL bEnableFMO; - OMX_BOOL bEnableASO; - OMX_BOOL bEnableRS; - OMX_VIDEO_AVCPROFILETYPE eProfile; - OMX_VIDEO_AVCLEVELTYPE eLevel; - OMX_U32 nAllowedPictureTypes; - OMX_BOOL bFrameMBsOnly; - OMX_BOOL bMBAFF; - OMX_BOOL bEntropyCodingCABAC; - OMX_BOOL bWeightedPPrediction; - OMX_U32 nWeightedBipredicitonMode; - OMX_BOOL bconstIpred ; - OMX_BOOL bDirect8x8Inference; - OMX_BOOL bDirectSpatialTemporal; - OMX_U32 nCabacInitIdc; - OMX_VIDEO_AVCLOOPFILTERTYPE eLoopFilterMode; -} OMX_VIDEO_PARAM_AVCTYPE; - -typedef struct OMX_VIDEO_PARAM_PROFILELEVELTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 eProfile; /**< type is OMX_VIDEO_AVCPROFILETYPE, OMX_VIDEO_H263PROFILETYPE, - or OMX_VIDEO_MPEG4PROFILETYPE depending on context */ - OMX_U32 eLevel; /**< type is OMX_VIDEO_AVCLEVELTYPE, OMX_VIDEO_H263LEVELTYPE, - or OMX_VIDEO_MPEG4PROFILETYPE depending on context */ - OMX_U32 nProfileIndex; /**< Used to query for individual profile support information, - This parameter is valid only for - OMX_IndexParamVideoProfileLevelQuerySupported index, - For all other indices this parameter is to be ignored. */ -} OMX_VIDEO_PARAM_PROFILELEVELTYPE; - -/** - * Structure for dynamically configuring bitrate mode of a codec. - * - * STRUCT MEMBERS: - * nSize : Size of the struct in bytes - * nVersion : OMX spec version info - * nPortIndex : Port that this struct applies to - * nEncodeBitrate : Target average bitrate to be generated in bps - */ -typedef struct OMX_VIDEO_CONFIG_BITRATETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nEncodeBitrate; -} OMX_VIDEO_CONFIG_BITRATETYPE; - -/** - * Defines Encoder Frame Rate setting - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * xEncodeFramerate : Encoding framerate represented in Q16 format - */ -typedef struct OMX_CONFIG_FRAMERATETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 xEncodeFramerate; /* Q16 format */ -} OMX_CONFIG_FRAMERATETYPE; - -typedef struct OMX_CONFIG_INTRAREFRESHVOPTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL IntraRefreshVOP; -} OMX_CONFIG_INTRAREFRESHVOPTYPE; - -typedef struct OMX_CONFIG_MACROBLOCKERRORMAPTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nErrMapSize; /* Size of the Error Map in bytes */ - OMX_U8 ErrMap[1]; /* Error map hint */ -} OMX_CONFIG_MACROBLOCKERRORMAPTYPE; - -typedef struct OMX_CONFIG_MBERRORREPORTINGTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bEnabled; -} OMX_CONFIG_MBERRORREPORTINGTYPE; - -typedef struct OMX_PARAM_MACROBLOCKSTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nMacroblocks; -} OMX_PARAM_MACROBLOCKSTYPE; - -/** - * AVC Slice Mode modes - * - * OMX_VIDEO_SLICEMODE_AVCDefault : Normal frame encoding, one slice per frame - * OMX_VIDEO_SLICEMODE_AVCMBSlice : NAL mode, number of MBs per frame - * OMX_VIDEO_SLICEMODE_AVCByteSlice : NAL mode, number of bytes per frame - */ -typedef enum OMX_VIDEO_AVCSLICEMODETYPE { - OMX_VIDEO_SLICEMODE_AVCDefault = 0, - OMX_VIDEO_SLICEMODE_AVCMBSlice, - OMX_VIDEO_SLICEMODE_AVCByteSlice, - OMX_VIDEO_SLICEMODE_AVCKhronosExtensions = 0x6F000000, /**< Reserved region for introducing Khronos Standard Extensions */ - OMX_VIDEO_SLICEMODE_AVCVendorStartUnused = 0x7F000000, /**< Reserved region for introducing Vendor Extensions */ - OMX_VIDEO_SLICEMODE_AVCLevelMax = 0x7FFFFFFF -} OMX_VIDEO_AVCSLICEMODETYPE; - -/** - * AVC FMO Slice Mode Params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nNumSliceGroups : Specifies the number of slice groups - * nSliceGroupMapType : Specifies the type of slice groups - * eSliceMode : Specifies the type of slice - */ -typedef struct OMX_VIDEO_PARAM_AVCSLICEFMO { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U8 nNumSliceGroups; - OMX_U8 nSliceGroupMapType; - OMX_VIDEO_AVCSLICEMODETYPE eSliceMode; -} OMX_VIDEO_PARAM_AVCSLICEFMO; - -/** - * AVC IDR Period Configs - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nIDRPeriod : Specifies periodicity of IDR frames - * nPFrames : Specifies internal of coding Intra frames - */ -typedef struct OMX_VIDEO_CONFIG_AVCINTRAPERIOD { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nIDRPeriod; - OMX_U32 nPFrames; -} OMX_VIDEO_CONFIG_AVCINTRAPERIOD; - -/** - * AVC NAL Size Configs - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nNaluBytes : Specifies the NAL unit size - */ -typedef struct OMX_VIDEO_CONFIG_NALSIZE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nNaluBytes; -} OMX_VIDEO_CONFIG_NALSIZE; - -/** @} */ - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif -/* File EOF */ - diff --git a/phonelibs/android_frameworks_native/include/media/openmax/OMX_VideoExt.h b/phonelibs/android_frameworks_native/include/media/openmax/OMX_VideoExt.h deleted file mode 100644 index 3971bc5cc..000000000 --- a/phonelibs/android_frameworks_native/include/media/openmax/OMX_VideoExt.h +++ /dev/null @@ -1,224 +0,0 @@ -/* - * Copyright (c) 2010 The Khronos Group Inc. - * - * Permission is hereby granted, free of charge, to any person obtaining - * a copy of this software and associated documentation files (the - * "Software"), to deal in the Software without restriction, including - * without limitation the rights to use, copy, modify, merge, publish, - * distribute, sublicense, and/or sell copies of the Software, and to - * permit persons to whom the Software is furnished to do so, subject - * to the following conditions: - * The above copyright notice and this permission notice shall be included - * in all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS - * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF - * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. - * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY - * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, - * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE - * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - * - */ - -/** OMX_VideoExt.h - OpenMax IL version 1.1.2 - * The OMX_VideoExt header file contains extensions to the - * definitions used by both the application and the component to - * access video items. - */ - -#ifndef OMX_VideoExt_h -#define OMX_VideoExt_h - -#ifdef __cplusplus -extern "C" { -#endif /* __cplusplus */ - -/* Each OMX header shall include all required header files to allow the - * header to compile without errors. The includes below are required - * for this header file to compile successfully - */ -#include - -/** NALU Formats */ -typedef enum OMX_NALUFORMATSTYPE { - OMX_NaluFormatStartCodes = 1, - OMX_NaluFormatOneNaluPerBuffer = 2, - OMX_NaluFormatOneByteInterleaveLength = 4, - OMX_NaluFormatTwoByteInterleaveLength = 8, - OMX_NaluFormatFourByteInterleaveLength = 16, - OMX_NaluFormatCodingMax = 0x7FFFFFFF -} OMX_NALUFORMATSTYPE; - -/** NAL Stream Format */ -typedef struct OMX_NALSTREAMFORMATTYPE{ - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_NALUFORMATSTYPE eNaluFormat; -} OMX_NALSTREAMFORMATTYPE; - -/** VP8 profiles */ -typedef enum OMX_VIDEO_VP8PROFILETYPE { - OMX_VIDEO_VP8ProfileMain = 0x01, - OMX_VIDEO_VP8ProfileUnknown = 0x6EFFFFFF, - OMX_VIDEO_VP8ProfileMax = 0x7FFFFFFF -} OMX_VIDEO_VP8PROFILETYPE; - -/** VP8 levels */ -typedef enum OMX_VIDEO_VP8LEVELTYPE { - OMX_VIDEO_VP8Level_Version0 = 0x01, - OMX_VIDEO_VP8Level_Version1 = 0x02, - OMX_VIDEO_VP8Level_Version2 = 0x04, - OMX_VIDEO_VP8Level_Version3 = 0x08, - OMX_VIDEO_VP8LevelUnknown = 0x6EFFFFFF, - OMX_VIDEO_VP8LevelMax = 0x7FFFFFFF -} OMX_VIDEO_VP8LEVELTYPE; - -/** VP8 Param */ -typedef struct OMX_VIDEO_PARAM_VP8TYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_VP8PROFILETYPE eProfile; - OMX_VIDEO_VP8LEVELTYPE eLevel; - OMX_U32 nDCTPartitions; - OMX_BOOL bErrorResilientMode; -} OMX_VIDEO_PARAM_VP8TYPE; - -/** Structure for configuring VP8 reference frames */ -typedef struct OMX_VIDEO_VP8REFERENCEFRAMETYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bPreviousFrameRefresh; - OMX_BOOL bGoldenFrameRefresh; - OMX_BOOL bAlternateFrameRefresh; - OMX_BOOL bUsePreviousFrame; - OMX_BOOL bUseGoldenFrame; - OMX_BOOL bUseAlternateFrame; -} OMX_VIDEO_VP8REFERENCEFRAMETYPE; - -/** Structure for querying VP8 reference frame type */ -typedef struct OMX_VIDEO_VP8REFERENCEFRAMEINFOTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bIsIntraFrame; - OMX_BOOL bIsGoldenOrAlternateFrame; -} OMX_VIDEO_VP8REFERENCEFRAMEINFOTYPE; - -/** Maximum number of VP8 temporal layers */ -#define OMX_VIDEO_ANDROID_MAXVP8TEMPORALLAYERS 3 - -/** VP8 temporal layer patterns */ -typedef enum OMX_VIDEO_ANDROID_VPXTEMPORALLAYERPATTERNTYPE { - OMX_VIDEO_VPXTemporalLayerPatternNone = 0, - OMX_VIDEO_VPXTemporalLayerPatternWebRTC = 1, - OMX_VIDEO_VPXTemporalLayerPatternMax = 0x7FFFFFFF -} OMX_VIDEO_ANDROID_VPXTEMPORALLAYERPATTERNTYPE; - -/** - * Android specific VP8 encoder params - * - * STRUCT MEMBERS: - * nSize : Size of the structure in bytes - * nVersion : OMX specification version information - * nPortIndex : Port that this structure applies to - * nKeyFrameInterval : Key frame interval in frames - * eTemporalPattern : Type of temporal layer pattern - * nTemporalLayerCount : Number of temporal coding layers - * nTemporalLayerBitrateRatio : Bitrate ratio allocation between temporal - * streams in percentage - * nMinQuantizer : Minimum (best quality) quantizer - * nMaxQuantizer : Maximum (worst quality) quantizer - */ -typedef struct OMX_VIDEO_PARAM_ANDROID_VP8ENCODERTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_U32 nKeyFrameInterval; - OMX_VIDEO_ANDROID_VPXTEMPORALLAYERPATTERNTYPE eTemporalPattern; - OMX_U32 nTemporalLayerCount; - OMX_U32 nTemporalLayerBitrateRatio[OMX_VIDEO_ANDROID_MAXVP8TEMPORALLAYERS]; - OMX_U32 nMinQuantizer; - OMX_U32 nMaxQuantizer; -} OMX_VIDEO_PARAM_ANDROID_VP8ENCODERTYPE; - -/** HEVC Profile enum type */ -typedef enum OMX_VIDEO_HEVCPROFILETYPE { - OMX_VIDEO_HEVCProfileUnknown = 0x0, - OMX_VIDEO_HEVCProfileMain = 0x1, - OMX_VIDEO_HEVCProfileMain10 = 0x2, - OMX_VIDEO_HEVCProfileMax = 0x7FFFFFFF -} OMX_VIDEO_HEVCPROFILETYPE; - -/** HEVC Level enum type */ -typedef enum OMX_VIDEO_HEVCLEVELTYPE { - OMX_VIDEO_HEVCLevelUnknown = 0x0, - OMX_VIDEO_HEVCMainTierLevel1 = 0x1, - OMX_VIDEO_HEVCHighTierLevel1 = 0x2, - OMX_VIDEO_HEVCMainTierLevel2 = 0x4, - OMX_VIDEO_HEVCHighTierLevel2 = 0x8, - OMX_VIDEO_HEVCMainTierLevel21 = 0x10, - OMX_VIDEO_HEVCHighTierLevel21 = 0x20, - OMX_VIDEO_HEVCMainTierLevel3 = 0x40, - OMX_VIDEO_HEVCHighTierLevel3 = 0x80, - OMX_VIDEO_HEVCMainTierLevel31 = 0x100, - OMX_VIDEO_HEVCHighTierLevel31 = 0x200, - OMX_VIDEO_HEVCMainTierLevel4 = 0x400, - OMX_VIDEO_HEVCHighTierLevel4 = 0x800, - OMX_VIDEO_HEVCMainTierLevel41 = 0x1000, - OMX_VIDEO_HEVCHighTierLevel41 = 0x2000, - OMX_VIDEO_HEVCMainTierLevel5 = 0x4000, - OMX_VIDEO_HEVCHighTierLevel5 = 0x8000, - OMX_VIDEO_HEVCMainTierLevel51 = 0x10000, - OMX_VIDEO_HEVCHighTierLevel51 = 0x20000, - OMX_VIDEO_HEVCMainTierLevel52 = 0x40000, - OMX_VIDEO_HEVCHighTierLevel52 = 0x80000, - OMX_VIDEO_HEVCMainTierLevel6 = 0x100000, - OMX_VIDEO_HEVCHighTierLevel6 = 0x200000, - OMX_VIDEO_HEVCMainTierLevel61 = 0x400000, - OMX_VIDEO_HEVCHighTierLevel61 = 0x800000, - OMX_VIDEO_HEVCMainTierLevel62 = 0x1000000, - OMX_VIDEO_HEVCHighTierLevel62 = 0x2000000, - OMX_VIDEO_HEVCHighTiermax = 0x7FFFFFFF -} OMX_VIDEO_HEVCLEVELTYPE; - -/** Structure for controlling HEVC video encoding and decoding */ -typedef struct OMX_VIDEO_PARAM_HEVCTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_VIDEO_HEVCPROFILETYPE eProfile; - OMX_VIDEO_HEVCLEVELTYPE eLevel; -} OMX_VIDEO_PARAM_HEVCTYPE; - -/** Structure to define if dependent slice segments should be used */ -typedef struct OMX_VIDEO_SLICESEGMENTSTYPE { - OMX_U32 nSize; - OMX_VERSIONTYPE nVersion; - OMX_U32 nPortIndex; - OMX_BOOL bDepedentSegments; - OMX_BOOL bEnableLoopFilterAcrossSlices; -} OMX_VIDEO_SLICESEGMENTSTYPE; - -/** Structure to return timestamps of rendered output frames as well as EOS - * for tunneled components. - */ -typedef struct OMX_VIDEO_RENDEREVENTTYPE { - OMX_S64 nMediaTimeUs; // timestamp of rendered video frame - OMX_S64 nSystemTimeNs; // system monotonic time at the time frame was rendered - // Use INT64_MAX for nMediaTimeUs to signal that the EOS - // has been reached. In this case, nSystemTimeNs MUST be - // the system time when the last frame was rendered. - // This MUST be done in addition to returning (and - // following) the render information for the last frame. -} OMX_VIDEO_RENDEREVENTTYPE; - -#ifdef __cplusplus -} -#endif /* __cplusplus */ - -#endif /* OMX_VideoExt_h */ -/* File EOF */ diff --git a/phonelibs/android_frameworks_native/include/powermanager/IPowerManager.h b/phonelibs/android_frameworks_native/include/powermanager/IPowerManager.h deleted file mode 100644 index 91ecc5aa3..000000000 --- a/phonelibs/android_frameworks_native/include/powermanager/IPowerManager.h +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (C) 2011 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_IPOWERMANAGER_H -#define ANDROID_IPOWERMANAGER_H - -#include -#include -#include - -namespace android { - -// ---------------------------------------------------------------------------- - -// must be kept in sync with interface defined in IPowerManager.aidl -class IPowerManager : public IInterface -{ -public: - DECLARE_META_INTERFACE(PowerManager); - - // FIXME remove the bool isOneWay parameters as they are not oneway in the .aidl - virtual status_t acquireWakeLock(int flags, const sp& lock, const String16& tag, - const String16& packageName, bool isOneWay = false) = 0; - virtual status_t acquireWakeLockWithUid(int flags, const sp& lock, const String16& tag, - const String16& packageName, int uid, bool isOneWay = false) = 0; - virtual status_t releaseWakeLock(const sp& lock, int flags, bool isOneWay = false) = 0; - virtual status_t updateWakeLockUids(const sp& lock, int len, const int *uids, - bool isOneWay = false) = 0; - // oneway in the .aidl - virtual status_t powerHint(int hintId, int data) = 0; -}; - -// ---------------------------------------------------------------------------- - -}; // namespace android - -#endif // ANDROID_IPOWERMANAGER_H diff --git a/phonelibs/android_frameworks_native/include/powermanager/PowerManager.h b/phonelibs/android_frameworks_native/include/powermanager/PowerManager.h deleted file mode 100644 index cbddc1153..000000000 --- a/phonelibs/android_frameworks_native/include/powermanager/PowerManager.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright (C) 2011 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_POWERMANAGER_H -#define ANDROID_POWERMANAGER_H - -namespace android { - -// must be kept in sync with definitions in PowerManager.java -enum { - POWERMANAGER_PARTIAL_WAKE_LOCK = 1, // equals PowerManager.PARTIAL_WAKE_LOCK constant -}; - -enum { - USER_ACTIVITY_EVENT_OTHER = 0, - USER_ACTIVITY_EVENT_BUTTON = 1, - USER_ACTIVITY_EVENT_TOUCH = 2, - - USER_ACTIVITY_EVENT_LAST = USER_ACTIVITY_EVENT_TOUCH, // Last valid event code. -}; - -}; // namespace android - -#endif // ANDROID_POWERMANAGER_H diff --git a/phonelibs/android_frameworks_native/include/private/binder/Static.h b/phonelibs/android_frameworks_native/include/private/binder/Static.h deleted file mode 100644 index d10464680..000000000 --- a/phonelibs/android_frameworks_native/include/private/binder/Static.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -// All static variables go here, to control initialization and -// destruction order in the library. - -#include - -#include -#include -#include -#include -#include - -namespace android { - -// For TextStream.cpp -extern Vector gTextBuffers; - -// For ProcessState.cpp -extern Mutex gProcessMutex; -extern sp gProcess; - -// For IServiceManager.cpp -extern Mutex gDefaultServiceManagerLock; -extern sp gDefaultServiceManager; -extern sp gPermissionController; - -} // namespace android diff --git a/phonelibs/android_frameworks_native/include/private/binder/binder_module.h b/phonelibs/android_frameworks_native/include/private/binder/binder_module.h deleted file mode 100644 index a8dd64f23..000000000 --- a/phonelibs/android_frameworks_native/include/private/binder/binder_module.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef _BINDER_MODULE_H_ -#define _BINDER_MODULE_H_ - -#ifdef __cplusplus -namespace android { -#endif - -/* obtain structures and constants from the kernel header */ - -#include -#include - -#ifdef __cplusplus -} // namespace android -#endif - -#endif // _BINDER_MODULE_H_ diff --git a/phonelibs/android_frameworks_native/include/private/gui/ComposerService.h b/phonelibs/android_frameworks_native/include/private/gui/ComposerService.h deleted file mode 100644 index ff2f9bf0f..000000000 --- a/phonelibs/android_frameworks_native/include/private/gui/ComposerService.h +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright (C) 2011 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_PRIVATE_GUI_COMPOSER_SERVICE_H -#define ANDROID_PRIVATE_GUI_COMPOSER_SERVICE_H - -#include -#include - -#include -#include - - -namespace android { - -// --------------------------------------------------------------------------- - -class IMemoryHeap; -class ISurfaceComposer; - -// --------------------------------------------------------------------------- - -// This holds our connection to the composer service (i.e. SurfaceFlinger). -// If the remote side goes away, we will re-establish the connection. -// Users of this class should not retain the value from -// getComposerService() for an extended period. -// -// (It's not clear that using Singleton is useful here anymore.) -class ComposerService : public Singleton -{ - sp mComposerService; - sp mDeathObserver; - Mutex mLock; - - ComposerService(); - void connectLocked(); - void composerServiceDied(); - friend class Singleton; -public: - - // Get a connection to the Composer Service. This will block until - // a connection is established. - static sp getComposerService(); -}; - -// --------------------------------------------------------------------------- -}; // namespace android - -#endif // ANDROID_PRIVATE_GUI_COMPOSER_SERVICE_H diff --git a/phonelibs/android_frameworks_native/include/private/gui/LayerState.h b/phonelibs/android_frameworks_native/include/private/gui/LayerState.h deleted file mode 100644 index 9ff840991..000000000 --- a/phonelibs/android_frameworks_native/include/private/gui/LayerState.h +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (C) 2008 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_SF_LAYER_STATE_H -#define ANDROID_SF_LAYER_STATE_H - -#include -#include - -#include - -#include -#include - -namespace android { - -class Parcel; -class ISurfaceComposerClient; - -/* - * Used to communicate layer information between SurfaceFlinger and its clients. - */ -struct layer_state_t { - - - enum { - eLayerHidden = 0x01, // SURFACE_HIDDEN in SurfaceControl.java - eLayerOpaque = 0x02, // SURFACE_OPAQUE - eLayerSecure = 0x80, // SECURE - }; - - enum { - ePositionChanged = 0x00000001, - eLayerChanged = 0x00000002, - eSizeChanged = 0x00000004, - eAlphaChanged = 0x00000008, - eMatrixChanged = 0x00000010, - eTransparentRegionChanged = 0x00000020, - eFlagsChanged = 0x00000040, - eLayerStackChanged = 0x00000080, - eCropChanged = 0x00000100, - eBlurChanged = 0x00400000, - eBlurMaskSurfaceChanged = 0x00800000, - eBlurMaskSamplingChanged = 0x01000000, - eBlurMaskAlphaThresholdChanged = 0x02000000, - }; - - layer_state_t() - : what(0), - x(0), y(0), z(0), w(0), h(0), layerStack(0), blur(0), - blurMaskSampling(0), blurMaskAlphaThreshold(0), alpha(0), flags(0), mask(0), - reserved(0) - { - matrix.dsdx = matrix.dtdy = 1.0f; - matrix.dsdy = matrix.dtdx = 0.0f; - crop.makeInvalid(); - } - - status_t write(Parcel& output) const; - status_t read(const Parcel& input); - - struct matrix22_t { - float dsdx; - float dtdx; - float dsdy; - float dtdy; - }; - sp surface; - uint32_t what; - float x; - float y; - uint32_t z; - uint32_t w; - uint32_t h; - uint32_t layerStack; - float blur; - sp blurMaskSurface; - uint32_t blurMaskSampling; - float blurMaskAlphaThreshold; - float alpha; - uint8_t flags; - uint8_t mask; - uint8_t reserved; - matrix22_t matrix; - Rect crop; - // non POD must be last. see write/read - Region transparentRegion; -}; - -struct ComposerState { - sp client; - layer_state_t state; - status_t write(Parcel& output) const; - status_t read(const Parcel& input); -}; - -struct DisplayState { - - enum { - eOrientationDefault = 0, - eOrientation90 = 1, - eOrientation180 = 2, - eOrientation270 = 3, - eOrientationUnchanged = 4, - eOrientationSwapMask = 0x01 - }; - - enum { - eSurfaceChanged = 0x01, - eLayerStackChanged = 0x02, - eDisplayProjectionChanged = 0x04, - eDisplaySizeChanged = 0x08 - }; - - uint32_t what; - sp token; - sp surface; - uint32_t layerStack; - uint32_t orientation; - Rect viewport; - Rect frame; - uint32_t width, height; - status_t write(Parcel& output) const; - status_t read(const Parcel& input); -}; - -}; // namespace android - -#endif // ANDROID_SF_LAYER_STATE_H - diff --git a/phonelibs/android_frameworks_native/include/private/gui/SyncFeatures.h b/phonelibs/android_frameworks_native/include/private/gui/SyncFeatures.h deleted file mode 100644 index 79fb75bcb..000000000 --- a/phonelibs/android_frameworks_native/include/private/gui/SyncFeatures.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (C) 2013 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_GUI_SYNC_FEATURES_H -#define ANDROID_GUI_SYNC_FEATURES_H - -#include -#include - - -namespace android { -// ---------------------------------------------------------------------------- - -class SyncFeatures : public Singleton { - friend class Singleton; - bool mHasNativeFenceSync; - bool mHasFenceSync; - bool mHasWaitSync; - String8 mString; - SyncFeatures(); - -public: - bool useNativeFenceSync() const; - bool useFenceSync() const; - bool useWaitSync() const; - String8 toString() const; -}; - -// ---------------------------------------------------------------------------- -}; // namespace android - -#endif // ANDROID_GUI_SYNC_FEATURES_H diff --git a/phonelibs/android_frameworks_native/include/private/ui/RegionHelper.h b/phonelibs/android_frameworks_native/include/private/ui/RegionHelper.h deleted file mode 100644 index 8c190dd40..000000000 --- a/phonelibs/android_frameworks_native/include/private/ui/RegionHelper.h +++ /dev/null @@ -1,301 +0,0 @@ -/* - * Copyright (C) 2009 The Android Open Source Project - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef ANDROID_UI_PRIVATE_REGION_HELPER_H -#define ANDROID_UI_PRIVATE_REGION_HELPER_H - -#include -#include - -namespace android { -// ---------------------------------------------------------------------------- - -template -class region_operator -{ -public: - typedef typename RECT::value_type TYPE; - static const TYPE max_value = 0x7FFFFFF; - - /* - * Common boolean operations: - * value is computed as 0b101 op 0b110 - * other boolean operation are possible, simply compute - * their corresponding value with the above formulae and use - * it when instantiating a region_operator. - */ - static const uint32_t LHS = 0x5; // 0b101 - static const uint32_t RHS = 0x6; // 0b110 - enum { - op_nand = LHS & ~RHS, - op_and = LHS & RHS, - op_or = LHS | RHS, - op_xor = LHS ^ RHS - }; - - struct region { - RECT const* rects; - size_t count; - TYPE dx; - TYPE dy; - inline region(const region& rhs) - : rects(rhs.rects), count(rhs.count), dx(rhs.dx), dy(rhs.dy) { } - inline region(RECT const* r, size_t c) - : rects(r), count(c), dx(), dy() { } - inline region(RECT const* r, size_t c, TYPE dx, TYPE dy) - : rects(r), count(c), dx(dx), dy(dy) { } - }; - - class region_rasterizer { - friend class region_operator; - virtual void operator()(const RECT& rect) = 0; - public: - virtual ~region_rasterizer() { }; - }; - - inline region_operator(int op, const region& lhs, const region& rhs) - : op_mask(op), spanner(lhs, rhs) - { - } - - void operator()(region_rasterizer& rasterizer) { - RECT current; - do { - SpannerInner spannerInner(spanner.lhs, spanner.rhs); - int inside = spanner.next(current.top, current.bottom); - spannerInner.prepare(inside); - do { - TYPE left, right; - int inside = spannerInner.next(current.left, current.right); - if ((op_mask >> inside) & 1) { - if (current.left < current.right && - current.top < current.bottom) { - rasterizer(current); - } - } - } while(!spannerInner.isDone()); - } while(!spanner.isDone()); - } - -private: - uint32_t op_mask; - - class SpannerBase - { - public: - SpannerBase() - : lhs_head(max_value), lhs_tail(max_value), - rhs_head(max_value), rhs_tail(max_value) { - } - - enum { - lhs_before_rhs = 0, - lhs_after_rhs = 1, - lhs_coincide_rhs = 2 - }; - - protected: - TYPE lhs_head; - TYPE lhs_tail; - TYPE rhs_head; - TYPE rhs_tail; - - inline int next(TYPE& head, TYPE& tail, - bool& more_lhs, bool& more_rhs) - { - int inside; - more_lhs = false; - more_rhs = false; - if (lhs_head < rhs_head) { - inside = lhs_before_rhs; - head = lhs_head; - if (lhs_tail <= rhs_head) { - tail = lhs_tail; - more_lhs = true; - } else { - lhs_head = rhs_head; - tail = rhs_head; - } - } else if (rhs_head < lhs_head) { - inside = lhs_after_rhs; - head = rhs_head; - if (rhs_tail <= lhs_head) { - tail = rhs_tail; - more_rhs = true; - } else { - rhs_head = lhs_head; - tail = lhs_head; - } - } else { - inside = lhs_coincide_rhs; - head = lhs_head; - if (lhs_tail <= rhs_tail) { - tail = rhs_head = lhs_tail; - more_lhs = true; - } - if (rhs_tail <= lhs_tail) { - tail = lhs_head = rhs_tail; - more_rhs = true; - } - } - return inside; - } - }; - - class Spanner : protected SpannerBase - { - friend class region_operator; - region lhs; - region rhs; - - public: - inline Spanner(const region& lhs, const region& rhs) - : lhs(lhs), rhs(rhs) - { - if (lhs.count) { - SpannerBase::lhs_head = lhs.rects->top + lhs.dy; - SpannerBase::lhs_tail = lhs.rects->bottom + lhs.dy; - } - if (rhs.count) { - SpannerBase::rhs_head = rhs.rects->top + rhs.dy; - SpannerBase::rhs_tail = rhs.rects->bottom + rhs.dy; - } - } - - inline bool isDone() const { - return !rhs.count && !lhs.count; - } - - inline int next(TYPE& top, TYPE& bottom) - { - bool more_lhs = false; - bool more_rhs = false; - int inside = SpannerBase::next(top, bottom, more_lhs, more_rhs); - if (more_lhs) { - advance(lhs, SpannerBase::lhs_head, SpannerBase::lhs_tail); - } - if (more_rhs) { - advance(rhs, SpannerBase::rhs_head, SpannerBase::rhs_tail); - } - return inside; - } - - private: - static inline - void advance(region& reg, TYPE& aTop, TYPE& aBottom) { - // got to next span - size_t count = reg.count; - RECT const * rects = reg.rects; - RECT const * const end = rects + count; - const int top = rects->top; - while (rects != end && rects->top == top) { - rects++; - count--; - } - if (rects != end) { - aTop = rects->top + reg.dy; - aBottom = rects->bottom + reg.dy; - } else { - aTop = max_value; - aBottom = max_value; - } - reg.rects = rects; - reg.count = count; - } - }; - - class SpannerInner : protected SpannerBase - { - region lhs; - region rhs; - - public: - inline SpannerInner(const region& lhs, const region& rhs) - : lhs(lhs), rhs(rhs) - { - } - - inline void prepare(int inside) { - if (inside == SpannerBase::lhs_before_rhs) { - if (lhs.count) { - SpannerBase::lhs_head = lhs.rects->left + lhs.dx; - SpannerBase::lhs_tail = lhs.rects->right + lhs.dx; - } - SpannerBase::rhs_head = max_value; - SpannerBase::rhs_tail = max_value; - } else if (inside == SpannerBase::lhs_after_rhs) { - SpannerBase::lhs_head = max_value; - SpannerBase::lhs_tail = max_value; - if (rhs.count) { - SpannerBase::rhs_head = rhs.rects->left + rhs.dx; - SpannerBase::rhs_tail = rhs.rects->right + rhs.dx; - } - } else { - if (lhs.count) { - SpannerBase::lhs_head = lhs.rects->left + lhs.dx; - SpannerBase::lhs_tail = lhs.rects->right + lhs.dx; - } - if (rhs.count) { - SpannerBase::rhs_head = rhs.rects->left + rhs.dx; - SpannerBase::rhs_tail = rhs.rects->right + rhs.dx; - } - } - } - - inline bool isDone() const { - return SpannerBase::lhs_head == max_value && - SpannerBase::rhs_head == max_value; - } - - inline int next(TYPE& left, TYPE& right) - { - bool more_lhs = false; - bool more_rhs = false; - int inside = SpannerBase::next(left, right, more_lhs, more_rhs); - if (more_lhs) { - advance(lhs, SpannerBase::lhs_head, SpannerBase::lhs_tail); - } - if (more_rhs) { - advance(rhs, SpannerBase::rhs_head, SpannerBase::rhs_tail); - } - return inside; - } - - private: - static inline - void advance(region& reg, TYPE& left, TYPE& right) { - if (reg.rects && reg.count) { - const int cur_span_top = reg.rects->top; - reg.rects++; - reg.count--; - if (!reg.count || reg.rects->top != cur_span_top) { - left = max_value; - right = max_value; - } else { - left = reg.rects->left + reg.dx; - right = reg.rects->right + reg.dx; - } - } - } - }; - - Spanner spanner; -}; - -// ---------------------------------------------------------------------------- -}; - -#endif /* ANDROID_UI_PRIVATE_REGION_HELPER_H */ diff --git a/phonelibs/libyuv/larch64/lib/libyuv.a b/phonelibs/libyuv/larch64/lib/libyuv.a new file mode 100644 index 000000000..fdbcda251 Binary files /dev/null and b/phonelibs/libyuv/larch64/lib/libyuv.a differ diff --git a/phonelibs/qrcode/QrCode.cc b/phonelibs/qrcode/QrCode.cc new file mode 100644 index 000000000..b9de86215 --- /dev/null +++ b/phonelibs/qrcode/QrCode.cc @@ -0,0 +1,862 @@ +/* + * QR Code generator library (C++) + * + * Copyright (c) Project Nayuki. (MIT License) + * https://www.nayuki.io/page/qr-code-generator-library + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * - The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * - The Software is provided "as is", without warranty of any kind, express or + * implied, including but not limited to the warranties of merchantability, + * fitness for a particular purpose and noninfringement. In no event shall the + * authors or copyright holders be liable for any claim, damages or other + * liability, whether in an action of contract, tort or otherwise, arising from, + * out of or in connection with the Software or the use or other dealings in the + * Software. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "QrCode.hpp" + +using std::int8_t; +using std::uint8_t; +using std::size_t; +using std::vector; + + +namespace qrcodegen { + +QrSegment::Mode::Mode(int mode, int cc0, int cc1, int cc2) : + modeBits(mode) { + numBitsCharCount[0] = cc0; + numBitsCharCount[1] = cc1; + numBitsCharCount[2] = cc2; +} + + +int QrSegment::Mode::getModeBits() const { + return modeBits; +} + + +int QrSegment::Mode::numCharCountBits(int ver) const { + return numBitsCharCount[(ver + 7) / 17]; +} + + +const QrSegment::Mode QrSegment::Mode::NUMERIC (0x1, 10, 12, 14); +const QrSegment::Mode QrSegment::Mode::ALPHANUMERIC(0x2, 9, 11, 13); +const QrSegment::Mode QrSegment::Mode::BYTE (0x4, 8, 16, 16); +const QrSegment::Mode QrSegment::Mode::KANJI (0x8, 8, 10, 12); +const QrSegment::Mode QrSegment::Mode::ECI (0x7, 0, 0, 0); + + +QrSegment QrSegment::makeBytes(const vector &data) { + if (data.size() > static_cast(INT_MAX)) + throw std::length_error("Data too long"); + BitBuffer bb; + for (uint8_t b : data) + bb.appendBits(b, 8); + return QrSegment(Mode::BYTE, static_cast(data.size()), std::move(bb)); +} + + +QrSegment QrSegment::makeNumeric(const char *digits) { + BitBuffer bb; + int accumData = 0; + int accumCount = 0; + int charCount = 0; + for (; *digits != '\0'; digits++, charCount++) { + char c = *digits; + if (c < '0' || c > '9') + throw std::domain_error("String contains non-numeric characters"); + accumData = accumData * 10 + (c - '0'); + accumCount++; + if (accumCount == 3) { + bb.appendBits(static_cast(accumData), 10); + accumData = 0; + accumCount = 0; + } + } + if (accumCount > 0) // 1 or 2 digits remaining + bb.appendBits(static_cast(accumData), accumCount * 3 + 1); + return QrSegment(Mode::NUMERIC, charCount, std::move(bb)); +} + + +QrSegment QrSegment::makeAlphanumeric(const char *text) { + BitBuffer bb; + int accumData = 0; + int accumCount = 0; + int charCount = 0; + for (; *text != '\0'; text++, charCount++) { + const char *temp = std::strchr(ALPHANUMERIC_CHARSET, *text); + if (temp == nullptr) + throw std::domain_error("String contains unencodable characters in alphanumeric mode"); + accumData = accumData * 45 + static_cast(temp - ALPHANUMERIC_CHARSET); + accumCount++; + if (accumCount == 2) { + bb.appendBits(static_cast(accumData), 11); + accumData = 0; + accumCount = 0; + } + } + if (accumCount > 0) // 1 character remaining + bb.appendBits(static_cast(accumData), 6); + return QrSegment(Mode::ALPHANUMERIC, charCount, std::move(bb)); +} + + +vector QrSegment::makeSegments(const char *text) { + // Select the most efficient segment encoding automatically + vector result; + if (*text == '\0'); // Leave result empty + else if (isNumeric(text)) + result.push_back(makeNumeric(text)); + else if (isAlphanumeric(text)) + result.push_back(makeAlphanumeric(text)); + else { + vector bytes; + for (; *text != '\0'; text++) + bytes.push_back(static_cast(*text)); + result.push_back(makeBytes(bytes)); + } + return result; +} + + +QrSegment QrSegment::makeEci(long assignVal) { + BitBuffer bb; + if (assignVal < 0) + throw std::domain_error("ECI assignment value out of range"); + else if (assignVal < (1 << 7)) + bb.appendBits(static_cast(assignVal), 8); + else if (assignVal < (1 << 14)) { + bb.appendBits(2, 2); + bb.appendBits(static_cast(assignVal), 14); + } else if (assignVal < 1000000L) { + bb.appendBits(6, 3); + bb.appendBits(static_cast(assignVal), 21); + } else + throw std::domain_error("ECI assignment value out of range"); + return QrSegment(Mode::ECI, 0, std::move(bb)); +} + + +QrSegment::QrSegment(Mode md, int numCh, const std::vector &dt) : + mode(md), + numChars(numCh), + data(dt) { + if (numCh < 0) + throw std::domain_error("Invalid value"); +} + + +QrSegment::QrSegment(Mode md, int numCh, std::vector &&dt) : + mode(md), + numChars(numCh), + data(std::move(dt)) { + if (numCh < 0) + throw std::domain_error("Invalid value"); +} + + +int QrSegment::getTotalBits(const vector &segs, int version) { + int result = 0; + for (const QrSegment &seg : segs) { + int ccbits = seg.mode.numCharCountBits(version); + if (seg.numChars >= (1L << ccbits)) + return -1; // The segment's length doesn't fit the field's bit width + if (4 + ccbits > INT_MAX - result) + return -1; // The sum will overflow an int type + result += 4 + ccbits; + if (seg.data.size() > static_cast(INT_MAX - result)) + return -1; // The sum will overflow an int type + result += static_cast(seg.data.size()); + } + return result; +} + + +bool QrSegment::isAlphanumeric(const char *text) { + for (; *text != '\0'; text++) { + if (std::strchr(ALPHANUMERIC_CHARSET, *text) == nullptr) + return false; + } + return true; +} + + +bool QrSegment::isNumeric(const char *text) { + for (; *text != '\0'; text++) { + char c = *text; + if (c < '0' || c > '9') + return false; + } + return true; +} + + +QrSegment::Mode QrSegment::getMode() const { + return mode; +} + + +int QrSegment::getNumChars() const { + return numChars; +} + + +const std::vector &QrSegment::getData() const { + return data; +} + + +const char *QrSegment::ALPHANUMERIC_CHARSET = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ $%*+-./:"; + + + +int QrCode::getFormatBits(Ecc ecl) { + switch (ecl) { + case Ecc::LOW : return 1; + case Ecc::MEDIUM : return 0; + case Ecc::QUARTILE: return 3; + case Ecc::HIGH : return 2; + default: throw std::logic_error("Assertion error"); + } +} + + +QrCode QrCode::encodeText(const char *text, Ecc ecl) { + vector segs = QrSegment::makeSegments(text); + return encodeSegments(segs, ecl); +} + + +QrCode QrCode::encodeBinary(const vector &data, Ecc ecl) { + vector segs{QrSegment::makeBytes(data)}; + return encodeSegments(segs, ecl); +} + + +QrCode QrCode::encodeSegments(const vector &segs, Ecc ecl, + int minVersion, int maxVersion, int mask, bool boostEcl) { + if (!(MIN_VERSION <= minVersion && minVersion <= maxVersion && maxVersion <= MAX_VERSION) || mask < -1 || mask > 7) + throw std::invalid_argument("Invalid value"); + + // Find the minimal version number to use + int version, dataUsedBits; + for (version = minVersion; ; version++) { + int dataCapacityBits = getNumDataCodewords(version, ecl) * 8; // Number of data bits available + dataUsedBits = QrSegment::getTotalBits(segs, version); + if (dataUsedBits != -1 && dataUsedBits <= dataCapacityBits) + break; // This version number is found to be suitable + if (version >= maxVersion) { // All versions in the range could not fit the given data + std::ostringstream sb; + if (dataUsedBits == -1) + sb << "Segment too long"; + else { + sb << "Data length = " << dataUsedBits << " bits, "; + sb << "Max capacity = " << dataCapacityBits << " bits"; + } + throw data_too_long(sb.str()); + } + } + if (dataUsedBits == -1) + throw std::logic_error("Assertion error"); + + // Increase the error correction level while the data still fits in the current version number + for (Ecc newEcl : vector{Ecc::MEDIUM, Ecc::QUARTILE, Ecc::HIGH}) { // From low to high + if (boostEcl && dataUsedBits <= getNumDataCodewords(version, newEcl) * 8) + ecl = newEcl; + } + + // Concatenate all segments to create the data bit string + BitBuffer bb; + for (const QrSegment &seg : segs) { + bb.appendBits(static_cast(seg.getMode().getModeBits()), 4); + bb.appendBits(static_cast(seg.getNumChars()), seg.getMode().numCharCountBits(version)); + bb.insert(bb.end(), seg.getData().begin(), seg.getData().end()); + } + if (bb.size() != static_cast(dataUsedBits)) + throw std::logic_error("Assertion error"); + + // Add terminator and pad up to a byte if applicable + size_t dataCapacityBits = static_cast(getNumDataCodewords(version, ecl)) * 8; + if (bb.size() > dataCapacityBits) + throw std::logic_error("Assertion error"); + bb.appendBits(0, std::min(4, static_cast(dataCapacityBits - bb.size()))); + bb.appendBits(0, (8 - static_cast(bb.size() % 8)) % 8); + if (bb.size() % 8 != 0) + throw std::logic_error("Assertion error"); + + // Pad with alternating bytes until data capacity is reached + for (uint8_t padByte = 0xEC; bb.size() < dataCapacityBits; padByte ^= 0xEC ^ 0x11) + bb.appendBits(padByte, 8); + + // Pack bits into bytes in big endian + vector dataCodewords(bb.size() / 8); + for (size_t i = 0; i < bb.size(); i++) + dataCodewords[i >> 3] |= (bb.at(i) ? 1 : 0) << (7 - (i & 7)); + + // Create the QR Code object + return QrCode(version, ecl, dataCodewords, mask); +} + + +QrCode::QrCode(int ver, Ecc ecl, const vector &dataCodewords, int msk) : + // Initialize fields and check arguments + version(ver), + errorCorrectionLevel(ecl) { + if (ver < MIN_VERSION || ver > MAX_VERSION) + throw std::domain_error("Version value out of range"); + if (msk < -1 || msk > 7) + throw std::domain_error("Mask value out of range"); + size = ver * 4 + 17; + size_t sz = static_cast(size); + modules = vector >(sz, vector(sz)); // Initially all white + isFunction = vector >(sz, vector(sz)); + + // Compute ECC, draw modules + drawFunctionPatterns(); + const vector allCodewords = addEccAndInterleave(dataCodewords); + drawCodewords(allCodewords); + + // Do masking + if (msk == -1) { // Automatically choose best mask + long minPenalty = LONG_MAX; + for (int i = 0; i < 8; i++) { + applyMask(i); + drawFormatBits(i); + long penalty = getPenaltyScore(); + if (penalty < minPenalty) { + msk = i; + minPenalty = penalty; + } + applyMask(i); // Undoes the mask due to XOR + } + } + if (msk < 0 || msk > 7) + throw std::logic_error("Assertion error"); + this->mask = msk; + applyMask(msk); // Apply the final choice of mask + drawFormatBits(msk); // Overwrite old format bits + + isFunction.clear(); + isFunction.shrink_to_fit(); +} + + +int QrCode::getVersion() const { + return version; +} + + +int QrCode::getSize() const { + return size; +} + + +QrCode::Ecc QrCode::getErrorCorrectionLevel() const { + return errorCorrectionLevel; +} + + +int QrCode::getMask() const { + return mask; +} + + +bool QrCode::getModule(int x, int y) const { + return 0 <= x && x < size && 0 <= y && y < size && module(x, y); +} + + +std::string QrCode::toSvgString(int border) const { + if (border < 0) + throw std::domain_error("Border must be non-negative"); + if (border > INT_MAX / 2 || border * 2 > INT_MAX - size) + throw std::overflow_error("Border too large"); + + std::ostringstream sb; + sb << "\n"; + sb << "\n"; + sb << "\n"; + sb << "\t\n"; + sb << "\t\n"; + sb << "\n"; + return sb.str(); +} + + +void QrCode::drawFunctionPatterns() { + // Draw horizontal and vertical timing patterns + for (int i = 0; i < size; i++) { + setFunctionModule(6, i, i % 2 == 0); + setFunctionModule(i, 6, i % 2 == 0); + } + + // Draw 3 finder patterns (all corners except bottom right; overwrites some timing modules) + drawFinderPattern(3, 3); + drawFinderPattern(size - 4, 3); + drawFinderPattern(3, size - 4); + + // Draw numerous alignment patterns + const vector alignPatPos = getAlignmentPatternPositions(); + size_t numAlign = alignPatPos.size(); + for (size_t i = 0; i < numAlign; i++) { + for (size_t j = 0; j < numAlign; j++) { + // Don't draw on the three finder corners + if (!((i == 0 && j == 0) || (i == 0 && j == numAlign - 1) || (i == numAlign - 1 && j == 0))) + drawAlignmentPattern(alignPatPos.at(i), alignPatPos.at(j)); + } + } + + // Draw configuration data + drawFormatBits(0); // Dummy mask value; overwritten later in the constructor + drawVersion(); +} + + +void QrCode::drawFormatBits(int msk) { + // Calculate error correction code and pack bits + int data = getFormatBits(errorCorrectionLevel) << 3 | msk; // errCorrLvl is uint2, msk is uint3 + int rem = data; + for (int i = 0; i < 10; i++) + rem = (rem << 1) ^ ((rem >> 9) * 0x537); + int bits = (data << 10 | rem) ^ 0x5412; // uint15 + if (bits >> 15 != 0) + throw std::logic_error("Assertion error"); + + // Draw first copy + for (int i = 0; i <= 5; i++) + setFunctionModule(8, i, getBit(bits, i)); + setFunctionModule(8, 7, getBit(bits, 6)); + setFunctionModule(8, 8, getBit(bits, 7)); + setFunctionModule(7, 8, getBit(bits, 8)); + for (int i = 9; i < 15; i++) + setFunctionModule(14 - i, 8, getBit(bits, i)); + + // Draw second copy + for (int i = 0; i < 8; i++) + setFunctionModule(size - 1 - i, 8, getBit(bits, i)); + for (int i = 8; i < 15; i++) + setFunctionModule(8, size - 15 + i, getBit(bits, i)); + setFunctionModule(8, size - 8, true); // Always black +} + + +void QrCode::drawVersion() { + if (version < 7) + return; + + // Calculate error correction code and pack bits + int rem = version; // version is uint6, in the range [7, 40] + for (int i = 0; i < 12; i++) + rem = (rem << 1) ^ ((rem >> 11) * 0x1F25); + long bits = static_cast(version) << 12 | rem; // uint18 + if (bits >> 18 != 0) + throw std::logic_error("Assertion error"); + + // Draw two copies + for (int i = 0; i < 18; i++) { + bool bit = getBit(bits, i); + int a = size - 11 + i % 3; + int b = i / 3; + setFunctionModule(a, b, bit); + setFunctionModule(b, a, bit); + } +} + + +void QrCode::drawFinderPattern(int x, int y) { + for (int dy = -4; dy <= 4; dy++) { + for (int dx = -4; dx <= 4; dx++) { + int dist = std::max(std::abs(dx), std::abs(dy)); // Chebyshev/infinity norm + int xx = x + dx, yy = y + dy; + if (0 <= xx && xx < size && 0 <= yy && yy < size) + setFunctionModule(xx, yy, dist != 2 && dist != 4); + } + } +} + + +void QrCode::drawAlignmentPattern(int x, int y) { + for (int dy = -2; dy <= 2; dy++) { + for (int dx = -2; dx <= 2; dx++) + setFunctionModule(x + dx, y + dy, std::max(std::abs(dx), std::abs(dy)) != 1); + } +} + + +void QrCode::setFunctionModule(int x, int y, bool isBlack) { + size_t ux = static_cast(x); + size_t uy = static_cast(y); + modules .at(uy).at(ux) = isBlack; + isFunction.at(uy).at(ux) = true; +} + + +bool QrCode::module(int x, int y) const { + return modules.at(static_cast(y)).at(static_cast(x)); +} + + +vector QrCode::addEccAndInterleave(const vector &data) const { + if (data.size() != static_cast(getNumDataCodewords(version, errorCorrectionLevel))) + throw std::invalid_argument("Invalid argument"); + + // Calculate parameter numbers + int numBlocks = NUM_ERROR_CORRECTION_BLOCKS[static_cast(errorCorrectionLevel)][version]; + int blockEccLen = ECC_CODEWORDS_PER_BLOCK [static_cast(errorCorrectionLevel)][version]; + int rawCodewords = getNumRawDataModules(version) / 8; + int numShortBlocks = numBlocks - rawCodewords % numBlocks; + int shortBlockLen = rawCodewords / numBlocks; + + // Split data into blocks and append ECC to each block + vector > blocks; + const vector rsDiv = reedSolomonComputeDivisor(blockEccLen); + for (int i = 0, k = 0; i < numBlocks; i++) { + vector dat(data.cbegin() + k, data.cbegin() + (k + shortBlockLen - blockEccLen + (i < numShortBlocks ? 0 : 1))); + k += static_cast(dat.size()); + const vector ecc = reedSolomonComputeRemainder(dat, rsDiv); + if (i < numShortBlocks) + dat.push_back(0); + dat.insert(dat.end(), ecc.cbegin(), ecc.cend()); + blocks.push_back(std::move(dat)); + } + + // Interleave (not concatenate) the bytes from every block into a single sequence + vector result; + for (size_t i = 0; i < blocks.at(0).size(); i++) { + for (size_t j = 0; j < blocks.size(); j++) { + // Skip the padding byte in short blocks + if (i != static_cast(shortBlockLen - blockEccLen) || j >= static_cast(numShortBlocks)) + result.push_back(blocks.at(j).at(i)); + } + } + if (result.size() != static_cast(rawCodewords)) + throw std::logic_error("Assertion error"); + return result; +} + + +void QrCode::drawCodewords(const vector &data) { + if (data.size() != static_cast(getNumRawDataModules(version) / 8)) + throw std::invalid_argument("Invalid argument"); + + size_t i = 0; // Bit index into the data + // Do the funny zigzag scan + for (int right = size - 1; right >= 1; right -= 2) { // Index of right column in each column pair + if (right == 6) + right = 5; + for (int vert = 0; vert < size; vert++) { // Vertical counter + for (int j = 0; j < 2; j++) { + size_t x = static_cast(right - j); // Actual x coordinate + bool upward = ((right + 1) & 2) == 0; + size_t y = static_cast(upward ? size - 1 - vert : vert); // Actual y coordinate + if (!isFunction.at(y).at(x) && i < data.size() * 8) { + modules.at(y).at(x) = getBit(data.at(i >> 3), 7 - static_cast(i & 7)); + i++; + } + // If this QR Code has any remainder bits (0 to 7), they were assigned as + // 0/false/white by the constructor and are left unchanged by this method + } + } + } + if (i != data.size() * 8) + throw std::logic_error("Assertion error"); +} + + +void QrCode::applyMask(int msk) { + if (msk < 0 || msk > 7) + throw std::domain_error("Mask value out of range"); + size_t sz = static_cast(size); + for (size_t y = 0; y < sz; y++) { + for (size_t x = 0; x < sz; x++) { + bool invert; + switch (msk) { + case 0: invert = (x + y) % 2 == 0; break; + case 1: invert = y % 2 == 0; break; + case 2: invert = x % 3 == 0; break; + case 3: invert = (x + y) % 3 == 0; break; + case 4: invert = (x / 3 + y / 2) % 2 == 0; break; + case 5: invert = x * y % 2 + x * y % 3 == 0; break; + case 6: invert = (x * y % 2 + x * y % 3) % 2 == 0; break; + case 7: invert = ((x + y) % 2 + x * y % 3) % 2 == 0; break; + default: throw std::logic_error("Assertion error"); + } + modules.at(y).at(x) = modules.at(y).at(x) ^ (invert & !isFunction.at(y).at(x)); + } + } +} + + +long QrCode::getPenaltyScore() const { + long result = 0; + + // Adjacent modules in row having same color, and finder-like patterns + for (int y = 0; y < size; y++) { + bool runColor = false; + int runX = 0; + std::array runHistory = {}; + for (int x = 0; x < size; x++) { + if (module(x, y) == runColor) { + runX++; + if (runX == 5) + result += PENALTY_N1; + else if (runX > 5) + result++; + } else { + finderPenaltyAddHistory(runX, runHistory); + if (!runColor) + result += finderPenaltyCountPatterns(runHistory) * PENALTY_N3; + runColor = module(x, y); + runX = 1; + } + } + result += finderPenaltyTerminateAndCount(runColor, runX, runHistory) * PENALTY_N3; + } + // Adjacent modules in column having same color, and finder-like patterns + for (int x = 0; x < size; x++) { + bool runColor = false; + int runY = 0; + std::array runHistory = {}; + for (int y = 0; y < size; y++) { + if (module(x, y) == runColor) { + runY++; + if (runY == 5) + result += PENALTY_N1; + else if (runY > 5) + result++; + } else { + finderPenaltyAddHistory(runY, runHistory); + if (!runColor) + result += finderPenaltyCountPatterns(runHistory) * PENALTY_N3; + runColor = module(x, y); + runY = 1; + } + } + result += finderPenaltyTerminateAndCount(runColor, runY, runHistory) * PENALTY_N3; + } + + // 2*2 blocks of modules having same color + for (int y = 0; y < size - 1; y++) { + for (int x = 0; x < size - 1; x++) { + bool color = module(x, y); + if ( color == module(x + 1, y) && + color == module(x, y + 1) && + color == module(x + 1, y + 1)) + result += PENALTY_N2; + } + } + + // Balance of black and white modules + int black = 0; + for (const vector &row : modules) { + for (bool color : row) { + if (color) + black++; + } + } + int total = size * size; // Note that size is odd, so black/total != 1/2 + // Compute the smallest integer k >= 0 such that (45-5k)% <= black/total <= (55+5k)% + int k = static_cast((std::abs(black * 20L - total * 10L) + total - 1) / total) - 1; + result += k * PENALTY_N4; + return result; +} + + +vector QrCode::getAlignmentPatternPositions() const { + if (version == 1) + return vector(); + else { + int numAlign = version / 7 + 2; + int step = (version == 32) ? 26 : + (version*4 + numAlign*2 + 1) / (numAlign*2 - 2) * 2; + vector result; + for (int i = 0, pos = size - 7; i < numAlign - 1; i++, pos -= step) + result.insert(result.begin(), pos); + result.insert(result.begin(), 6); + return result; + } +} + + +int QrCode::getNumRawDataModules(int ver) { + if (ver < MIN_VERSION || ver > MAX_VERSION) + throw std::domain_error("Version number out of range"); + int result = (16 * ver + 128) * ver + 64; + if (ver >= 2) { + int numAlign = ver / 7 + 2; + result -= (25 * numAlign - 10) * numAlign - 55; + if (ver >= 7) + result -= 36; + } + if (!(208 <= result && result <= 29648)) + throw std::logic_error("Assertion error"); + return result; +} + + +int QrCode::getNumDataCodewords(int ver, Ecc ecl) { + return getNumRawDataModules(ver) / 8 + - ECC_CODEWORDS_PER_BLOCK [static_cast(ecl)][ver] + * NUM_ERROR_CORRECTION_BLOCKS[static_cast(ecl)][ver]; +} + + +vector QrCode::reedSolomonComputeDivisor(int degree) { + if (degree < 1 || degree > 255) + throw std::domain_error("Degree out of range"); + // Polynomial coefficients are stored from highest to lowest power, excluding the leading term which is always 1. + // For example the polynomial x^3 + 255x^2 + 8x + 93 is stored as the uint8 array {255, 8, 93}. + vector result(static_cast(degree)); + result.at(result.size() - 1) = 1; // Start off with the monomial x^0 + + // Compute the product polynomial (x - r^0) * (x - r^1) * (x - r^2) * ... * (x - r^{degree-1}), + // and drop the highest monomial term which is always 1x^degree. + // Note that r = 0x02, which is a generator element of this field GF(2^8/0x11D). + uint8_t root = 1; + for (int i = 0; i < degree; i++) { + // Multiply the current product by (x - r^i) + for (size_t j = 0; j < result.size(); j++) { + result.at(j) = reedSolomonMultiply(result.at(j), root); + if (j + 1 < result.size()) + result.at(j) ^= result.at(j + 1); + } + root = reedSolomonMultiply(root, 0x02); + } + return result; +} + + +vector QrCode::reedSolomonComputeRemainder(const vector &data, const vector &divisor) { + vector result(divisor.size()); + for (uint8_t b : data) { // Polynomial division + uint8_t factor = b ^ result.at(0); + result.erase(result.begin()); + result.push_back(0); + for (size_t i = 0; i < result.size(); i++) + result.at(i) ^= reedSolomonMultiply(divisor.at(i), factor); + } + return result; +} + + +uint8_t QrCode::reedSolomonMultiply(uint8_t x, uint8_t y) { + // Russian peasant multiplication + int z = 0; + for (int i = 7; i >= 0; i--) { + z = (z << 1) ^ ((z >> 7) * 0x11D); + z ^= ((y >> i) & 1) * x; + } + if (z >> 8 != 0) + throw std::logic_error("Assertion error"); + return static_cast(z); +} + + +int QrCode::finderPenaltyCountPatterns(const std::array &runHistory) const { + int n = runHistory.at(1); + if (n > size * 3) + throw std::logic_error("Assertion error"); + bool core = n > 0 && runHistory.at(2) == n && runHistory.at(3) == n * 3 && runHistory.at(4) == n && runHistory.at(5) == n; + return (core && runHistory.at(0) >= n * 4 && runHistory.at(6) >= n ? 1 : 0) + + (core && runHistory.at(6) >= n * 4 && runHistory.at(0) >= n ? 1 : 0); +} + + +int QrCode::finderPenaltyTerminateAndCount(bool currentRunColor, int currentRunLength, std::array &runHistory) const { + if (currentRunColor) { // Terminate black run + finderPenaltyAddHistory(currentRunLength, runHistory); + currentRunLength = 0; + } + currentRunLength += size; // Add white border to final run + finderPenaltyAddHistory(currentRunLength, runHistory); + return finderPenaltyCountPatterns(runHistory); +} + + +void QrCode::finderPenaltyAddHistory(int currentRunLength, std::array &runHistory) const { + if (runHistory.at(0) == 0) + currentRunLength += size; // Add white border to initial run + std::copy_backward(runHistory.cbegin(), runHistory.cend() - 1, runHistory.end()); + runHistory.at(0) = currentRunLength; +} + + +bool QrCode::getBit(long x, int i) { + return ((x >> i) & 1) != 0; +} + + +/*---- Tables of constants ----*/ + +const int QrCode::PENALTY_N1 = 3; +const int QrCode::PENALTY_N2 = 3; +const int QrCode::PENALTY_N3 = 40; +const int QrCode::PENALTY_N4 = 10; + + +const int8_t QrCode::ECC_CODEWORDS_PER_BLOCK[4][41] = { + // Version: (note that index 0 is for padding, and is set to an illegal value) + //0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40 Error correction level + {-1, 7, 10, 15, 20, 26, 18, 20, 24, 30, 18, 20, 24, 26, 30, 22, 24, 28, 30, 28, 28, 28, 28, 30, 30, 26, 28, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // Low + {-1, 10, 16, 26, 18, 24, 16, 18, 22, 22, 26, 30, 22, 22, 24, 24, 28, 28, 26, 26, 26, 26, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28, 28}, // Medium + {-1, 13, 22, 18, 26, 18, 24, 18, 22, 20, 24, 28, 26, 24, 20, 30, 24, 28, 28, 26, 30, 28, 30, 30, 30, 30, 28, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // Quartile + {-1, 17, 28, 22, 16, 22, 28, 26, 26, 24, 28, 24, 28, 22, 24, 24, 30, 28, 28, 26, 28, 30, 24, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30}, // High +}; + +const int8_t QrCode::NUM_ERROR_CORRECTION_BLOCKS[4][41] = { + // Version: (note that index 0 is for padding, and is set to an illegal value) + //0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40 Error correction level + {-1, 1, 1, 1, 1, 1, 2, 2, 2, 2, 4, 4, 4, 4, 4, 6, 6, 6, 6, 7, 8, 8, 9, 9, 10, 12, 12, 12, 13, 14, 15, 16, 17, 18, 19, 19, 20, 21, 22, 24, 25}, // Low + {-1, 1, 1, 1, 2, 2, 4, 4, 4, 5, 5, 5, 8, 9, 9, 10, 10, 11, 13, 14, 16, 17, 17, 18, 20, 21, 23, 25, 26, 28, 29, 31, 33, 35, 37, 38, 40, 43, 45, 47, 49}, // Medium + {-1, 1, 1, 2, 2, 4, 4, 6, 6, 8, 8, 8, 10, 12, 16, 12, 17, 16, 18, 21, 20, 23, 23, 25, 27, 29, 34, 34, 35, 38, 40, 43, 45, 48, 51, 53, 56, 59, 62, 65, 68}, // Quartile + {-1, 1, 1, 2, 4, 4, 4, 5, 6, 8, 8, 11, 11, 16, 16, 18, 16, 19, 21, 25, 25, 25, 34, 30, 32, 35, 37, 40, 42, 45, 48, 51, 54, 57, 60, 63, 66, 70, 74, 77, 81}, // High +}; + + +data_too_long::data_too_long(const std::string &msg) : + std::length_error(msg) {} + + + +BitBuffer::BitBuffer() + : std::vector() {} + + +void BitBuffer::appendBits(std::uint32_t val, int len) { + if (len < 0 || len > 31 || val >> len != 0) + throw std::domain_error("Value out of range"); + for (int i = len - 1; i >= 0; i--) // Append bit by bit + this->push_back(((val >> i) & 1) != 0); +} + +} diff --git a/phonelibs/qrcode/QrCode.hpp b/phonelibs/qrcode/QrCode.hpp new file mode 100644 index 000000000..7341e4102 --- /dev/null +++ b/phonelibs/qrcode/QrCode.hpp @@ -0,0 +1,556 @@ +/* + * QR Code generator library (C++) + * + * Copyright (c) Project Nayuki. (MIT License) + * https://www.nayuki.io/page/qr-code-generator-library + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * - The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * - The Software is provided "as is", without warranty of any kind, express or + * implied, including but not limited to the warranties of merchantability, + * fitness for a particular purpose and noninfringement. In no event shall the + * authors or copyright holders be liable for any claim, damages or other + * liability, whether in an action of contract, tort or otherwise, arising from, + * out of or in connection with the Software or the use or other dealings in the + * Software. + */ + +#pragma once + +#include +#include +#include +#include +#include + + +namespace qrcodegen { + +/* + * A segment of character/binary/control data in a QR Code symbol. + * Instances of this class are immutable. + * The mid-level way to create a segment is to take the payload data + * and call a static factory function such as QrSegment::makeNumeric(). + * The low-level way to create a segment is to custom-make the bit buffer + * and call the QrSegment() constructor with appropriate values. + * This segment class imposes no length restrictions, but QR Codes have restrictions. + * Even in the most favorable conditions, a QR Code can only hold 7089 characters of data. + * Any segment longer than this is meaningless for the purpose of generating QR Codes. + */ +class QrSegment final { + + /*---- Public helper enumeration ----*/ + + /* + * Describes how a segment's data bits are interpreted. Immutable. + */ + public: class Mode final { + + /*-- Constants --*/ + + public: static const Mode NUMERIC; + public: static const Mode ALPHANUMERIC; + public: static const Mode BYTE; + public: static const Mode KANJI; + public: static const Mode ECI; + + + /*-- Fields --*/ + + // The mode indicator bits, which is a uint4 value (range 0 to 15). + private: int modeBits; + + // Number of character count bits for three different version ranges. + private: int numBitsCharCount[3]; + + + /*-- Constructor --*/ + + private: Mode(int mode, int cc0, int cc1, int cc2); + + + /*-- Methods --*/ + + /* + * (Package-private) Returns the mode indicator bits, which is an unsigned 4-bit value (range 0 to 15). + */ + public: int getModeBits() const; + + /* + * (Package-private) Returns the bit width of the character count field for a segment in + * this mode in a QR Code at the given version number. The result is in the range [0, 16]. + */ + public: int numCharCountBits(int ver) const; + + }; + + + + /*---- Static factory functions (mid level) ----*/ + + /* + * Returns a segment representing the given binary data encoded in + * byte mode. All input byte vectors are acceptable. Any text string + * can be converted to UTF-8 bytes and encoded as a byte mode segment. + */ + public: static QrSegment makeBytes(const std::vector &data); + + + /* + * Returns a segment representing the given string of decimal digits encoded in numeric mode. + */ + public: static QrSegment makeNumeric(const char *digits); + + + /* + * Returns a segment representing the given text string encoded in alphanumeric mode. + * The characters allowed are: 0 to 9, A to Z (uppercase only), space, + * dollar, percent, asterisk, plus, hyphen, period, slash, colon. + */ + public: static QrSegment makeAlphanumeric(const char *text); + + + /* + * Returns a list of zero or more segments to represent the given text string. The result + * may use various segment modes and switch modes to optimize the length of the bit stream. + */ + public: static std::vector makeSegments(const char *text); + + + /* + * Returns a segment representing an Extended Channel Interpretation + * (ECI) designator with the given assignment value. + */ + public: static QrSegment makeEci(long assignVal); + + + /*---- Public static helper functions ----*/ + + /* + * Tests whether the given string can be encoded as a segment in alphanumeric mode. + * A string is encodable iff each character is in the following set: 0 to 9, A to Z + * (uppercase only), space, dollar, percent, asterisk, plus, hyphen, period, slash, colon. + */ + public: static bool isAlphanumeric(const char *text); + + + /* + * Tests whether the given string can be encoded as a segment in numeric mode. + * A string is encodable iff each character is in the range 0 to 9. + */ + public: static bool isNumeric(const char *text); + + + + /*---- Instance fields ----*/ + + /* The mode indicator of this segment. Accessed through getMode(). */ + private: Mode mode; + + /* The length of this segment's unencoded data. Measured in characters for + * numeric/alphanumeric/kanji mode, bytes for byte mode, and 0 for ECI mode. + * Always zero or positive. Not the same as the data's bit length. + * Accessed through getNumChars(). */ + private: int numChars; + + /* The data bits of this segment. Accessed through getData(). */ + private: std::vector data; + + + /*---- Constructors (low level) ----*/ + + /* + * Creates a new QR Code segment with the given attributes and data. + * The character count (numCh) must agree with the mode and the bit buffer length, + * but the constraint isn't checked. The given bit buffer is copied and stored. + */ + public: QrSegment(Mode md, int numCh, const std::vector &dt); + + + /* + * Creates a new QR Code segment with the given parameters and data. + * The character count (numCh) must agree with the mode and the bit buffer length, + * but the constraint isn't checked. The given bit buffer is moved and stored. + */ + public: QrSegment(Mode md, int numCh, std::vector &&dt); + + + /*---- Methods ----*/ + + /* + * Returns the mode field of this segment. + */ + public: Mode getMode() const; + + + /* + * Returns the character count field of this segment. + */ + public: int getNumChars() const; + + + /* + * Returns the data bits of this segment. + */ + public: const std::vector &getData() const; + + + // (Package-private) Calculates the number of bits needed to encode the given segments at + // the given version. Returns a non-negative number if successful. Otherwise returns -1 if a + // segment has too many characters to fit its length field, or the total bits exceeds INT_MAX. + public: static int getTotalBits(const std::vector &segs, int version); + + + /*---- Private constant ----*/ + + /* The set of all legal characters in alphanumeric mode, where + * each character value maps to the index in the string. */ + private: static const char *ALPHANUMERIC_CHARSET; + +}; + + + +/* + * A QR Code symbol, which is a type of two-dimension barcode. + * Invented by Denso Wave and described in the ISO/IEC 18004 standard. + * Instances of this class represent an immutable square grid of black and white cells. + * The class provides static factory functions to create a QR Code from text or binary data. + * The class covers the QR Code Model 2 specification, supporting all versions (sizes) + * from 1 to 40, all 4 error correction levels, and 4 character encoding modes. + * + * Ways to create a QR Code object: + * - High level: Take the payload data and call QrCode::encodeText() or QrCode::encodeBinary(). + * - Mid level: Custom-make the list of segments and call QrCode::encodeSegments(). + * - Low level: Custom-make the array of data codeword bytes (including + * segment headers and final padding, excluding error correction codewords), + * supply the appropriate version number, and call the QrCode() constructor. + * (Note that all ways require supplying the desired error correction level.) + */ +class QrCode final { + + /*---- Public helper enumeration ----*/ + + /* + * The error correction level in a QR Code symbol. + */ + public: enum class Ecc { + LOW = 0 , // The QR Code can tolerate about 7% erroneous codewords + MEDIUM , // The QR Code can tolerate about 15% erroneous codewords + QUARTILE, // The QR Code can tolerate about 25% erroneous codewords + HIGH , // The QR Code can tolerate about 30% erroneous codewords + }; + + + // Returns a value in the range 0 to 3 (unsigned 2-bit integer). + private: static int getFormatBits(Ecc ecl); + + + + /*---- Static factory functions (high level) ----*/ + + /* + * Returns a QR Code representing the given Unicode text string at the given error correction level. + * As a conservative upper bound, this function is guaranteed to succeed for strings that have 2953 or fewer + * UTF-8 code units (not Unicode code points) if the low error correction level is used. The smallest possible + * QR Code version is automatically chosen for the output. The ECC level of the result may be higher than + * the ecl argument if it can be done without increasing the version. + */ + public: static QrCode encodeText(const char *text, Ecc ecl); + + + /* + * Returns a QR Code representing the given binary data at the given error correction level. + * This function always encodes using the binary segment mode, not any text mode. The maximum number of + * bytes allowed is 2953. The smallest possible QR Code version is automatically chosen for the output. + * The ECC level of the result may be higher than the ecl argument if it can be done without increasing the version. + */ + public: static QrCode encodeBinary(const std::vector &data, Ecc ecl); + + + /*---- Static factory functions (mid level) ----*/ + + /* + * Returns a QR Code representing the given segments with the given encoding parameters. + * The smallest possible QR Code version within the given range is automatically + * chosen for the output. Iff boostEcl is true, then the ECC level of the result + * may be higher than the ecl argument if it can be done without increasing the + * version. The mask number is either between 0 to 7 (inclusive) to force that + * mask, or -1 to automatically choose an appropriate mask (which may be slow). + * This function allows the user to create a custom sequence of segments that switches + * between modes (such as alphanumeric and byte) to encode text in less space. + * This is a mid-level API; the high-level API is encodeText() and encodeBinary(). + */ + public: static QrCode encodeSegments(const std::vector &segs, Ecc ecl, + int minVersion=1, int maxVersion=40, int mask=-1, bool boostEcl=true); // All optional parameters + + + + /*---- Instance fields ----*/ + + // Immutable scalar parameters: + + /* The version number of this QR Code, which is between 1 and 40 (inclusive). + * This determines the size of this barcode. */ + private: int version; + + /* The width and height of this QR Code, measured in modules, between + * 21 and 177 (inclusive). This is equal to version * 4 + 17. */ + private: int size; + + /* The error correction level used in this QR Code. */ + private: Ecc errorCorrectionLevel; + + /* The index of the mask pattern used in this QR Code, which is between 0 and 7 (inclusive). + * Even if a QR Code is created with automatic masking requested (mask = -1), + * the resulting object still has a mask value between 0 and 7. */ + private: int mask; + + // Private grids of modules/pixels, with dimensions of size*size: + + // The modules of this QR Code (false = white, true = black). + // Immutable after constructor finishes. Accessed through getModule(). + private: std::vector > modules; + + // Indicates function modules that are not subjected to masking. Discarded when constructor finishes. + private: std::vector > isFunction; + + + + /*---- Constructor (low level) ----*/ + + /* + * Creates a new QR Code with the given version number, + * error correction level, data codeword bytes, and mask number. + * This is a low-level API that most users should not use directly. + * A mid-level API is the encodeSegments() function. + */ + public: QrCode(int ver, Ecc ecl, const std::vector &dataCodewords, int msk); + + + + /*---- Public instance methods ----*/ + + /* + * Returns this QR Code's version, in the range [1, 40]. + */ + public: int getVersion() const; + + + /* + * Returns this QR Code's size, in the range [21, 177]. + */ + public: int getSize() const; + + + /* + * Returns this QR Code's error correction level. + */ + public: Ecc getErrorCorrectionLevel() const; + + + /* + * Returns this QR Code's mask, in the range [0, 7]. + */ + public: int getMask() const; + + + /* + * Returns the color of the module (pixel) at the given coordinates, which is false + * for white or true for black. The top left corner has the coordinates (x=0, y=0). + * If the given coordinates are out of bounds, then false (white) is returned. + */ + public: bool getModule(int x, int y) const; + + + /* + * Returns a string of SVG code for an image depicting this QR Code, with the given number + * of border modules. The string always uses Unix newlines (\n), regardless of the platform. + */ + public: std::string toSvgString(int border) const; + + + + /*---- Private helper methods for constructor: Drawing function modules ----*/ + + // Reads this object's version field, and draws and marks all function modules. + private: void drawFunctionPatterns(); + + + // Draws two copies of the format bits (with its own error correction code) + // based on the given mask and this object's error correction level field. + private: void drawFormatBits(int msk); + + + // Draws two copies of the version bits (with its own error correction code), + // based on this object's version field, iff 7 <= version <= 40. + private: void drawVersion(); + + + // Draws a 9*9 finder pattern including the border separator, + // with the center module at (x, y). Modules can be out of bounds. + private: void drawFinderPattern(int x, int y); + + + // Draws a 5*5 alignment pattern, with the center module + // at (x, y). All modules must be in bounds. + private: void drawAlignmentPattern(int x, int y); + + + // Sets the color of a module and marks it as a function module. + // Only used by the constructor. Coordinates must be in bounds. + private: void setFunctionModule(int x, int y, bool isBlack); + + + // Returns the color of the module at the given coordinates, which must be in range. + private: bool module(int x, int y) const; + + + /*---- Private helper methods for constructor: Codewords and masking ----*/ + + // Returns a new byte string representing the given data with the appropriate error correction + // codewords appended to it, based on this object's version and error correction level. + private: std::vector addEccAndInterleave(const std::vector &data) const; + + + // Draws the given sequence of 8-bit codewords (data and error correction) onto the entire + // data area of this QR Code. Function modules need to be marked off before this is called. + private: void drawCodewords(const std::vector &data); + + + // XORs the codeword modules in this QR Code with the given mask pattern. + // The function modules must be marked and the codeword bits must be drawn + // before masking. Due to the arithmetic of XOR, calling applyMask() with + // the same mask value a second time will undo the mask. A final well-formed + // QR Code needs exactly one (not zero, two, etc.) mask applied. + private: void applyMask(int msk); + + + // Calculates and returns the penalty score based on state of this QR Code's current modules. + // This is used by the automatic mask choice algorithm to find the mask pattern that yields the lowest score. + private: long getPenaltyScore() const; + + + + /*---- Private helper functions ----*/ + + // Returns an ascending list of positions of alignment patterns for this version number. + // Each position is in the range [0,177), and are used on both the x and y axes. + // This could be implemented as lookup table of 40 variable-length lists of unsigned bytes. + private: std::vector getAlignmentPatternPositions() const; + + + // Returns the number of data bits that can be stored in a QR Code of the given version number, after + // all function modules are excluded. This includes remainder bits, so it might not be a multiple of 8. + // The result is in the range [208, 29648]. This could be implemented as a 40-entry lookup table. + private: static int getNumRawDataModules(int ver); + + + // Returns the number of 8-bit data (i.e. not error correction) codewords contained in any + // QR Code of the given version number and error correction level, with remainder bits discarded. + // This stateless pure function could be implemented as a (40*4)-cell lookup table. + private: static int getNumDataCodewords(int ver, Ecc ecl); + + + // Returns a Reed-Solomon ECC generator polynomial for the given degree. This could be + // implemented as a lookup table over all possible parameter values, instead of as an algorithm. + private: static std::vector reedSolomonComputeDivisor(int degree); + + + // Returns the Reed-Solomon error correction codeword for the given data and divisor polynomials. + private: static std::vector reedSolomonComputeRemainder(const std::vector &data, const std::vector &divisor); + + + // Returns the product of the two given field elements modulo GF(2^8/0x11D). + // All inputs are valid. This could be implemented as a 256*256 lookup table. + private: static std::uint8_t reedSolomonMultiply(std::uint8_t x, std::uint8_t y); + + + // Can only be called immediately after a white run is added, and + // returns either 0, 1, or 2. A helper function for getPenaltyScore(). + private: int finderPenaltyCountPatterns(const std::array &runHistory) const; + + + // Must be called at the end of a line (row or column) of modules. A helper function for getPenaltyScore(). + private: int finderPenaltyTerminateAndCount(bool currentRunColor, int currentRunLength, std::array &runHistory) const; + + + // Pushes the given value to the front and drops the last value. A helper function for getPenaltyScore(). + private: void finderPenaltyAddHistory(int currentRunLength, std::array &runHistory) const; + + + // Returns true iff the i'th bit of x is set to 1. + private: static bool getBit(long x, int i); + + + /*---- Constants and tables ----*/ + + // The minimum version number supported in the QR Code Model 2 standard. + public: static constexpr int MIN_VERSION = 1; + + // The maximum version number supported in the QR Code Model 2 standard. + public: static constexpr int MAX_VERSION = 40; + + + // For use in getPenaltyScore(), when evaluating which mask is best. + private: static const int PENALTY_N1; + private: static const int PENALTY_N2; + private: static const int PENALTY_N3; + private: static const int PENALTY_N4; + + + private: static const std::int8_t ECC_CODEWORDS_PER_BLOCK[4][41]; + private: static const std::int8_t NUM_ERROR_CORRECTION_BLOCKS[4][41]; + +}; + + + +/*---- Public exception class ----*/ + +/* + * Thrown when the supplied data does not fit any QR Code version. Ways to handle this exception include: + * - Decrease the error correction level if it was greater than Ecc::LOW. + * - If the encodeSegments() function was called with a maxVersion argument, then increase + * it if it was less than QrCode::MAX_VERSION. (This advice does not apply to the other + * factory functions because they search all versions up to QrCode::MAX_VERSION.) + * - Split the text data into better or optimal segments in order to reduce the number of bits required. + * - Change the text or binary data to be shorter. + * - Change the text to fit the character set of a particular segment mode (e.g. alphanumeric). + * - Propagate the error upward to the caller/user. + */ +class data_too_long : public std::length_error { + + public: explicit data_too_long(const std::string &msg); + +}; + + + +/* + * An appendable sequence of bits (0s and 1s). Mainly used by QrSegment. + */ +class BitBuffer final : public std::vector { + + /*---- Constructor ----*/ + + // Creates an empty bit buffer (length 0). + public: BitBuffer(); + + + + /*---- Method ----*/ + + // Appends the given number of low-order bits of the given value + // to this buffer. Requires 0 <= len <= 31 and val < 2^len. + public: void appendBits(std::uint32_t val, int len); + +}; + +} diff --git a/phonelibs/snpe/larch64 b/phonelibs/snpe/larch64 new file mode 120000 index 000000000..e5c222b1c --- /dev/null +++ b/phonelibs/snpe/larch64 @@ -0,0 +1 @@ +aarch64-linux-gcc4.9 \ No newline at end of file diff --git a/release/build_release2.sh b/release/build_release2.sh index f47686a6b..656eb20f7 100755 --- a/release/build_release2.sh +++ b/release/build_release2.sh @@ -56,7 +56,7 @@ export PYTHONPATH="/data/openpilot:/data/openpilot/pyextra" SCONS_CACHE=1 scons -j3 # Run tests -nosetests -s selfdrive/test/test_openpilot.py +python selfdrive/test/test_manager.py selfdrive/car/tests/test_car_interfaces.py # Cleanup diff --git a/selfdrive/assets/offroad/circled-checkmark-empty.png b/selfdrive/assets/offroad/circled-checkmark-empty.png deleted file mode 100644 index e6740f110..000000000 Binary files a/selfdrive/assets/offroad/circled-checkmark-empty.png and /dev/null differ diff --git a/selfdrive/assets/offroad/circled-checkmark.png b/selfdrive/assets/offroad/circled-checkmark.png deleted file mode 100644 index bc6b49585..000000000 Binary files a/selfdrive/assets/offroad/circled-checkmark.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_arrow.png b/selfdrive/assets/offroad/illustration_arrow.png deleted file mode 100644 index 221908664..000000000 Binary files a/selfdrive/assets/offroad/illustration_arrow.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_sim_absent.png b/selfdrive/assets/offroad/illustration_sim_absent.png deleted file mode 100644 index 554097409..000000000 Binary files a/selfdrive/assets/offroad/illustration_sim_absent.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_sim_present.png b/selfdrive/assets/offroad/illustration_sim_present.png deleted file mode 100644 index 0856795f0..000000000 Binary files a/selfdrive/assets/offroad/illustration_sim_present.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_training_lane_01.png b/selfdrive/assets/offroad/illustration_training_lane_01.png deleted file mode 100644 index 27d9bcee3..000000000 Binary files a/selfdrive/assets/offroad/illustration_training_lane_01.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_training_lane_02.png b/selfdrive/assets/offroad/illustration_training_lane_02.png deleted file mode 100644 index 4f3e2ef44..000000000 Binary files a/selfdrive/assets/offroad/illustration_training_lane_02.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_training_lead_01.png b/selfdrive/assets/offroad/illustration_training_lead_01.png deleted file mode 100644 index 12f3f6bae..000000000 Binary files a/selfdrive/assets/offroad/illustration_training_lead_01.png and /dev/null differ diff --git a/selfdrive/assets/offroad/illustration_training_lead_02.png b/selfdrive/assets/offroad/illustration_training_lead_02.png deleted file mode 100644 index 26c9ffe71..000000000 Binary files a/selfdrive/assets/offroad/illustration_training_lead_02.png and /dev/null differ diff --git a/selfdrive/assets/offroad/indicator_wifi_0.png b/selfdrive/assets/offroad/indicator_wifi_0.png deleted file mode 100644 index 9cf9762ad..000000000 Binary files a/selfdrive/assets/offroad/indicator_wifi_0.png and /dev/null differ diff --git a/selfdrive/assets/offroad/indicator_wifi_100.png b/selfdrive/assets/offroad/indicator_wifi_100.png deleted file mode 100644 index dc9f28fab..000000000 Binary files a/selfdrive/assets/offroad/indicator_wifi_100.png and /dev/null differ diff --git a/selfdrive/assets/offroad/indicator_wifi_25.png b/selfdrive/assets/offroad/indicator_wifi_25.png deleted file mode 100644 index cbf9bc89e..000000000 Binary files a/selfdrive/assets/offroad/indicator_wifi_25.png and /dev/null differ diff --git a/selfdrive/assets/offroad/indicator_wifi_50.png b/selfdrive/assets/offroad/indicator_wifi_50.png deleted file mode 100644 index 8ee118a41..000000000 Binary files a/selfdrive/assets/offroad/indicator_wifi_50.png and /dev/null differ diff --git a/selfdrive/assets/offroad/indicator_wifi_75.png b/selfdrive/assets/offroad/indicator_wifi_75.png deleted file mode 100644 index bcbebce85..000000000 Binary files a/selfdrive/assets/offroad/indicator_wifi_75.png and /dev/null differ diff --git a/selfdrive/assets/offroad/tc.html b/selfdrive/assets/offroad/tc.html new file mode 100644 index 000000000..4ab749071 --- /dev/null +++ b/selfdrive/assets/offroad/tc.html @@ -0,0 +1,50 @@ + + + + + openpilot Terms of Service + + + + +

openpilot Terms and Conditions

+

The Terms and Conditions below are effective for all users

+

Last Updated on October 18, 2019

+

Please read these Terms of Use (“Terms”) carefully before using openpilot which is open-sourced software developed by Comma.ai, Inc., a corporation organized under the laws of Delaware (“comma,” “us,” “we,” or “our”).

+

Before using and by accessing openpilot, you indicate that you have read, understood, and agree to these Terms. These Terms apply to all users and others who access or use openpilot. If others use openpilot through your user account or vehicle, you are responsible to ensure that they only use openpilot when it is safe to do so, and in compliance with these Terms and with applicable law. If you disagree with any part of the Terms, you should not access or use openpilot.

+

Communications

+

You agree that comma may contact you by email or telephone in connection with openpilot or for other business purposes. You may opt out of receiving email messages at any time by contacting us at support@comma.ai.

+

We collect, use, and share information from and about you and your vehicle in connection with openpilot. You consent to comma accessing the systems associated with openpilot, without additional notice or consent, for the purposes of providing openpilot, data collection, software updates, safety and cybersecurity, suspension or removal of your account, and as disclosed in the Privacy Policy (available at https://my.comma.ai/privacy).

+

Safety

+

openpilot performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) designed for use in compatible motor vehicles. While using openpilot, it is your responsibility to obey all laws, traffic rules, and traffic regulations governing your vehicle and its operation. Access to and use of openpilot is at your own risk and responsibility, and openpilot should be accessed and/or used only when you can do so safely.

+

openpilot does not make your vehicle “autonomous” or capable of operation without the active monitoring of a licensed driver. It is designed to assist a licensed driver. A licensed driver must pay attention to the road, remain aware of navigation at all times, and be prepared to take immediate action. Failure to do so can cause damage, injury, or death.

+

Supported Locations and Models

+

openpilot is compatible only with particular makes and models of vehicles. For a complete list of currently supported vehicles, visit https://comma.ai. openpilot will not function properly when installed in an incompatible vehicle. openpilot is compatible only within the geographical boundaries of the United States of America.

+

Indemnification

+

To the maximum extent allowable by law, you agree to defend, indemnify and hold harmless comma, and its employees, partners, suppliers, contractors, investors, agents, officers, directors, and affiliates, from and against any and all claims, damages, causes of action, penalties, interest, demands, obligations, losses, liabilities, costs or debt, additional taxes, and expenses (including but not limited to attorneys’ fees), resulting from or arising out of (i) your use and access of, or inability to use or access, openpilot, (ii) your breach of these Terms, (iii) the inaccuracy of any information, representation or warranty made by you, (iv) activities of anyone other than you in connection with openpilot conducted through your comma device or account, (v) any other of your activities under or in connection with these Terms or openpilot.

+

Limitation of Liability

+

In no event shall comma, nor its directors, employees, partners, agents, suppliers, or affiliates, be liable for any indirect, incidental, special, consequential or punitive damages, including without limitation, loss of profits, data, use, goodwill, or other intangible losses, resulting from (i) your access to or use of or inability to access or use of the Software; or (ii) any conduct or content of any third party on the Software whether based on warranty, contract, tort (including negligence) or any other legal theory, whether or not we have been informed of the possibility of such damage, and even if a remedy set forth herein is found to have failed of its essential purpose.

+

No Warranty or Obligations to Maintain or Service

+

comma provides openpilot without representations, conditions, or warranties of any kind. openpilot is provided on an “AS IS” and “AS AVAILABLE” basis, including with all faults and errors as may occur. To the extent permitted by law and unless prohibited by law, comma on behalf of itself and all persons and parties acting by, through, or for comma, explicitly disclaims all warranties or conditions, express, implied, or collateral, including any implied warranties of merchantability, satisfactory quality, and fitness for a particular purpose in respect of openpilot.

+

To the extent permitted by law, comma does not warrant the operation, performance, or availability of openpilot under all conditions. comma is not responsible for any failures caused by server errors, misdirected or redirected transmissions, failed internet connections, interruptions or failures in the transmission of data, any computer virus, or any acts or omissions of third parties that damage the network or impair wireless service.

+

We undertake reasonable measures to preserve and secure information collected through our openpilot. However, no data collection, transmission or storage system is 100% secure, and there is always a risk that your information may be intercepted without our consent. In using openpilot, you acknowledge that comma is not responsible for intercepted information, and you hereby release us from any and all claims arising out of or related to the use of intercepted information in any unauthorized manner.

+

By providing openpilot, comma does not transfer or license its intellectual property or grant rights in its brand names, nor does comma make representations with respect to third-party intellectual property rights.

+

We are not obligated to provide any maintenance or support for openpilot, technical or otherwise. If we voluntarily provide any maintenance or support for openpilot, we may stop any such maintenance, support, or services at any time in our sole discretion.

+

Modification of Software

+

In no event shall comma, nor its directors, employees, partners, agents, suppliers, or affiliates, be liable if you choose to modify the software.

+

Changes

+

We reserve the right, at our sole discretion, to modify or replace these Terms at any time. If a revision is material we will provide at least 15 days’ notice prior to any new terms taking effect. What constitutes a material change will be determined at our sole discretion.

+

By continuing to access or use our Software after any revisions become effective, you agree to be bound by the revised terms. If you do not agree to the new terms, you are no longer authorized to use the Software.

+

Contact Us

+

If you have any questions about these Terms, please contact us at support@comma.ai.

+ + diff --git a/selfdrive/assets/training/step0.jpg b/selfdrive/assets/training/step0.jpg new file mode 100644 index 000000000..65917358b Binary files /dev/null and b/selfdrive/assets/training/step0.jpg differ diff --git a/selfdrive/assets/training/step1.jpg b/selfdrive/assets/training/step1.jpg new file mode 100644 index 000000000..d03e1f3b4 Binary files /dev/null and b/selfdrive/assets/training/step1.jpg differ diff --git a/selfdrive/assets/training/step10.jpg b/selfdrive/assets/training/step10.jpg new file mode 100644 index 000000000..c720acc52 Binary files /dev/null and b/selfdrive/assets/training/step10.jpg differ diff --git a/selfdrive/assets/training/step11.jpg b/selfdrive/assets/training/step11.jpg new file mode 100644 index 000000000..776a389bc Binary files /dev/null and b/selfdrive/assets/training/step11.jpg differ diff --git a/selfdrive/assets/training/step12.jpg b/selfdrive/assets/training/step12.jpg new file mode 100644 index 000000000..2a364b3c7 Binary files /dev/null and b/selfdrive/assets/training/step12.jpg differ diff --git a/selfdrive/assets/training/step13.jpg b/selfdrive/assets/training/step13.jpg new file mode 100644 index 000000000..8f958f3bc Binary files /dev/null and b/selfdrive/assets/training/step13.jpg differ diff --git a/selfdrive/assets/training/step14.jpg b/selfdrive/assets/training/step14.jpg new file mode 100644 index 000000000..9b1044ae6 Binary files /dev/null and b/selfdrive/assets/training/step14.jpg differ diff --git a/selfdrive/assets/training/step2.jpg b/selfdrive/assets/training/step2.jpg new file mode 100644 index 000000000..1b1aa83bd Binary files /dev/null and b/selfdrive/assets/training/step2.jpg differ diff --git a/selfdrive/assets/training/step3.jpg b/selfdrive/assets/training/step3.jpg new file mode 100644 index 000000000..556a36b0e Binary files /dev/null and b/selfdrive/assets/training/step3.jpg differ diff --git a/selfdrive/assets/training/step4.jpg b/selfdrive/assets/training/step4.jpg new file mode 100644 index 000000000..4e981f1b8 Binary files /dev/null and b/selfdrive/assets/training/step4.jpg differ diff --git a/selfdrive/assets/training/step5.jpg b/selfdrive/assets/training/step5.jpg new file mode 100644 index 000000000..880374aa2 Binary files /dev/null and b/selfdrive/assets/training/step5.jpg differ diff --git a/selfdrive/assets/training/step6.jpg b/selfdrive/assets/training/step6.jpg new file mode 100644 index 000000000..0201bc8ed Binary files /dev/null and b/selfdrive/assets/training/step6.jpg differ diff --git a/selfdrive/assets/training/step7.jpg b/selfdrive/assets/training/step7.jpg new file mode 100644 index 000000000..4198df06d Binary files /dev/null and b/selfdrive/assets/training/step7.jpg differ diff --git a/selfdrive/assets/training/step8.jpg b/selfdrive/assets/training/step8.jpg new file mode 100644 index 000000000..7e07ee8ac Binary files /dev/null and b/selfdrive/assets/training/step8.jpg differ diff --git a/selfdrive/assets/training/step9.jpg b/selfdrive/assets/training/step9.jpg new file mode 100644 index 000000000..7e8114f8a Binary files /dev/null and b/selfdrive/assets/training/step9.jpg differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 27e8b587f..c3645ed8d 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -125,9 +125,9 @@ def listDataDirectory(): @dispatcher.add_method def reboot(): - thermal_sock = messaging.sub_sock("thermal", timeout=1000) - ret = messaging.recv_one(thermal_sock) - if ret is None or ret.thermal.started: + sock = messaging.sub_sock("deviceState", timeout=1000) + ret = messaging.recv_one(sock) + if ret is None or ret.deviceState.started: raise Exception("Reboot unavailable") def do_reboot(): @@ -280,6 +280,8 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event): cloudlog.exception("athenad.ws_proxy_send.exception") end_event.set() + signal_sock.close() + def ws_recv(ws, end_event): while not end_event.is_set(): diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 49120d972..cd8bc5820 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -14,9 +14,9 @@ ATHENA_MGR_PID_PARAM = "AthenadPid" def main(): params = Params() dongle_id = params.get("DongleId").decode('utf-8') - cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=True) + cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty) crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.bind_extra(version=version, dirty=dirty) crash.install() try: diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 71e450ba4..5b82b023b 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -1,7 +1,6 @@ #include #include #include -#include #include #include #include @@ -37,14 +36,11 @@ #define SATURATE_IL 1600 #define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a') -Panda * panda = NULL; +Panda * panda = nullptr; std::atomic safety_setter_thread_running(false); -volatile sig_atomic_t do_exit = 0; -bool spoofing_started = false; -bool fake_send = false; -bool connected_once = false; -bool ignition = false; +std::atomic ignition(false); +ExitHandler do_exit; struct tm get_time(){ time_t rawtime; time(&rawtime); @@ -67,7 +63,7 @@ void safety_setter_thread() { panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327); // switch to SILENT when CarVin param is read - while (1) { + while (true) { if (do_exit || !panda->connected){ safety_setter_thread_running = false; return; @@ -81,7 +77,7 @@ void safety_setter_thread() { LOGW("got CarVin %s", str_vin.c_str()); break; } - usleep(100*1000); + util::sleep_for(100); } // VIN query done, stop listening to OBDII @@ -89,7 +85,7 @@ void safety_setter_thread() { std::vector params; LOGW("waiting for params to set safety model"); - while (1) { + while (true) { if (do_exit || !panda->connected){ safety_setter_thread_running = false; return; @@ -97,7 +93,7 @@ void safety_setter_thread() { params = Params().read_db_bytes("CarParams"); if (params.size() > 0) break; - usleep(100*1000); + util::sleep_for(100); } LOGW("got %d bytes CarParams", params.size()); @@ -121,9 +117,12 @@ void safety_setter_thread() { bool usb_connect() { + static bool connected_once = false; + + std::unique_ptr tmp_panda; try { - assert(panda == NULL); - panda = new Panda(); + assert(panda == nullptr); + tmp_panda = std::make_unique(); } catch (std::exception &e) { return false; } @@ -131,15 +130,15 @@ bool usb_connect() { Params params = Params(); if (getenv("BOARDD_LOOPBACK")) { - panda->set_loopback(true); + tmp_panda->set_loopback(true); } - const char *fw_sig_buf = panda->get_firmware_version(); - if (fw_sig_buf){ - params.write_db_value("PandaFirmware", fw_sig_buf, 128); + if (auto fw_sig = tmp_panda->get_firmware_version(); fw_sig) { + params.write_db_value("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size()); // Convert to hex for offroad char fw_sig_hex_buf[16] = {0}; + const uint8_t *fw_sig_buf = fw_sig->data(); for (size_t i = 0; i < 8; i++){ fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4); fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF); @@ -147,31 +146,24 @@ bool usb_connect() { params.write_db_value("PandaFirmwareHex", fw_sig_hex_buf, 16); LOGW("fw signature: %.*s", 16, fw_sig_hex_buf); - - delete[] fw_sig_buf; } else { return false; } // get panda serial - const char *serial_buf = panda->get_serial(); - if (serial_buf) { - size_t serial_sz = strnlen(serial_buf, 16); - - params.write_db_value("PandaDongleId", serial_buf, serial_sz); - LOGW("panda serial: %.*s", serial_sz, serial_buf); - - delete[] serial_buf; + if (auto serial = tmp_panda->get_serial(); serial) { + params.write_db_value("PandaDongleId", serial->c_str(), serial->length()); + LOGW("panda serial: %s", serial->c_str()); } else { return false; } // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton #ifndef __x86_64__ if (!connected_once) { - panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); + tmp_panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP); } #endif - if (panda->has_rtc){ + if (tmp_panda->has_rtc){ struct tm sys_time = get_time(); - struct tm rtc_time = panda->get_rtc(); + struct tm rtc_time = tmp_panda->get_rtc(); if (!time_valid(sys_time) && time_valid(rtc_time)) { LOGE("System time wrong, setting from RTC"); @@ -183,25 +175,28 @@ bool usb_connect() { } connected_once = true; + panda = tmp_panda.release(); return true; } // must be called before threads or with mutex -void usb_retry_connect() { +static bool usb_retry_connect() { LOGW("attempting to connect"); - while (!usb_connect()) { usleep(100*1000); } - LOGW("connected to board"); + while (!do_exit && !usb_connect()) { util::sleep_for(100); } + if (panda) { + LOGW("connected to board"); + } + return !do_exit; } void can_recv(PubMaster &pm) { - // create message - MessageBuilder msg; - auto event = msg.initEvent(); - panda->can_receive(event); - pm.send("can", msg); + kj::Array can_data; + panda->can_receive(can_data); + auto bytes = can_data.asBytes(); + pm.send("can", bytes.begin(), bytes.size()); } -void can_send_thread() { +void can_send_thread(bool fake_send) { LOGD("start send thread"); Context * context = Context::create(); @@ -255,9 +250,8 @@ void can_recv_thread() { uint64_t cur_time = nanos_since_boot(); int64_t remaining = next_frame_time - cur_time; - if (remaining > 0){ - useconds_t sleep = remaining / 1000; - usleep(sleep); + if (remaining > 0) { + std::this_thread::sleep_for(std::chrono::nanoseconds(remaining)); } else { if (ignition){ LOGW("missed cycles (%d) %lld", (int)-1*remaining/dt, remaining); @@ -269,41 +263,38 @@ void can_recv_thread() { } } -void can_health_thread() { - LOGD("start health thread"); - PubMaster pm({"health"}); +void panda_state_thread(bool spoofing_started) { + LOGD("start panda state thread"); + PubMaster pm({"pandaState"}); uint32_t no_ignition_cnt = 0; bool ignition_last = false; Params params = Params(); - // Broadcast empty health message when panda is not yet connected - while (!panda){ + // Broadcast empty pandaState message when panda is not yet connected + while (!do_exit && !panda) { MessageBuilder msg; - auto healthData = msg.initEvent().initHealth(); + auto pandaState = msg.initEvent().initPandaState(); - healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); - pm.send("health", msg); - usleep(500*1000); + pandaState.setPandaType(cereal::PandaState::PandaType::UNKNOWN); + pm.send("pandaState", msg); + util::sleep_for(500); } // run at 2hz while (!do_exit && panda->connected) { - MessageBuilder msg; - auto healthData = msg.initEvent().initHealth(); - - health_t health = panda->get_health(); + health_t pandaState = panda->get_state(); if (spoofing_started) { - health.ignition_line = 1; + pandaState.ignition_line = 1; } // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node - if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { + if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } - ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); + ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); if (ignition) { no_ignition_cnt = 0; @@ -313,12 +304,12 @@ void can_health_thread() { #ifndef __x86_64__ bool power_save_desired = !ignition; - if (health.power_save_enabled != power_save_desired){ + if (pandaState.power_save_enabled != power_save_desired){ panda->set_power_saving(power_save_desired); } // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect - if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #endif @@ -350,53 +341,62 @@ void can_health_thread() { ignition_last = ignition; uint16_t fan_speed_rpm = panda->get_fan_speed(); - // set fields - healthData.setUptime(health.uptime); - healthData.setVoltage(health.voltage); - healthData.setCurrent(health.current); - healthData.setIgnitionLine(health.ignition_line); - healthData.setIgnitionCan(health.ignition_can); - healthData.setControlsAllowed(health.controls_allowed); - healthData.setGasInterceptorDetected(health.gas_interceptor_detected); - healthData.setHasGps(panda->is_pigeon); - healthData.setCanRxErrs(health.can_rx_errs); - healthData.setCanSendErrs(health.can_send_errs); - healthData.setCanFwdErrs(health.can_fwd_errs); - healthData.setGmlanSendErrs(health.gmlan_send_errs); - healthData.setHwType(panda->hw_type); - healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode)); - healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model)); - healthData.setFanSpeedRpm(fan_speed_rpm); - healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status)); - healthData.setPowerSaveEnabled((bool)(health.power_save_enabled)); + // build msg + MessageBuilder msg; + auto ps = msg.initEvent().initPandaState(); + ps.setUptime(pandaState.uptime); + +#ifdef QCOM2 + ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input"))); + ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input"))); +#else + ps.setVoltage(pandaState.voltage); + ps.setCurrent(pandaState.current); +#endif + + ps.setIgnitionLine(pandaState.ignition_line); + ps.setIgnitionCan(pandaState.ignition_can); + ps.setControlsAllowed(pandaState.controls_allowed); + ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); + ps.setHasGps(panda->is_pigeon); + ps.setCanRxErrs(pandaState.can_rx_errs); + ps.setCanSendErrs(pandaState.can_send_errs); + ps.setCanFwdErrs(pandaState.can_fwd_errs); + ps.setGmlanSendErrs(pandaState.gmlan_send_errs); + ps.setPandaType(panda->hw_type); + ps.setUsbPowerMode(cereal::PandaState::UsbPowerMode(pandaState.usb_power_mode)); + ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); + ps.setFanSpeedRpm(fan_speed_rpm); + ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); + ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); // Convert faults bitset to capnp list - std::bitset fault_bits(health.faults); - auto faults = healthData.initFaults(fault_bits.count()); + std::bitset fault_bits(pandaState.faults); + auto faults = ps.initFaults(fault_bits.count()); size_t i = 0; - for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_TIM9); f++){ + for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); + f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TIM9); f++){ if (fault_bits.test(f)) { - faults.set(i, cereal::HealthData::FaultType(f)); + faults.set(i, cereal::PandaState::FaultType(f)); i++; } } - pm.send("health", msg); + pm.send("pandaState", msg); panda->send_heartbeat(); - usleep(500*1000); + util::sleep_for(500); } } void hardware_control_thread() { LOGD("start hardware control thread"); - SubMaster sm({"thermal", "frontFrame"}); + SubMaster sm({"deviceState", "driverCameraState"}); uint64_t last_front_frame_t = 0; uint16_t prev_fan_speed = 999; uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) bool prev_charging_disabled = false; #endif unsigned int cnt = 0; @@ -405,16 +405,16 @@ void hardware_control_thread() { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? -#ifdef QCOM - if (sm.updated("thermal")){ +#if defined(QCOM) || defined(QCOM2) + if (sm.updated("deviceState")){ // Charging mode - bool charging_disabled = sm["thermal"].getThermal().getChargingDisabled(); + bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled(); if (charging_disabled != prev_charging_disabled){ if (charging_disabled){ - panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT); + panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CLIENT); LOGW("TURN OFF CHARGING!\n"); } else { - panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP); + panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP); LOGW("TURN ON CHARGING!\n"); } prev_charging_disabled = charging_disabled; @@ -423,18 +423,18 @@ void hardware_control_thread() { #endif // Other pandas don't have fan/IR to control - if (panda->hw_type != cereal::HealthData::HwType::UNO && panda->hw_type != cereal::HealthData::HwType::DOS) continue; - if (sm.updated("thermal")){ + if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue; + if (sm.updated("deviceState")){ // Fan speed - uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeed(); + uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); if (fan_speed != prev_fan_speed || cnt % 100 == 0){ panda->set_fan_speed(fan_speed); prev_fan_speed = fan_speed; } } - if (sm.updated("frontFrame")){ - auto event = sm["frontFrame"]; - int cur_integ_lines = event.getFrontFrame().getIntegLines(); + if (sm.updated("driverCameraState")){ + auto event = sm["driverCameraState"]; + int cur_integ_lines = event.getDriverCameraState().getIntegLines(); last_front_frame_t = event.getLogMonoTime(); if (cur_integ_lines <= CUTOFF_IL) { @@ -459,18 +459,15 @@ void hardware_control_thread() { } } -static void pigeon_publish_raw(PubMaster &pm, std::string dat) { +static void pigeon_publish_raw(PubMaster &pm, const std::string &dat) { // create message MessageBuilder msg; - auto ublox_raw = msg.initEvent().initUbloxRaw(dat.length()); - memcpy(ublox_raw.begin(), dat.data(), dat.length()); - + msg.initEvent().setUbloxRaw(capnp::Data::Reader((uint8_t*)dat.data(), dat.length())); pm.send("ubloxRaw", msg); } - void pigeon_thread() { - if (!panda->is_pigeon){ return; }; + if (!panda->is_pigeon) { return; }; // ubloxRaw = 8042 PubMaster pm({"ubloxRaw"}); @@ -483,12 +480,13 @@ void pigeon_thread() { #endif while (!do_exit && panda->connected) { + bool need_reset = false; std::string recv = pigeon->receive(); if (recv.length() > 0) { if (recv[0] == (char)0x00){ if (ignition) { LOGW("received invalid ublox message while onroad, resetting panda GPS"); - pigeon->init(); + need_reset = true; } } else { pigeon_publish_raw(pm, recv); @@ -497,14 +495,14 @@ void pigeon_thread() { // init pigeon on rising ignition edge // since it was turned off in low power mode - if(ignition && !ignition_last) { + if((ignition && !ignition_last) || need_reset) { pigeon->init(); } ignition_last = ignition; // 10ms - 100 Hz - usleep(10*1000); + util::sleep_for(10); } delete pigeon; @@ -521,32 +519,23 @@ int main() { err = set_core_affinity(3); LOG("set affinity returns %d", err); - // check the environment - if (getenv("STARTED")) { - spoofing_started = true; - } - - if (getenv("FAKESEND")) { - fake_send = true; - } - panda_set_power(true); while (!do_exit){ std::vector threads; - threads.push_back(std::thread(can_health_thread)); + threads.push_back(std::thread(panda_state_thread, getenv("STARTED") != nullptr)); // connect to the board - usb_retry_connect(); - - threads.push_back(std::thread(can_send_thread)); - threads.push_back(std::thread(can_recv_thread)); - threads.push_back(std::thread(hardware_control_thread)); - threads.push_back(std::thread(pigeon_thread)); + if (usb_retry_connect()) { + threads.push_back(std::thread(can_send_thread, getenv("FAKESEND") != nullptr)); + threads.push_back(std::thread(can_recv_thread)); + threads.push_back(std::thread(hardware_control_thread)); + threads.push_back(std::thread(pigeon_thread)); + } for (auto &t : threads) t.join(); delete panda; - panda = NULL; + panda = nullptr; } } diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc index 2fa3f5b9b..bbbceee6e 100644 --- a/selfdrive/boardd/can_list_to_can_capnp.cc +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -22,8 +22,10 @@ void can_list_to_can_capnp_cpp(const std::vector &can_list, std::stri c.setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size())); c.setSrc(it->src); } - auto bytes = msg.toBytes(); - out.append((const char *)bytes.begin(), bytes.size()); + const uint64_t msg_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word); + out.resize(msg_size); + kj::ArrayOutputStream output_stream(kj::ArrayPtr((unsigned char *)out.data(), msg_size)); + capnp::writeMessage(output_stream, msg); } } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index d89e302fa..84697965f 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -1,50 +1,35 @@ #include #include #include - +#include #include #include "common/swaglog.h" #include "common/gpio.h" - +#include "common/util.h" +#include "messaging.hpp" #include "panda.h" -#ifdef QCOM2 -bool is_legacy_panda_reset() { - FILE *file = fopen("/persist/LEGACY_PANDA_RESET", "r"); - if(file) { - fclose(file); - } - return (file != NULL); -} -#endif - void panda_set_power(bool power){ #ifdef QCOM2 int err = 0; - bool is_legacy = is_legacy_panda_reset(); err += gpio_init(GPIO_STM_RST_N, true); err += gpio_init(GPIO_STM_BOOT0, true); - err += gpio_set(GPIO_STM_RST_N, is_legacy ? false : true); + err += gpio_set(GPIO_STM_RST_N, true); err += gpio_set(GPIO_STM_BOOT0, false); - usleep(100*1000); // 100 ms + util::sleep_for(100); // 100 ms - err += gpio_set(GPIO_STM_RST_N, is_legacy ? power : (!power)); + err += gpio_set(GPIO_STM_RST_N, !power); assert(err == 0); #endif } Panda::Panda(){ - int err; - - err = pthread_mutex_init(&usb_lock, NULL); - if (err != 0) { goto fail; } - // init libusb - err = libusb_init(&ctx); + int err = libusb_init(&ctx); if (err != 0) { goto fail; } #if LIBUSB_API_VERSION >= 0x01000106 @@ -68,12 +53,12 @@ Panda::Panda(){ hw_type = get_hw_type(); is_pigeon = - (hw_type == cereal::HealthData::HwType::GREY_PANDA) || - (hw_type == cereal::HealthData::HwType::BLACK_PANDA) || - (hw_type == cereal::HealthData::HwType::UNO) || - (hw_type == cereal::HealthData::HwType::DOS); - has_rtc = (hw_type == cereal::HealthData::HwType::UNO) || - (hw_type == cereal::HealthData::HwType::DOS); + (hw_type == cereal::PandaState::PandaType::GREY_PANDA) || + (hw_type == cereal::PandaState::PandaType::BLACK_PANDA) || + (hw_type == cereal::PandaState::PandaType::UNO) || + (hw_type == cereal::PandaState::PandaType::DOS); + has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || + (hw_type == cereal::PandaState::PandaType::DOS); return; @@ -83,10 +68,9 @@ fail: } Panda::~Panda(){ - pthread_mutex_lock(&usb_lock); + std::lock_guard lk(usb_lock); cleanup(); connected = false; - pthread_mutex_unlock(&usb_lock); } void Panda::cleanup(){ @@ -117,14 +101,12 @@ int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigne return LIBUSB_ERROR_NO_DEVICE; } - pthread_mutex_lock(&usb_lock); + std::lock_guard lk(usb_lock); do { err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); if (err < 0) handle_usb_issue(err, __func__); } while (err < 0 && connected); - pthread_mutex_unlock(&usb_lock); - return err; } @@ -132,12 +114,15 @@ int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int err; const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - pthread_mutex_lock(&usb_lock); + if (!connected){ + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(usb_lock); do { err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); if (err < 0) handle_usb_issue(err, __func__); } while (err < 0 && connected); - pthread_mutex_unlock(&usb_lock); return err; } @@ -150,7 +135,7 @@ int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int lengt return 0; } - pthread_mutex_lock(&usb_lock); + std::lock_guard lk(usb_lock); do { // Try sending can messages. If the receive buffer on the panda is full it will NAK // and libusb will try again. After 5ms, it will time out. We will drop the messages. @@ -164,7 +149,6 @@ int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int lengt } } while(err != 0 && connected); - pthread_mutex_unlock(&usb_lock); return transferred; } @@ -176,7 +160,7 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length return 0; } - pthread_mutex_lock(&usb_lock); + std::lock_guard lk(usb_lock); do { err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); @@ -191,8 +175,6 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length } while(err != 0 && connected); - pthread_mutex_unlock(&usb_lock); - return transferred; } @@ -204,11 +186,11 @@ void Panda::set_unsafe_mode(uint16_t unsafe_mode) { usb_write(0xdf, unsafe_mode, 0); } -cereal::HealthData::HwType Panda::get_hw_type() { +cereal::PandaState::PandaType Panda::get_hw_type() { unsigned char hw_query[1] = {0}; usb_read(0xc1, 0, 0, hw_query, 1); - return (cereal::HealthData::HwType)(hw_query[0]); + return (cereal::PandaState::PandaType)(hw_query[0]); } void Panda::set_rtc(struct tm sys_time){ @@ -260,7 +242,7 @@ void Panda::set_ir_pwr(uint16_t ir_pwr) { usb_write(0xb0, ir_pwr, 0); } -health_t Panda::get_health(){ +health_t Panda::get_state(){ health_t health {0}; usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); return health; @@ -270,38 +252,24 @@ void Panda::set_loopback(bool loopback){ usb_write(0xe5, loopback, 0); } -const char* Panda::get_firmware_version(){ - const char* fw_sig_buf = new char[128](); - - int read_1 = usb_read(0xd3, 0, 0, (unsigned char*)fw_sig_buf, 64); - int read_2 = usb_read(0xd4, 0, 0, (unsigned char*)fw_sig_buf + 64, 64); - - if ((read_1 == 64) && (read_2 == 64)) { - return fw_sig_buf; - } - - delete[] fw_sig_buf; - return NULL; +std::optional> Panda::get_firmware_version(){ + std::vector fw_sig_buf(128); + int read_1 = usb_read(0xd3, 0, 0, &fw_sig_buf[0], 64); + int read_2 = usb_read(0xd4, 0, 0, &fw_sig_buf[64], 64); + return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt; } -const char* Panda::get_serial(){ - const char* serial_buf = new char[16](); - - int err = usb_read(0xd0, 0, 0, (unsigned char*)serial_buf, 16); - - if (err >= 0) { - return serial_buf; - } - - delete[] serial_buf; - return NULL; +std::optional Panda::get_serial() { + char serial_buf[17] = {'\0'}; + int err = usb_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); + return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; } void Panda::set_power_saving(bool power_saving){ usb_write(0xe7, power_saving, 0); } -void Panda::set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode){ +void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode){ usb_write(0xe6, (uint16_t)power_mode, 0); } @@ -310,9 +278,10 @@ void Panda::send_heartbeat(){ } void Panda::can_send(capnp::List::Reader can_data_list){ - int msg_count = can_data_list.size(); + static std::vector send; + const int msg_count = can_data_list.size(); - uint32_t *send = new uint32_t[msg_count*0x10](); + send.resize(msg_count*0x10); for (int i = 0; i < msg_count; i++) { auto cmsg = can_data_list[i]; @@ -327,12 +296,10 @@ void Panda::can_send(capnp::List::Reader can_data_list){ memcpy(&send[i*4+2], can_data.begin(), can_data.size()); } - usb_bulk_write(3, (unsigned char*)send, msg_count*0x10, 5); - - delete[] send; + usb_bulk_write(3, (unsigned char*)send.data(), send.size(), 5); } -int Panda::can_receive(cereal::Event::Builder &event){ +int Panda::can_receive(kj::Array& out_buf) { uint32_t data[RECV_SIZE/4]; int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE); @@ -344,7 +311,8 @@ int Panda::can_receive(cereal::Event::Builder &event){ } size_t num_msg = recv / 0x10; - auto canData = event.initCan(num_msg); + MessageBuilder msg; + auto canData = msg.initEvent().initCan(num_msg); // populate message for (int i = 0; i < num_msg; i++) { @@ -361,6 +329,6 @@ int Panda::can_receive(cereal::Event::Builder &event){ canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); canData[i].setSrc((data[i*4+1] >> 4) & 0xff); } - + out_buf = capnp::messageToFlatArray(msg); return recv; } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 119c123b1..26bd26360 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -3,6 +3,9 @@ #include #include #include +#include +#include +#include #include @@ -41,7 +44,7 @@ class Panda { private: libusb_context *ctx = NULL; libusb_device_handle *dev_handle = NULL; - pthread_mutex_t usb_lock; + std::mutex usb_lock; void handle_usb_issue(int err, const char func[]); void cleanup(); @@ -49,8 +52,8 @@ class Panda { Panda(); ~Panda(); - bool connected = true; - cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; + std::atomic connected = true; + cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool is_pigeon = false; bool has_rtc = false; @@ -61,7 +64,7 @@ class Panda { int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); // Panda functionality - cereal::HealthData::HwType get_hw_type(); + cereal::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0); void set_unsafe_mode(uint16_t unsafe_mode); void set_rtc(struct tm sys_time); @@ -69,14 +72,13 @@ class Panda { void set_fan_speed(uint16_t fan_speed); uint16_t get_fan_speed(); void set_ir_pwr(uint16_t ir_pwr); - health_t get_health(); + health_t get_state(); void set_loopback(bool loopback); - const char* get_firmware_version(); - const char* get_serial(); + std::optional> get_firmware_version(); + std::optional get_serial(); void set_power_saving(bool power_saving); - void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode); + void set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode); void send_heartbeat(); void can_send(capnp::List::Reader can_data_list); - int can_receive(cereal::Event::Builder &event); - + int can_receive(kj::Array& out_buf); }; diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 4ec7ebf8c..8cd079040 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -6,6 +6,7 @@ #include "common/swaglog.h" #include "common/gpio.h" +#include "common/util.h" #include "pigeon.h" @@ -32,27 +33,27 @@ Pigeon * Pigeon::connect(const char * tty){ } void Pigeon::init() { - usleep(1000*1000); + util::sleep_for(1000); LOGW("panda GPS start"); // power off pigeon - set_power(0); - usleep(100*1000); + set_power(false); + util::sleep_for(100); // 9600 baud at init set_baud(9600); // power on pigeon - set_power(1); - usleep(500*1000); + set_power(true); + util::sleep_for(500); // baud rate upping send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A"s); - usleep(100*1000); + util::sleep_for(100); // set baud rate to 460800 set_baud(460800); - usleep(100*1000); + util::sleep_for(100); // init from ubloxd // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py @@ -75,6 +76,7 @@ void Pigeon::init() { send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s); + send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); LOGW("panda GPS on"); } @@ -88,7 +90,7 @@ void PandaPigeon::set_baud(int baud) { panda->usb_write(0xe4, 1, baud/300); } -void PandaPigeon::send(std::string s) { +void PandaPigeon::send(const std::string &s) { int len = s.length(); const char * dat = s.data(); @@ -104,11 +106,11 @@ void PandaPigeon::send(std::string s) { std::string PandaPigeon::receive() { std::string r; - - while (true){ - unsigned char dat[0x40]; + r.reserve(0x1000 + 0x40); + unsigned char dat[0x40]; + while (r.length() < 0x1000){ int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat)); - if (len <= 0 || r.length() > 0x1000) break; + if (len <= 0) break; r.append((char*)dat, len); } @@ -131,8 +133,10 @@ void TTYPigeon::connect(const char * tty) { if (pigeon_tty_fd < 0){ handle_tty_issue(errno, __func__); assert(pigeon_tty_fd >= 0); + } - assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0); + int err = tcgetattr(pigeon_tty_fd, &pigeon_tty); + assert(err == 0); // configure tty pigeon_tty.c_cflag &= ~PARENB; // disable parity @@ -151,7 +155,8 @@ void TTYPigeon::connect(const char * tty) { pigeon_tty.c_cc[VMIN] = 0; // min amount of characters returned pigeon_tty.c_cc[VTIME] = 0; // max blocking time in s/10 (0=inf) - assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0); + err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); + assert(err == 0); } void TTYPigeon::set_baud(int baud){ @@ -168,22 +173,25 @@ void TTYPigeon::set_baud(int baud){ } // make sure everything is tx'ed before changing baud - assert(tcdrain(pigeon_tty_fd) == 0); + int err = tcdrain(pigeon_tty_fd); + assert(err == 0); // change baud - assert(tcgetattr(pigeon_tty_fd, &pigeon_tty) == 0); - assert(cfsetspeed(&pigeon_tty, baud_const) == 0); - assert(tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty) == 0); + err = tcgetattr(pigeon_tty_fd, &pigeon_tty); + assert(err == 0); + err = cfsetspeed(&pigeon_tty, baud_const); + assert(err == 0); + err = tcsetattr(pigeon_tty_fd, TCSANOW, &pigeon_tty); + assert(err == 0); // flush - assert(tcflush(pigeon_tty_fd, TCIOFLUSH) == 0); + err = tcflush(pigeon_tty_fd, TCIOFLUSH); + assert(err == 0); } -void TTYPigeon::send(std::string s) { - int len = s.length(); - const char * dat = s.data(); +void TTYPigeon::send(const std::string &s) { + int err = write(pigeon_tty_fd, s.data(), s.length()); - int err = write(pigeon_tty_fd, dat, len); if(err < 0) { handle_tty_issue(err, __func__); } err = tcdrain(pigeon_tty_fd); if(err < 0) { handle_tty_issue(err, __func__); } @@ -191,13 +199,13 @@ void TTYPigeon::send(std::string s) { std::string TTYPigeon::receive() { std::string r; - - while (true){ - char dat[0x40]; + r.reserve(0x1000 + 0x40); + char dat[0x40]; + while (r.length() < 0x1000){ int len = read(pigeon_tty_fd, dat, sizeof(dat)); if(len < 0) { handle_tty_issue(len, __func__); - } else if (len == 0 || r.length() > 0x1000){ + } else if (len == 0){ break; } else { r.append(dat, len); diff --git a/selfdrive/boardd/pigeon.h b/selfdrive/boardd/pigeon.h index 667ac7061..927d74058 100644 --- a/selfdrive/boardd/pigeon.h +++ b/selfdrive/boardd/pigeon.h @@ -13,7 +13,7 @@ class Pigeon { void init(); virtual void set_baud(int baud) = 0; - virtual void send(std::string s) = 0; + virtual void send(const std::string &s) = 0; virtual std::string receive() = 0; virtual void set_power(bool power) = 0; }; @@ -24,7 +24,7 @@ public: ~PandaPigeon(); void connect(Panda * p); void set_baud(int baud); - void send(std::string s); + void send(const std::string &s); std::string receive(); void set_power(bool power); }; @@ -37,7 +37,7 @@ public: ~TTYPigeon(); void connect(const char* tty); void set_baud(int baud); - void send(std::string s); + void send(const std::string &s); std::string receive(); void set_power(bool power); }; diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index feb55a59e..9fb5da90c 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -28,9 +28,6 @@ else: env = env.Clone() env['FRAMEWORKS'] = ['OpenCL', 'OpenGL'] -env.SharedLibrary('snapshot/visionipc', - ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) - env.Program('camerad', [ 'main.cc', 'cameras/camera_common.cc', diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 273103aa1..df472d5b4 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -1,6 +1,6 @@ #include +#include #include -#include #include #include @@ -24,22 +24,16 @@ #include "common/util.h" #include "imgproc/utils.h" -const int env_xmin = getenv("XMIN") ? atoi(getenv("XMIN")) : 0; -const int env_xmax = getenv("XMAX") ? atoi(getenv("XMAX")) : -1; -const int env_ymin = getenv("YMIN") ? atoi(getenv("YMIN")) : 0; -const int env_ymax = getenv("YMAX") ? atoi(getenv("YMAX")) : -1; -const int env_scale = getenv("SCALE") ? atoi(getenv("SCALE")) : 1; - -static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b) { +static cl_program build_debayer_program(cl_device_id device_id, cl_context context, const CameraInfo *ci, const CameraBuf *b, const CameraState *s) { char args[4096]; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " - "-DBAYER_FLIP=%d -DHDR=%d", + "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d", ci->frame_width, ci->frame_height, ci->frame_stride, b->rgb_width, b->rgb_height, b->rgb_stride, - ci->bayer_flip, ci->hdr); + ci->bayer_flip, ci->hdr, s->camera_num); #ifdef QCOM2 return cl_program_from_file(context, device_id, "cameras/real_debayer.cl", args); #else @@ -47,17 +41,24 @@ static cl_program build_debayer_program(cl_device_id device_id, cl_context conte #endif } -void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, int frame_cnt, - const char *name, release_cb relase_callback) { +void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType rgb_type, VisionStreamType yuv_type, release_cb release_callback) { + vipc_server = v; + this->rgb_type = rgb_type; + this->yuv_type = yuv_type; + this->release_callback = release_callback; + const CameraInfo *ci = &s->ci; camera_state = s; frame_buf_count = frame_cnt; - frame_size = ci->frame_height * ci->frame_stride; + // RAW frame + const int frame_size = ci->frame_height * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); camera_bufs_metadata = std::make_unique(frame_buf_count); + for (int i = 0; i < frame_buf_count; i++) { - camera_bufs[i] = visionbuf_allocate_cl(frame_size, device_id, context); + camera_bufs[i].allocate(frame_size); + camera_bufs[i].init_cl(device_id, context); } rgb_width = ci->frame_width; @@ -72,44 +73,25 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, #else float db_s = 1.0; #endif + const mat3 transform = (mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + }}; + yuv_transform = ci->bayer ? transform_scale_buffer(transform, db_s) : transform; + + vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height); + rgb_stride = vipc_server->get_buffer(rgb_type)->stride; + + vipc_server->create_buffers(yuv_type, YUV_COUNT, false, rgb_width, rgb_height); if (ci->bayer) { - yuv_transform = transform_scale_buffer(s->transform, db_s); - } else { - yuv_transform = s->transform; - } - - for (int i = 0; i < UI_BUF_COUNT; i++) { - VisionImg img = visionimg_alloc_rgb24(device_id, context, rgb_width, rgb_height, &rgb_bufs[i]); - if (i == 0) { - rgb_stride = img.stride; - } - } - tbuffer_init(&ui_tb, UI_BUF_COUNT, name); - tbuffer_init2(&camera_tb, frame_buf_count, "frame", relase_callback, s); - - // yuv back for recording and orbd - pool_init(&yuv_pool, YUV_COUNT); - yuv_tb = pool_get_tbuffer(&yuv_pool); - - yuv_width = rgb_width; - yuv_height = rgb_height; - yuv_buf_size = rgb_width * rgb_height * 3 / 2; - - for (int i = 0; i < YUV_COUNT; i++) { - yuv_ion[i] = visionbuf_allocate_cl(yuv_buf_size, device_id, context); - yuv_bufs[i].y = (uint8_t *)yuv_ion[i].addr; - yuv_bufs[i].u = yuv_bufs[i].y + (yuv_width * yuv_height); - yuv_bufs[i].v = yuv_bufs[i].u + (yuv_width / 2 * yuv_height / 2); - } - - if (ci->bayer) { - cl_program prg_debayer = build_debayer_program(device_id, context, ci, this); + cl_program prg_debayer = build_debayer_program(device_id, context, ci, this, s); krnl_debayer = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); CL_CHECK(clReleaseProgram(prg_debayer)); } - rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, yuv_width, yuv_height, rgb_stride); + rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, rgb_width, rgb_height, rgb_stride); #ifdef __APPLE__ q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); @@ -121,14 +103,9 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, CameraBuf::~CameraBuf() { for (int i = 0; i < frame_buf_count; i++) { - visionbuf_free(&camera_bufs[i]); - } - for (int i = 0; i < UI_BUF_COUNT; i++) { - visionbuf_free(&rgb_bufs[i]); - } - for (int i = 0; i < YUV_COUNT; i++) { - visionbuf_free(&yuv_ion[i]); + camera_bufs[i].free(); } + rgb_to_yuv_destroy(&rgb_to_yuv_state); if (krnl_debayer) { @@ -138,32 +115,31 @@ CameraBuf::~CameraBuf() { } bool CameraBuf::acquire() { - const int buf_idx = tbuffer_acquire(&camera_tb); - if (buf_idx < 0) { - return false; - } - const FrameMetadata &frame_data = camera_bufs_metadata[buf_idx]; + if (!safe_queue.try_pop(cur_buf_idx, 1)) return false; + + const FrameMetadata &frame_data = camera_bufs_metadata[cur_buf_idx]; if (frame_data.frame_id == -1) { LOGE("no frame data? wtf"); - tbuffer_release(&camera_tb, buf_idx); + release(); return false; } cur_frame_data = frame_data; - cur_rgb_idx = tbuffer_select(&ui_tb); - cur_rgb_buf = &rgb_bufs[cur_rgb_idx]; + cur_rgb_buf = vipc_server->get_buffer(rgb_type); cl_event debayer_event; - cl_mem camrabuf_cl = camera_bufs[buf_idx].buf_cl; + cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl; if (camera_state->ci.bayer) { CL_CHECK(clSetKernelArg(krnl_debayer, 0, sizeof(cl_mem), &camrabuf_cl)); CL_CHECK(clSetKernelArg(krnl_debayer, 1, sizeof(cl_mem), &cur_rgb_buf->buf_cl)); #ifdef QCOM2 - constexpr int localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(float); + constexpr int localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(short int); const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)}; const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE}; CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0)); + int ggain = camera_state->analog_gain + 4*camera_state->dc_gain_enabled; + CL_CHECK(clSetKernelArg(krnl_debayer, 3, sizeof(int), &ggain)); CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize, 0, 0, &debayer_event)); #else @@ -177,7 +153,6 @@ bool CameraBuf::acquire() { &debayer_work_size, NULL, 0, 0, &debayer_event)); #endif } else { - assert(cur_rgb_buf->len >= frame_size); assert(rgb_stride == camera_state->ci.frame_stride); CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &debayer_event)); @@ -186,38 +161,37 @@ bool CameraBuf::acquire() { clWaitForEvents(1, &debayer_event); CL_CHECK(clReleaseEvent(debayer_event)); - tbuffer_release(&camera_tb, buf_idx); - visionbuf_sync(cur_rgb_buf, VISIONBUF_SYNC_FROM_DEVICE); + cur_yuv_buf = vipc_server->get_buffer(yuv_type); + yuv_metas[cur_yuv_buf->idx] = frame_data; + rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl); - cur_yuv_idx = pool_select(&yuv_pool); - yuv_metas[cur_yuv_idx] = frame_data; - rgb_to_yuv_queue(&rgb_to_yuv_state, q, cur_rgb_buf->buf_cl, yuv_ion[cur_yuv_idx].buf_cl); - visionbuf_sync(&yuv_ion[cur_yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - - // keep another reference around till were done processing - pool_acquire(&yuv_pool, cur_yuv_idx); - pool_push(&yuv_pool, cur_yuv_idx); - - tbuffer_dispatch(&ui_tb, cur_rgb_idx); + VisionIpcBufExtra extra = { + frame_data.frame_id, + frame_data.timestamp_sof, + frame_data.timestamp_eof, + }; + vipc_server->send(cur_rgb_buf, &extra); + vipc_server->send(cur_yuv_buf, &extra); return true; } void CameraBuf::release() { - pool_release(&yuv_pool, cur_yuv_idx); + if (release_callback){ + release_callback((void*)camera_state, cur_buf_idx); + } } -void CameraBuf::stop() { - tbuffer_stop(&ui_tb); - tbuffer_stop(&camera_tb); - pool_stop(&yuv_pool); +void CameraBuf::queue(size_t buf_idx) { + safe_queue.push(buf_idx); } // common functions -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt) { +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data) { framed.setFrameId(frame_data.frame_id); framed.setTimestampEof(frame_data.timestamp_eof); + framed.setTimestampSof(frame_data.timestamp_sof); framed.setFrameLength(frame_data.frame_length); framed.setIntegLines(frame_data.integ_lines); framed.setGlobalGain(frame_data.global_gain); @@ -228,30 +202,33 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setGainFrac(frame_data.gain_frac); } -void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride) { - if (dat != nullptr) { - int scale = env_scale; - int x_min = env_xmin; int y_min = env_ymin; int x_max = w-1; int y_max = h-1; - if (env_xmax != -1) x_max = env_xmax; - if (env_ymax != -1) y_max = env_ymax; - int new_width = (x_max - x_min + 1) / scale; - int new_height = (y_max - y_min + 1) / scale; - uint8_t *resized_dat = new uint8_t[new_width*new_height*3]; +kj::Array get_frame_image(const CameraBuf *b) { + static const int x_min = getenv("XMIN") ? atoi(getenv("XMIN")) : 0; + static const int y_min = getenv("YMIN") ? atoi(getenv("YMIN")) : 0; + static const int env_xmax = getenv("XMAX") ? atoi(getenv("XMAX")) : -1; + static const int env_ymax = getenv("YMAX") ? atoi(getenv("YMAX")) : -1; + static const int scale = getenv("SCALE") ? atoi(getenv("SCALE")) : 1; - int goff = x_min*3 + y_min*stride; - for (int r=0;rcur_rgb_buf); + + const int x_max = env_xmax != -1 ? env_xmax : b->rgb_width - 1; + const int y_max = env_ymax != -1 ? env_ymax : b->rgb_height - 1; + const int new_width = (x_max - x_min + 1) / scale; + const int new_height = (y_max - y_min + 1) / scale; + const uint8_t *dat = (const uint8_t *)b->cur_rgb_buf->addr; + + kj::Array frame_image = kj::heapArray(new_width*new_height*3); + uint8_t *resized_dat = frame_image.begin(); + int goff = x_min*3 + y_min*b->rgb_stride; + for (int r=0;rrgb_stride*scale+c*3*scale], 3*sizeof(uint8_t)); } - framed.setImage(kj::arrayPtr((const uint8_t*)resized_dat, new_width*new_height*3)); - delete[] resized_dat; } + return kj::mv(frame_image); } -void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { - const CameraBuf *b = &c->buf; - +static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) { uint8_t* thumbnail_buffer = NULL; unsigned long thumbnail_len = 0; @@ -279,6 +256,7 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { #endif JSAMPROW row_pointer[1]; + const uint8_t *bgr_ptr = (const uint8_t *)b->cur_rgb_buf->addr; for (int ii = 0; ii < b->rgb_height/4; ii+=1) { for (int j = 0; j < b->rgb_width*3; j+=12) { for (int k = 0; k < 3; k++) { @@ -308,9 +286,7 @@ void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr) { thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof); thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); - if (s->pm != NULL) { - s->pm->send("thumbnail", msg); - } + pm->send("thumbnail", msg); free(thumbnail_buffer); } @@ -318,76 +294,99 @@ void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, in const CameraBuf *b = &c->buf; uint32_t lum_binning[256] = {0}; + unsigned int lum_total = 0; for (int y = y_start; y < y_end; y += y_skip) { for (int x = x_start; x < x_end; x += x_skip) { - uint8_t lum = pix_ptr[(y * b->yuv_width) + x]; + uint8_t lum = pix_ptr[(y * b->rgb_width) + x]; +#ifdef QCOM2 + if (lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) { + continue; + } +#endif lum_binning[lum]++; + lum_total += 1; } } - unsigned int lum_total = (y_end - y_start) * (x_end - x_start) / x_skip / y_skip; unsigned int lum_cur = 0; int lum_med = 0; int lum_med_alt = 0; for (lum_med=255; lum_med>=0; lum_med--) { lum_cur += lum_binning[lum_med]; #ifdef QCOM2 - bool reach_hlc_perc = false; - if (c->camera_num == 0) { // wide - reach_hlc_perc = lum_cur > 2*lum_total / (3*HLC_A); - } else { - reach_hlc_perc = lum_cur > lum_total / HLC_A; - } - if (reach_hlc_perc && lum_med > HLC_THRESH) { - lum_med_alt = 86; + int lum_med_tmp = 0; + int hb = HLC_THRESH; + if (lum_cur > 0 && lum_med > hb) { + lum_med_tmp = 4 * (lum_med - hb) + 100; } + lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp; #endif if (lum_cur >= lum_total / 2) { break; } } - lum_med = lum_med_alt>lum_med?lum_med_alt:lum_med; + lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*(lum_med_alt - lum_med)/lum_total/2:lum_med; camera_autoexposure(c, lum_med / 256.0); } -extern volatile sig_atomic_t do_exit; +extern ExitHandler do_exit; -void *processing_thread(MultiCameraState *cameras, const char *tname, - CameraState *cs, process_thread_cb callback) { - set_thread_name(tname); +void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + const char *thread_name = nullptr; + if (cs == &cameras->road_cam) { + thread_name = "RoadCamera"; + } else if (cs == &cameras->driver_cam) { + thread_name = "DriverCamera"; + } else { + thread_name = "WideRoadCamera"; + } + set_thread_name(thread_name); for (int cnt = 0; !do_exit; cnt++) { if (!cs->buf.acquire()) continue; callback(cameras, cs, cnt); + if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) { + // this takes 10ms??? + publish_thumbnail(cameras->pm, &(cs->buf)); + } cs->buf.release(); } return NULL; } -std::thread start_process_thread(MultiCameraState *cameras, const char *tname, - CameraState *cs, process_thread_cb callback) { - return std::thread(processing_thread, cameras, tname, cs, callback); +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + return std::thread(processing_thread, cameras, cs, callback); } -void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { +void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; static int x_min = 0, x_max = 0, y_min = 0, y_max = 0; - static const bool rhd_front = Params().read_db_bool("IsRHD"); + static const bool is_rhd = Params().read_db_bool("IsRHD"); // auto exposure if (cnt % 3 == 0) { if (sm->update(0) > 0 && sm->updated("driverState")) { auto state = (*sm)["driverState"].getDriverState(); - // set front camera metering target + // set driver camera metering target if (state.getFaceProb() > 0.4) { auto face_position = state.getFacePosition(); - int x_offset = rhd_front ? 0 : b->rgb_width - (0.5 * b->rgb_height); - x_offset += (face_position[0] * (rhd_front ? -1.0 : 1.0) + 0.5) * (0.5 * b->rgb_height); - const int y_offset = (face_position[1] + 0.5) * b->rgb_height; - +#ifndef QCOM2 + int frame_width = b->rgb_width; + int frame_height = b->rgb_height; +#else + int frame_width = 668; + int frame_height = frame_width / 1.33; +#endif + int x_offset = is_rhd ? 0 : frame_width - (0.5 * frame_height); + x_offset += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height); + int y_offset = (face_position[1] + 0.5) * frame_height; +#ifdef QCOM2 + x_offset += 630; + y_offset += 156; +#endif x_min = std::max(0, x_offset - 72); x_max = std::min(b->rgb_width - 1, x_offset + 72); y_min = std::max(0, y_offset - 72); @@ -401,27 +400,29 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i // use driver face crop for AE if (x_max == 0) { // default setting - x_min = rhd_front ? 0 : b->rgb_width * 3 / 5; - x_max = rhd_front ? b->rgb_width * 2 / 5 : b->rgb_width; +#ifndef QCOM2 + x_min = is_rhd ? 0 : b->rgb_width * 3 / 5; + x_max = is_rhd ? b->rgb_width * 2 / 5 : b->rgb_width; y_min = b->rgb_height / 3; y_max = b->rgb_height; - } -#ifdef QCOM2 - x_min = 96; - x_max = 1832; - y_min = 242; - y_max = 1148; - skip = 4; +#else + x_min = 96; + x_max = 1832; + y_min = 242; + y_max = 1148; + skip = 4; #endif - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x_min, x_max, 2, y_min, y_max, skip); + } + + set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x_min, x_max, 2, y_min, y_max, skip); } MessageBuilder msg; - auto framed = msg.initEvent().initFrontFrame(); + auto framed = msg.initEvent().initDriverCameraState(); framed.setFrameType(cereal::FrameData::FrameType::FRONT); - fill_frame_data(framed, b->cur_frame_data, cnt); - if (env_send_front) { - fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); + fill_frame_data(framed, b->cur_frame_data); + if (env_send_driver) { + framed.setImage(get_frame_image(b)); } - pm->send("frontFrame", msg); + pm->send("driverCameraState", msg); } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 1eb68422e..9e26ef4d5 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -5,16 +5,17 @@ #include #include #include -#include "common/buffering.h" #include "common/mat.h" #include "common/swaglog.h" -#include "common/visionbuf.h" +#include "common/queue.h" +#include "visionbuf.h" #include "common/visionimg.h" #include "imgproc/utils.h" #include "messaging.hpp" #include "transforms/rgb_to_yuv.h" -#include "common/visionipc.h" +#include "visionipc.h" +#include "visionipc_server.h" #define CAMERA_ID_IMX298 0 #define CAMERA_ID_IMX179 1 @@ -35,12 +36,13 @@ #define LOG_CAMERA_ID_QCAMERA 3 #define LOG_CAMERA_ID_MAX 4 -const bool env_send_front = getenv("SEND_FRONT") != NULL; -const bool env_send_rear = getenv("SEND_REAR") != NULL; -const bool env_send_wide = getenv("SEND_WIDE") != NULL; +const bool env_send_driver = getenv("SEND_DRIVER") != NULL; +const bool env_send_road = getenv("SEND_ROAD") != NULL; +const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL; + +typedef void (*release_cb)(void *cookie, int buf_idx); typedef struct CameraInfo { - const char* name; int frame_width, frame_height; int frame_stride; bool bayer; @@ -82,61 +84,50 @@ typedef struct CameraExpInfo { extern CameraInfo cameras_supported[CAMERA_ID_MAX]; -typedef struct { - uint8_t *y, *u, *v; -} YUVBuf; - struct MultiCameraState; struct CameraState; -typedef void (*release_cb)(void *cookie, int buf_idx); class CameraBuf { -public: - +private: + VisionIpcServer *vipc_server; CameraState *camera_state; cl_kernel krnl_debayer; - cl_command_queue q; - Pool yuv_pool; - VisionBuf yuv_ion[YUV_COUNT]; - YUVBuf yuv_bufs[YUV_COUNT]; - FrameMetadata yuv_metas[YUV_COUNT]; - size_t yuv_buf_size; - int yuv_width, yuv_height; RGBToYUVState rgb_to_yuv_state; - int rgb_width, rgb_height, rgb_stride; - VisionBuf rgb_bufs[UI_BUF_COUNT]; + FrameMetadata yuv_metas[YUV_COUNT]; + VisionStreamType rgb_type, yuv_type; + + int cur_buf_idx; - mat3 yuv_transform; + SafeQueue safe_queue; - int cur_yuv_idx, cur_rgb_idx; + int frame_buf_count; + release_cb release_callback; + +public: + cl_command_queue q; FrameMetadata cur_frame_data; VisionBuf *cur_rgb_buf; - - + VisionBuf *cur_yuv_buf; std::unique_ptr camera_bufs; std::unique_ptr camera_bufs_metadata; - TBuffer camera_tb, ui_tb; - TBuffer *yuv_tb; // only for visionserver + int rgb_width, rgb_height, rgb_stride; + + mat3 yuv_transform; CameraBuf() = default; ~CameraBuf(); - void init(cl_device_id device_id, cl_context context, CameraState *s, int frame_cnt, - const char *name = "frame", release_cb relase_callback = nullptr); + void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType rgb_type, VisionStreamType yuv_type, release_cb release_callback=nullptr); bool acquire(); void release(); - void stop(); - int frame_buf_count; - int frame_size; + void queue(size_t buf_idx); }; typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); -void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, uint32_t cnt); -void fill_frame_image(cereal::FrameData::Builder &framed, uint8_t *dat, int w, int h, int stride); -void create_thumbnail(MultiCameraState *s, CameraState *c, uint8_t *bgr_ptr); +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data); +kj::Array get_frame_image(const CameraBuf *b); void set_exposure_target(CameraState *c, const uint8_t *pix_ptr, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); -std::thread start_process_thread(MultiCameraState *cameras, const char *tname, - CameraState *cs, process_thread_cb callback); -void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); +void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt); diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index aab8d72c6..7fbd3d347 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -2,73 +2,59 @@ #include #include -#include -#include -#include +#include + #include "messaging.hpp" - #include "common/util.h" -#include "common/timing.h" -#include "common/swaglog.h" -#include "buffering.h" - -extern "C" { -#include -} - -extern volatile sig_atomic_t do_exit; #define FRAME_WIDTH 1164 #define FRAME_HEIGHT 874 +extern ExitHandler do_exit; + namespace { -void camera_open(CameraState *s, bool rear) { -} -void camera_close(CameraState *s) { - s->buf.stop(); -} - -void camera_init(CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx) { +void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { assert(camera_id < ARRAYSIZE(cameras_supported)); s->ci = cameras_supported[camera_id]; assert(s->ci.frame_width != 0); + s->camera_num = camera_id; s->fps = fps; - s->buf.init(device_id, ctx, s, FRAME_BUF_COUNT, "camera"); + s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type); } -void run_frame_stream(MultiCameraState *s) { - s->sm = new SubMaster({"frame"}); - - CameraState *const rear_camera = &s->rear; - auto *tb = &rear_camera->buf.camera_tb; +void run_frame_stream(CameraState &camera, const char* frame_pkt) { + SubMaster sm({frame_pkt}); + size_t buf_idx = 0; while (!do_exit) { - if (s->sm->update(1000) == 0) continue; + if (sm.update(1000) == 0) continue; - auto frame = (*(s->sm))["frame"].getFrame(); - - const int buf_idx = tbuffer_select(tb); - rear_camera->buf.camera_bufs_metadata[buf_idx] = { - .frame_id = frame.getFrameId(), - .timestamp_eof = frame.getTimestampEof(), - .frame_length = static_cast(frame.getFrameLength()), - .integ_lines = static_cast(frame.getIntegLines()), - .global_gain = static_cast(frame.getGlobalGain()), + auto msg = static_cast(sm[frame_pkt]); + auto frame = msg.get(frame_pkt).as(); + camera.buf.camera_bufs_metadata[buf_idx] = { + .frame_id = frame.get("frameId").as(), + .timestamp_eof = frame.get("timestampEof").as(), + .frame_length = frame.get("frameLength").as(), + .integ_lines = frame.get("integLines").as(), + .global_gain = frame.get("globalGain").as(), }; - cl_command_queue q = rear_camera->buf.camera_bufs[buf_idx].copy_q; - cl_mem yuv_cl = rear_camera->buf.camera_bufs[buf_idx].buf_cl; + cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q; + cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl; - clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, frame.getImage().size(), frame.getImage().begin(), 0, NULL, NULL); - tbuffer_dispatch(tb, buf_idx); + auto image = frame.get("image").as(); + clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL); + camera.buf.queue(buf_idx); + buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT; } } } // namespace +// TODO: make this more generic CameraInfo cameras_supported[CAMERA_ID_MAX] = { [CAMERA_ID_IMX298] = { .frame_width = FRAME_WIDTH, @@ -81,54 +67,27 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_width = 1632, .frame_height = 1224, .frame_stride = 2040, // seems right - .bayer = true, + .bayer = false, .bayer_flip = 3, .hdr = false }, }; -void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - camera_init(&s->rear, CAMERA_ID_IMX298, 20, device_id, ctx); - s->rear.transform = (mat3){{ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - }}; - - camera_init(&s->front, CAMERA_ID_OV8865, 10, device_id, ctx); - s->front.transform = (mat3){{ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - }}; +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { + camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 20, device_id, ctx, + VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); + camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 10, device_id, ctx, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); } +void cameras_open(MultiCameraState *s) {} +void cameras_close(MultiCameraState *s) {} void camera_autoexposure(CameraState *s, float grey_frac) {} - -void cameras_open(MultiCameraState *s) { - // LOG("*** open front ***"); - camera_open(&s->front, false); - - // LOG("*** open rear ***"); - camera_open(&s->rear, true); -} - -void cameras_close(MultiCameraState *s) { - camera_close(&s->rear); -} - -// called by processing_thread -void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) { - if (cnt % 100 == 3) { - const CameraBuf *b = &c->buf; - create_thumbnail(s, c, (uint8_t*)b->cur_rgb_buf->addr); - } -} +void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {} void cameras_run(MultiCameraState *s) { - std::thread t = start_process_thread(s, "processing", &s->rear, camera_process_rear); + std::thread t = start_process_thread(s, &s->road_cam, process_road_camera); set_thread_name("frame_streaming"); - run_frame_stream(s); - cameras_close(s); + run_frame_stream(s->road_cam, "roadCameraState"); t.join(); } diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h index 5d6f71b87..fe03c1170 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.h +++ b/selfdrive/camerad/cameras/camera_frame_stream.h @@ -15,28 +15,27 @@ typedef struct CameraState { int camera_id; + int camera_num; CameraInfo ci; int fps; float digital_gain; float cur_gain_frac; - mat3 transform; - CameraBuf buf; } CameraState; typedef struct MultiCameraState { int ispif_fd; - CameraState rear; - CameraState front; + CameraState road_cam; + CameraState driver_cam; SubMaster *sm; PubMaster *pm; } MultiCameraState; -void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_init(VisionIpcServer * v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 451829ec4..051854b3f 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -14,7 +14,6 @@ #include #include -#include #include "msmb_isp.h" #include "msmb_ispif.h" #include "msmb_camera.h" @@ -26,18 +25,15 @@ #include "common/params.h" #include "clutil.h" -#include "cereal/gen/cpp/log.capnp.h" - #include "sensor_i2c.h" - #include "camera_qcom.h" -extern volatile sig_atomic_t do_exit; +extern ExitHandler do_exit; // global var for AE/AF ops -std::atomic rear_exp{{0}}; -std::atomic front_exp{{0}}; +std::atomic road_cam_exp{{0}}; +std::atomic driver_cam_exp{{0}}; CameraInfo cameras_supported[CAMERA_ID_MAX] = { [CAMERA_ID_IMX298] = { @@ -98,9 +94,10 @@ static void camera_release_buffer(void* cookie, int buf_idx) { ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &s->ss[0].qbuf_info[buf_idx]); } -static void camera_init(CameraState *s, int camera_id, int camera_num, +static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num, uint32_t pixel_clock, uint32_t line_length_pclk, - unsigned int max_gain, unsigned int fps, cl_device_id device_id, cl_context ctx) { + unsigned int max_gain, unsigned int fps, cl_device_id device_id, cl_context ctx, + VisionStreamType rgb_type, VisionStreamType yuv_type) { s->camera_num = camera_num; s->camera_id = camera_id; @@ -115,12 +112,11 @@ static void camera_init(CameraState *s, int camera_id, int camera_num, s->self_recover = 0; - s->buf.init(device_id, ctx, s, FRAME_BUF_COUNT, "frame", camera_release_buffer); + s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type, camera_release_buffer); pthread_mutex_init(&s->frame_info_lock, NULL); } - int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, msm_camera_i2c_data_type data_type) { struct msm_camera_i2c_reg_setting out_settings = { .reg_setting = arr, @@ -129,23 +125,13 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size .data_type = data_type, .delay = 0, }; - struct sensorb_cfg_data cfg_data = {0}; - cfg_data.cfgtype = CFG_WRITE_I2C_ARRAY; - cfg_data.cfg.setting = &out_settings; + sensorb_cfg_data cfg_data = {.cfgtype = CFG_WRITE_I2C_ARRAY, .cfg.setting = &out_settings}; return ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &cfg_data); } static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { - int err; - int analog_gain = std::min(gain, 448); - - if (gain > 448) { - s->digital_gain = (512.0/(512-(gain))) / 8.0; - } else { - s->digital_gain = 1.0; - } - + s->digital_gain = gain > 448 ? (512.0/(512-(gain))) / 8.0 : 1.0; //printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->cur_frame_length, analog_gain, s->digital_gain); struct msm_camera_i2c_reg_array reg_array[] = { @@ -171,7 +157,7 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int {0x104,0x0,0}, }; - err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); if (err != 0) { LOGE("apply_exposure err %d", err); } @@ -179,8 +165,8 @@ static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, int } static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { - //printf("front camera: %d %d %d\n", gain, integ_lines, frame_length); - int err, coarse_gain_bitmap, fine_gain_bitmap; + //printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length); + int coarse_gain_bitmap, fine_gain_bitmap; // get bitmaps from iso static const int gains[] = {0, 100, 200, 400, 800}; @@ -210,7 +196,7 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int //{0x104,0x0,0}, }; - err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); if (err != 0) { LOGE("apply_exposure err %d", err); } @@ -218,9 +204,7 @@ static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, int } static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_lines, int frame_length) { - //printf("front camera: %d %d %d\n", gain, integ_lines, frame_length); - int err; - + //printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length); struct msm_camera_i2c_reg_array reg_array[] = { {0x104,0x1,0}, @@ -233,7 +217,7 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li {0x104,0x0,0}, }; - err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, reg_array, ARRAYSIZE(reg_array), MSM_CAMERA_I2C_BYTE_DATA); if (err != 0) { LOGE("apply_exposure err %d", err); } @@ -251,7 +235,7 @@ cl_program build_conv_program(cl_device_id device_id, cl_context context, int im return cl_program_from_file(context, device_id, "imgproc/conv.cl", args); } -void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { char project_name[1024] = {0}; property_get("ro.boot.project_name", project_name, ""); @@ -287,7 +271,7 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { // 508 = ISO 12800, 16x digital gain // 510 = ISO 25600, 32x digital gain - camera_init(&s->rear, CAMERA_ID_IMX298, 0, + camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 0, /*pixel_clock=*/600000000, /*line_length_pclk=*/5536, /*max_gain=*/510, //0 (ISO 100)- 448 (ISO 800, max analog gain) - 511 (super noisy) #ifdef HIGH_FPS @@ -295,53 +279,43 @@ void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx) { #else /*fps*/ 20, #endif - device_id, ctx); - s->rear.apply_exposure = imx298_apply_exposure; + device_id, ctx, + VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); + s->road_cam.apply_exposure = imx298_apply_exposure; if (s->device == DEVICE_OP3T) { - camera_init(&s->front, CAMERA_ID_S5K3P8SP, 1, + camera_init(v, &s->driver_cam, CAMERA_ID_S5K3P8SP, 1, /*pixel_clock=*/560000000, /*line_length_pclk=*/5120, - /*max_gain=*/510, 10, device_id, ctx); - s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; + /*max_gain=*/510, 10, device_id, ctx, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); + s->driver_cam.apply_exposure = imx179_s5k3p8sp_apply_exposure; } else if (s->device == DEVICE_LP3) { - camera_init(&s->front, CAMERA_ID_OV8865, 1, + camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1, /*pixel_clock=*/72000000, /*line_length_pclk=*/1602, - /*max_gain=*/510, 10, device_id, ctx); - s->front.apply_exposure = ov8865_apply_exposure; + /*max_gain=*/510, 10, device_id, ctx, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); + s->driver_cam.apply_exposure = ov8865_apply_exposure; } else { - camera_init(&s->front, CAMERA_ID_IMX179, 1, + camera_init(v, &s->driver_cam, CAMERA_ID_IMX179, 1, /*pixel_clock=*/251200000, /*line_length_pclk=*/3440, - /*max_gain=*/224, 20, device_id, ctx); - s->front.apply_exposure = imx179_s5k3p8sp_apply_exposure; + /*max_gain=*/224, 20, device_id, ctx, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); + s->driver_cam.apply_exposure = imx179_s5k3p8sp_apply_exposure; } - // assume the device is upside-down (not anymore) - s->rear.transform = (mat3){{ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - }}; + s->road_cam.device = s->device; + s->driver_cam.device = s->device; - // probably wrong - s->front.transform = (mat3){{ - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - }}; - - s->rear.device = s->device; - s->front.device = s->device; - - s->sm_front = new SubMaster({"driverState"}); - s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"}); + s->sm = new SubMaster({"driverState"}); + s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"}); for (int i = 0; i < FRAME_BUF_COUNT; i++) { // TODO: make lengths correct - s->focus_bufs[i] = visionbuf_allocate(0xb80); - s->stats_bufs[i] = visionbuf_allocate(0xb80); + s->focus_bufs[i].allocate(0xb80); + s->stats_bufs[i].allocate(0xb80); } - const int width = s->rear.buf.rgb_width/NUM_SEGMENTS_X; - const int height = s->rear.buf.rgb_height/NUM_SEGMENTS_Y; + const int width = s->road_cam.buf.rgb_width/NUM_SEGMENTS_X; + const int height = s->road_cam.buf.rgb_height/NUM_SEGMENTS_Y; s->prg_rgb_laplacian = build_conv_program(device_id, ctx, width, height, 3); s->krnl_rgb_laplacian = CL_CHECK_ERR(clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err)); // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter @@ -376,7 +350,7 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { gain_frac = std::clamp(gain_frac, 1.0f/64, 1.0f); // linearize gain response - // TODO: will be wrong for front camera + // TODO: will be wrong for driver camera // 0.125 -> 448 // 0.25 -> 480 // 0.5 -> 496 @@ -440,7 +414,6 @@ static void do_autoexposure(CameraState *s, float grey_frac) { pthread_mutex_unlock(&s->frame_info_lock); set_exposure(s, s->cur_exposure_frac, cur_gain_frac); - } else { // keep the old for others float new_exposure = s->cur_exposure_frac; new_exposure *= pow(1.05, (target_grey - grey_frac) / 0.05 ); @@ -458,11 +431,8 @@ static void do_autoexposure(CameraState *s, float grey_frac) { } static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { - int err; - - struct msm_eeprom_cfg_data cfg = {}; - cfg.cfgtype = CFG_EEPROM_GET_CAL_DATA; - err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); + msm_eeprom_cfg_data cfg = {.cfgtype = CFG_EEPROM_GET_CAL_DATA}; + int err = ioctl(eeprom_fd, VIDIOC_MSM_EEPROM_CFG, &cfg); assert(err >= 0); uint32_t num_bytes = cfg.cfg.get_data.num_bytes; @@ -483,8 +453,6 @@ static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) { } static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) { - int err; - const int ois_registers[][2] = { // == SET_FADJ_PARAM() == (factory adjustment) @@ -526,7 +494,6 @@ static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) { }; - struct msm_ois_cfg_data cfg = {0}; struct msm_camera_i2c_seq_reg_array ois_reg_settings[ARRAYSIZE(ois_registers)] = {{0}}; for (int i=0; i= 0); - struct sensor_init_cfg_data sensor_init_cfg = {}; - - // init rear sensor + // init road camera sensor struct msm_camera_sensor_slave_info slave_info = {0}; if (s->device == DEVICE_LP3) { @@ -572,98 +533,34 @@ static void sensors_init(MultiCameraState *s) { .actuator_name = "dw9800w", .ois_name = "", .flash_name = "pmic", - .camera_id = CAMERA_0, + .camera_id = CAMERA_0, .slave_addr = 32, .i2c_freq_mode = I2C_FAST_MODE, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, - .sensor_id_info = { - .sensor_id_reg_addr = 22, - .sensor_id = 664, - .sensor_id_mask = 0, - .module_id = 9, - .vcm_id = 6, - }, + .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664, .module_id = 9, .vcm_id = 6}, .power_setting_array = { .power_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 5, - .config_val = 2, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 3, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 24000000, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 2, - .delay = 10, - }, + {.seq_type = SENSOR_GPIO, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2}, + {.seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1}, + {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1}, + {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 10}, }, .size = 7, .power_down_setting_a = { - { - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 5, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 3, - .config_val = 0, - .delay = 1, - }, + {.seq_type = SENSOR_CLK, .delay = 1}, + {.seq_type = SENSOR_GPIO, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_GPIO, .seq_val = 5}, + {.seq_type = SENSOR_VREG, .seq_val = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1}, }, .size_down = 6, }, .is_init_params_valid = 0, - .sensor_init_params = { - .modes_supported = 1, - .position = BACK_CAMERA_B, - .sensor_mount_angle = 90, - }, + .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90}, .output_format = MSM_SENSOR_BAYER, }; } else { @@ -676,112 +573,34 @@ static void sensors_init(MultiCameraState *s) { .slave_addr = 52, .i2c_freq_mode = I2C_CUSTOM_MODE, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, - .sensor_id_info = { - .sensor_id_reg_addr = 22, - .sensor_id = 664, - .sensor_id_mask = 0, - }, + .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664}, .power_setting_array = { .power_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 2, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 2, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 2, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 2, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 6, - .config_val = 2, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 3, - .config_val = 0, - .delay = 5, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 4, - .config_val = 0, - .delay = 5, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 24000000, - .delay = 2, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 2, - .delay = 2, - }, + {.seq_type = SENSOR_GPIO, .delay = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 2}, + {.seq_type = SENSOR_VREG, .delay = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 2}, + {.seq_type = SENSOR_GPIO, .seq_val = 6, .config_val = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 5}, + {.seq_type = SENSOR_VREG, .seq_val = 4, .delay = 5}, + {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 2}, + {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 2}, }, .size = 9, .power_down_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 10, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 4, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 3, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 6, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - }, + {.seq_type = SENSOR_GPIO, .delay = 10}, + {.seq_type = SENSOR_CLK, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 4}, + {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1}, + {.seq_type = SENSOR_GPIO, .seq_val = 6}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG}, + {.seq_type = SENSOR_VREG, .seq_val = 2}, }, .size_down = 8, }, .is_init_params_valid = 0, - .sensor_init_params = { - .modes_supported = 1, - .position = BACK_CAMERA_B, - .sensor_mount_angle = 360, - }, + .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 360}, .output_format = MSM_SENSOR_BAYER, }; } @@ -789,13 +608,11 @@ static void sensors_init(MultiCameraState *s) { (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_setting_a[0]; slave_info.power_setting_array.power_down_setting = (struct msm_sensor_power_setting *)&slave_info.power_setting_array.power_down_setting_a[0]; - sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; - sensor_init_cfg.cfg.setting = &slave_info; + sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &slave_info}; err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (rear): %d", err); + LOG("sensor init cfg (road camera): %d", err); assert(err >= 0); - struct msm_camera_sensor_slave_info slave_info2 = {0}; if (s->device == DEVICE_LP3) { slave_info2 = (struct msm_camera_sensor_slave_info){ @@ -808,88 +625,32 @@ static void sensors_init(MultiCameraState *s) { .slave_addr = 108, .i2c_freq_mode = I2C_FAST_MODE, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, - .sensor_id_info = { - .sensor_id_reg_addr = 12299, - .sensor_id = 34917, - .sensor_id_mask = 0, - .module_id = 2, - .vcm_id = 0, - }, + .sensor_id_info = {.sensor_id_reg_addr = 12299, .sensor_id = 34917, .module_id = 2}, .power_setting_array = { .power_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 5, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 24000000, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 2, - .delay = 1, - }, + {.seq_type = SENSOR_GPIO, .delay = 5}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2}, + {.seq_type = SENSOR_VREG}, + {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1}, + {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1}, }, .size = 6, .power_down_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 5, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 1, - }, + {.seq_type = SENSOR_GPIO, .delay = 5}, + {.seq_type = SENSOR_CLK, .delay = 1}, + {.seq_type = SENSOR_VREG}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1}, }, .size_down = 5, }, .is_init_params_valid = 0, - .sensor_init_params = { - .modes_supported = 1, - .position = FRONT_CAMERA_B, - .sensor_mount_angle = 270, - }, + .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270}, .output_format = MSM_SENSOR_BAYER, }; - } else if (s->front.camera_id == CAMERA_ID_S5K3P8SP) { - // init front camera + } else if (s->driver_cam.camera_id == CAMERA_ID_S5K3P8SP) { + // init driver camera slave_info2 = (struct msm_camera_sensor_slave_info){ .sensor_name = "s5k3p8sp", .eeprom_name = "s5k3p8sp_m24c64s", @@ -899,86 +660,32 @@ static void sensors_init(MultiCameraState *s) { .slave_addr = 32, .i2c_freq_mode = I2C_FAST_MODE, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, - .sensor_id_info = { - .sensor_id_reg_addr = 0, - .sensor_id = 12552, - .sensor_id_mask = 0, - }, + .sensor_id_info = {.sensor_id = 12552}, .power_setting_array = { .power_setting_a = { - { - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 24000000, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 2, - .delay = 1, - }, + {.seq_type = SENSOR_GPIO, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 1}, + {.seq_type = SENSOR_VREG, .delay = 1}, + {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1}, + {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1}, }, .size = 6, .power_down_setting_a = { - { - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 1, - }, + {.seq_type = SENSOR_CLK, .delay = 1}, + {.seq_type = SENSOR_GPIO, .delay = 1}, + {.seq_type = SENSOR_VREG, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 1, .delay = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1}, }, .size_down = 5, }, .is_init_params_valid = 0, - .sensor_init_params = { - .modes_supported = 1, - .position = FRONT_CAMERA_B, - .sensor_mount_angle = 270, - }, + .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270}, .output_format = MSM_SENSOR_BAYER, }; } else { - // init front camera + // init driver camera slave_info2 = (struct msm_camera_sensor_slave_info){ .sensor_name = "imx179", .eeprom_name = "sony_imx179", @@ -988,77 +695,27 @@ static void sensors_init(MultiCameraState *s) { .slave_addr = 32, .i2c_freq_mode = I2C_FAST_MODE, .addr_type = MSM_CAMERA_I2C_WORD_ADDR, - .sensor_id_info = { - .sensor_id_reg_addr = 2, - .sensor_id = 377, - .sensor_id_mask = 4095, - }, + .sensor_id_info = {.sensor_id_reg_addr = 2, .sensor_id = 377, .sensor_id_mask = 4095}, .power_setting_array = { .power_setting_a = { - { - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 2, - .delay = 0, - },{ - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 24000000, - .delay = 0, - }, + {.seq_type = SENSOR_VREG, .seq_val = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG}, + {.seq_type = SENSOR_GPIO, .config_val = 2}, + {.seq_type = SENSOR_CLK, .config_val = 24000000}, }, .size = 5, .power_down_setting_a = { - { - .seq_type = SENSOR_CLK, - .seq_val = 0, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_GPIO, - .seq_val = 0, - .config_val = 0, - .delay = 1, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 0, - .config_val = 0, - .delay = 2, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 1, - .config_val = 0, - .delay = 0, - },{ - .seq_type = SENSOR_VREG, - .seq_val = 2, - .config_val = 0, - .delay = 0, - }, + {.seq_type = SENSOR_CLK}, + {.seq_type = SENSOR_GPIO, .delay = 1}, + {.seq_type = SENSOR_VREG, .delay = 2}, + {.seq_type = SENSOR_VREG, .seq_val = 1}, + {.seq_type = SENSOR_VREG, .seq_val = 2}, }, .size_down = 5, }, .is_init_params_valid = 0, - .sensor_init_params = { - .modes_supported = 1, - .position = FRONT_CAMERA_B, - .sensor_mount_angle = 270, - }, + .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270}, .output_format = MSM_SENSOR_BAYER, }; } @@ -1069,20 +726,14 @@ static void sensors_init(MultiCameraState *s) { sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; sensor_init_cfg.cfg.setting = &slave_info2; err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (front): %d", err); + LOG("sensor init cfg (driver): %d", err); assert(err >= 0); } -static void camera_open(CameraState *s, bool rear) { +static void camera_open(CameraState *s, bool is_road_cam) { int err; - struct sensorb_cfg_data sensorb_cfg_data = {}; struct csid_cfg_data csid_cfg_data = {}; - struct csiphy_cfg_data csiphy_cfg_data = {}; - struct msm_camera_csiphy_params csiphy_params = {}; - struct msm_camera_csid_params csid_params = {}; - struct msm_vfe_input_cfg input_cfg = {}; - struct msm_vfe_axi_stream_update_cmd update_cmd = {}; struct v4l2_event_subscription sub = {}; struct msm_actuator_cfg_data actuator_cfg_data = {}; @@ -1090,7 +741,7 @@ static void camera_open(CameraState *s, bool rear) { // open devices const char *sensor_dev; - if (rear) { + if (is_road_cam) { s->csid_fd = open("/dev/v4l-subdev3", O_RDWR | O_NONBLOCK); assert(s->csid_fd >= 0); s->csiphy_fd = open("/dev/v4l-subdev0", O_RDWR | O_NONBLOCK); @@ -1142,7 +793,7 @@ static void camera_open(CameraState *s, bool rear) { s->sensor_fd = open(sensor_dev, O_RDWR | O_NONBLOCK); if (s->sensor_fd >= 0) break; LOGW("waiting for sensors..."); - sleep(1); + util::sleep_for(1000); // sleep one second } assert(s->sensor_fd >= 0); @@ -1151,8 +802,7 @@ static void camera_open(CameraState *s, bool rear) { // CSIPHY: release csiphy struct msm_camera_csi_lane_params csi_lane_params = {0}; csi_lane_params.csi_lane_mask = 0x1f; - csiphy_cfg_data.cfg.csi_lane_params = &csi_lane_params; - csiphy_cfg_data.cfgtype = CSIPHY_RELEASE; + csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE}; err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); LOG("release csiphy: %d", err); @@ -1162,12 +812,11 @@ static void camera_open(CameraState *s, bool rear) { LOG("release csid: %d", err); // SENSOR: send power down - memset(&sensorb_cfg_data, 0, sizeof(sensorb_cfg_data)); - sensorb_cfg_data.cfgtype = CFG_POWER_DOWN; + struct sensorb_cfg_data sensorb_cfg_data = {.cfgtype = CFG_POWER_DOWN}; err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); LOG("sensor power down: %d", err); - if (rear && s->device != DEVICE_LP3) { + if (is_road_cam && s->device != DEVICE_LP3) { // ois powerdown ois_cfg_data.cfgtype = CFG_OIS_POWERDOWN; err = ioctl(s->ois_fd, VIDIOC_MSM_OIS_CFG, &ois_cfg_data); @@ -1218,8 +867,7 @@ static void camera_open(CameraState *s, bool rear) { LOG("init csid: %d", err); // CSIPHY: init csiphy - memset(&csiphy_cfg_data, 0, sizeof(csiphy_cfg_data)); - csiphy_cfg_data.cfgtype = CSIPHY_INIT; + csiphy_cfg_data = {.cfgtype = CSIPHY_INIT}; err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); LOG("init csiphy: %d", err); @@ -1237,8 +885,7 @@ static void camera_open(CameraState *s, bool rear) { LOG("stop stream: %d", err); // SENSOR: send power up - memset(&sensorb_cfg_data, 0, sizeof(sensorb_cfg_data)); - sensorb_cfg_data.cfgtype = CFG_POWER_UP; + sensorb_cfg_data = {.cfgtype = CFG_POWER_UP}; err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); LOG("sensor power up: %d", err); @@ -1258,7 +905,7 @@ static void camera_open(CameraState *s, bool rear) { } LOG("sensor init i2c: %d", err); - if (rear) { + if (is_road_cam) { // init the actuator actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP; err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); @@ -1283,54 +930,17 @@ static void camera_open(CameraState *s, bool rear) { LOG(" -> macro_dac: %d infinity_dac: %d", macro_dac, s->infinity_dac); struct msm_actuator_reg_params_t actuator_reg_params[] = { - { - .reg_write_type = MSM_ACTUATOR_WRITE_DAC, - .hw_mask = 0, - .reg_addr = 240, - .hw_shift = 0, - .data_type = 10, - .addr_type = 4, - .reg_data = 0, - .delay = 0, - }, { - .reg_write_type = MSM_ACTUATOR_WRITE_DAC, - .hw_mask = 0, - .reg_addr = 241, - .hw_shift = 0, - .data_type = 10, - .addr_type = 4, - .reg_data = 0, - .delay = 0, - }, { - .reg_write_type = MSM_ACTUATOR_WRITE_DAC, - .hw_mask = 0, - .reg_addr = 242, - .hw_shift = 0, - .data_type = 10, - .addr_type = 4, - .reg_data = 0, - .delay = 0, - }, { - .reg_write_type = MSM_ACTUATOR_WRITE_DAC, - .hw_mask = 0, - .reg_addr = 243, - .hw_shift = 0, - .data_type = 10, - .addr_type = 4, - .reg_data = 0, - .delay = 0, - }, + {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 240, .data_type = 10, .addr_type = 4}, + {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 241, .data_type = 10, .addr_type = 4}, + {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 242, .data_type = 10, .addr_type = 4}, + {.reg_write_type = MSM_ACTUATOR_WRITE_DAC, .reg_addr = 243, .data_type = 10, .addr_type = 4}, }; //... struct reg_settings_t actuator_init_settings[1] = {0}; struct region_params_t region_params[] = { - { - .step_bound = {512, 0,}, - .code_per_step = 118, - .qvalue = 128, - }, + {.step_bound = {512, 0,}, .code_per_step = 118, .qvalue = 128} }; actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO; @@ -1393,14 +1003,10 @@ static void camera_open(CameraState *s, bool rear) { struct msm_actuator_reg_params_t actuator_reg_params[] = { { .reg_write_type = MSM_ACTUATOR_WRITE_DAC, - .hw_mask = 0, // MSB here at address 3 .reg_addr = 3, - .hw_shift = 0, .data_type = 9, .addr_type = 4, - .reg_data = 0, - .delay = 0, }, }; @@ -1416,11 +1022,7 @@ static void camera_open(CameraState *s, bool rear) { }; struct region_params_t region_params[] = { - { - .step_bound = {238, 0,}, - .code_per_step = 235, - .qvalue = 128, - }, + {.step_bound = {238, 0,}, .code_per_step = 235, .qvalue = 128} }; actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO; @@ -1436,14 +1038,9 @@ static void camera_open(CameraState *s, bool rear) { .i2c_data_type = MSM_ACTUATOR_WORD_DATA, .reg_tbl_params = &actuator_reg_params[0], .init_settings = &actuator_init_settings[0], - .park_lens = { - .damping_step = 1023, - .damping_delay = 14000, - .hw_params = 11, - .max_step = 20, - } + .park_lens = {.damping_step = 1023, .damping_delay = 14000, .hw_params = 11, .max_step = 20}, }, - .af_tuning_params = { + .af_tuning_params = { .initial_code = (int16_t)s->infinity_dac, .pwd_step = 0, .region_size = 1, @@ -1463,27 +1060,16 @@ static void camera_open(CameraState *s, bool rear) { } // CSIPHY: configure csiphy + struct msm_camera_csiphy_params csiphy_params = {}; if (s->camera_id == CAMERA_ID_IMX298) { - csiphy_params.lane_cnt = 4; - csiphy_params.settle_cnt = 14; - csiphy_params.lane_mask = 0x1f; - csiphy_params.csid_core = 0; + csiphy_params = {.lane_cnt = 4, .settle_cnt = 14, .lane_mask = 0x1f, .csid_core = 0}; } else if (s->camera_id == CAMERA_ID_S5K3P8SP) { - csiphy_params.lane_cnt = 4; - csiphy_params.settle_cnt = 24; - csiphy_params.lane_mask = 0x1f; - csiphy_params.csid_core = 0; + csiphy_params = {.lane_cnt = 4, .settle_cnt = 24, .lane_mask = 0x1f, .csid_core = 0}; } else if (s->camera_id == CAMERA_ID_IMX179) { - csiphy_params.lane_cnt = 4; - csiphy_params.settle_cnt = 11; - csiphy_params.lane_mask = 0x1f; - csiphy_params.csid_core = 2; + csiphy_params = {.lane_cnt = 4, .settle_cnt = 11, .lane_mask = 0x1f, .csid_core = 2}; } else if (s->camera_id == CAMERA_ID_OV8865) { // guess! - csiphy_params.lane_cnt = 4; - csiphy_params.settle_cnt = 24; - csiphy_params.lane_mask = 0x1f; - csiphy_params.csid_core = 2; + csiphy_params = {.lane_cnt = 4, .settle_cnt = 24, .lane_mask = 0x1f, .csid_core = 2}; } csiphy_cfg_data.cfgtype = CSIPHY_CFG; csiphy_cfg_data.cfg.csiphy_params = &csiphy_params; @@ -1491,27 +1077,19 @@ static void camera_open(CameraState *s, bool rear) { LOG("csiphy configure: %d", err); // CSID: configure csid - csid_params.lane_cnt = 4; - csid_params.lane_assign = 0x4320; - if (rear) { - csid_params.phy_sel = 0; - } else { - csid_params.phy_sel = 2; - } - csid_params.lut_params.num_cid = rear ? 3 : 1; - #define CSI_STATS 0x35 #define CSI_PD 0x36 - - csid_params.lut_params.vc_cfg_a[0].cid = 0; - csid_params.lut_params.vc_cfg_a[0].dt = CSI_RAW10; - csid_params.lut_params.vc_cfg_a[0].decode_format = CSI_DECODE_10BIT; - csid_params.lut_params.vc_cfg_a[1].cid = 1; - csid_params.lut_params.vc_cfg_a[1].dt = CSI_PD; - csid_params.lut_params.vc_cfg_a[1].decode_format = CSI_DECODE_10BIT; - csid_params.lut_params.vc_cfg_a[2].cid = 2; - csid_params.lut_params.vc_cfg_a[2].dt = CSI_STATS; - csid_params.lut_params.vc_cfg_a[2].decode_format = CSI_DECODE_10BIT; + struct msm_camera_csid_params csid_params = { + .lane_cnt = 4, + .lane_assign = 0x4320, + .phy_sel = (uint8_t)(is_road_cam ? 0 : 2), + .lut_params.num_cid = (uint8_t)(is_road_cam ? 3 : 1), + .lut_params.vc_cfg_a = { + {.cid = 0, .dt = CSI_RAW10, .decode_format = CSI_DECODE_10BIT}, + {.cid = 1, .dt = CSI_PD, .decode_format = CSI_DECODE_10BIT}, + {.cid = 2, .dt = CSI_STATS, .decode_format = CSI_DECODE_10BIT}, + }, + }; csid_params.lut_params.vc_cfg[0] = &csid_params.lut_params.vc_cfg_a[0]; csid_params.lut_params.vc_cfg[1] = &csid_params.lut_params.vc_cfg_a[1]; @@ -1523,17 +1101,15 @@ static void camera_open(CameraState *s, bool rear) { LOG("csid configure: %d", err); // ISP: SMMU_ATTACH - struct msm_vfe_smmu_attach_cmd smmu_attach_cmd = { - .security_mode = 0, - .iommu_attach_mode = IOMMU_ATTACH - }; + msm_vfe_smmu_attach_cmd smmu_attach_cmd = {.security_mode = 0, .iommu_attach_mode = IOMMU_ATTACH}; err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd); LOG("isp smmu attach: %d", err); // ******************* STREAM RAW ***************************** // configure QMET input - for (int i = 0; i < (rear ? 3 : 1); i++) { + struct msm_vfe_input_cfg input_cfg = {}; + for (int i = 0; i < (is_road_cam ? 3 : 1); i++) { StreamState *ss = &s->ss[i]; memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg)); @@ -1546,7 +1122,7 @@ static void camera_open(CameraState *s, bool rear) { // ISP: REQUEST_STREAM ss->stream_req.axi_stream_handle = 0; - if (rear) { + if (is_road_cam) { ss->stream_req.session_id = 2; ss->stream_req.stream_id = /*ISP_META_CHANNEL_BIT | */ISP_NATIVE_BUF_BIT | (1+i); } else { @@ -1562,7 +1138,7 @@ static void camera_open(CameraState *s, bool rear) { ss->stream_req.stream_src = (msm_vfe_axi_stream_src)(RDI_INTF_0+i); #ifdef HIGH_FPS - if (rear) { + if (is_road_cam) { ss->stream_req.frame_skip_pattern = EVERY_3FRAME; } #endif @@ -1602,6 +1178,7 @@ static void camera_open(CameraState *s, bool rear) { } // ISP: UPDATE_STREAM + struct msm_vfe_axi_stream_update_cmd update_cmd = {}; update_cmd.num_streams = 1; update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id; update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle; @@ -1619,7 +1196,7 @@ static void camera_open(CameraState *s, bool rear) { // ISP: START_STREAM s->stream_cfg.cmd = START_STREAM; - s->stream_cfg.num_streams = rear ? 3 : 1; + s->stream_cfg.num_streams = is_road_cam ? 3 : 1; for (int i = 0; i < s->stream_cfg.num_streams; i++) { s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle; } @@ -1634,14 +1211,12 @@ static struct damping_params_t actuator_ringing_params = { .hw_params = 0x0000e422, }; -static void rear_start(CameraState *s) { - int err; - +static void road_camera_start(CameraState *s) { struct msm_actuator_cfg_data actuator_cfg_data = {0}; set_exposure(s, 1.0, 1.0); - err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); LOG("sensor start regs: %d", err); // focus on infinity assuming phone is perpendicular @@ -1701,8 +1276,6 @@ static void rear_start(CameraState *s) { } void actuator_move(CameraState *s, uint16_t target) { - int err; - int step = target - s->cur_lens_pos; // LP3 moves only on even positions. TODO: use proper sensor params if (s->device == DEVICE_LP3) { @@ -1722,8 +1295,8 @@ void actuator_move(CameraState *s, uint16_t target) { .curr_lens_pos = s->cur_lens_pos, .ringing_params = &actuator_ringing_params, }; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - //LOGD("actuator move focus: %d", err); + int err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); + LOG("actuator move focus: %d", err); s->cur_step_pos = dest_step_pos; s->cur_lens_pos = actuator_cfg_data.cfg.move.curr_lens_pos; @@ -1783,7 +1356,7 @@ static std::optional get_accel_z(SubMaster *sm) { if (sm->update(0) > 0) { for (auto event : (*sm)["sensorEvents"].getSensorEvents()) { if (event.which() == cereal::SensorEventData::ACCELERATION) { - if (auto v = event.getAcceleration().getV(); v.size() >= 3) + if (auto v = event.getAcceleration().getV(); v.size() >= 3) return -v[2]; break; } @@ -1825,57 +1398,38 @@ static void do_autofocus(CameraState *s, SubMaster *sm) { void camera_autoexposure(CameraState *s, float grey_frac) { if (s->camera_num == 0) { - CameraExpInfo tmp = rear_exp.load(); + CameraExpInfo tmp = road_cam_exp.load(); tmp.op_id++; tmp.grey_frac = grey_frac; - rear_exp.store(tmp); + road_cam_exp.store(tmp); } else { - CameraExpInfo tmp = front_exp.load(); + CameraExpInfo tmp = driver_cam_exp.load(); tmp.op_id++; tmp.grey_frac = grey_frac; - front_exp.store(tmp); + driver_cam_exp.store(tmp); } } -static void front_start(CameraState *s) { - int err; - +static void driver_camera_start(CameraState *s) { set_exposure(s, 1.0, 1.0); - - err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); + int err = sensor_write_regs(s, start_reg_array, ARRAYSIZE(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA); LOG("sensor start regs: %d", err); } void cameras_open(MultiCameraState *s) { - int err; - struct ispif_cfg_data ispif_cfg_data = {}; - struct msm_ispif_param_data ispif_params = {}; - ispif_params.num = 4; - // rear camera - ispif_params.entries[0].vfe_intf = VFE0; - ispif_params.entries[0].intftype = RDI0; - ispif_params.entries[0].num_cids = 1; - ispif_params.entries[0].cids[0] = CID0; - ispif_params.entries[0].csid = CSID0; - // front camera - ispif_params.entries[1].vfe_intf = VFE1; - ispif_params.entries[1].intftype = RDI0; - ispif_params.entries[1].num_cids = 1; - ispif_params.entries[1].cids[0] = CID0; - ispif_params.entries[1].csid = CSID2; - // rear camera (focus) - ispif_params.entries[2].vfe_intf = VFE0; - ispif_params.entries[2].intftype = RDI1; - ispif_params.entries[2].num_cids = CID1; - ispif_params.entries[2].cids[0] = CID1; - ispif_params.entries[2].csid = CSID0; - // rear camera (stats, for AE) - ispif_params.entries[3].vfe_intf = VFE0; - ispif_params.entries[3].intftype = RDI2; - ispif_params.entries[3].num_cids = 1; - ispif_params.entries[3].cids[0] = CID2; - ispif_params.entries[3].csid = CSID0; - + struct msm_ispif_param_data ispif_params = { + .num = 4, + .entries = { + // road camera + {.vfe_intf = VFE0, .intftype = RDI0, .num_cids = 1, .cids[0] = CID0, .csid = CSID0}, + // driver camera + {.vfe_intf = VFE1, .intftype = RDI0, .num_cids = 1, .cids[0] = CID0, .csid = CSID2}, + // road camera (focus) + {.vfe_intf = VFE0, .intftype = RDI1, .num_cids = CID1, .cids[0] = CID1, .csid = CSID0}, + // road camera (stats, for AE) + {.vfe_intf = VFE0, .intftype = RDI2, .num_cids = 1, .cids[0] = CID2, .csid = CSID0}, + }, + }; s->msmcfg_fd = open("/dev/media0", O_RDWR | O_NONBLOCK); assert(s->msmcfg_fd >= 0); @@ -1898,15 +1452,15 @@ void cameras_open(MultiCameraState *s) { // err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); // LOG("ispif stop: %d", err); - LOG("*** open front ***"); - s->front.ss[0].bufs = s->front.buf.camera_bufs.get(); - camera_open(&s->front, false); + LOG("*** open driver camera ***"); + s->driver_cam.ss[0].bufs = s->driver_cam.buf.camera_bufs.get(); + camera_open(&s->driver_cam, false); - LOG("*** open rear ***"); - s->rear.ss[0].bufs = s->rear.buf.camera_bufs.get(); - s->rear.ss[1].bufs = s->focus_bufs; - s->rear.ss[2].bufs = s->stats_bufs; - camera_open(&s->rear, true); + LOG("*** open road camera ***"); + s->road_cam.ss[0].bufs = s->road_cam.buf.camera_bufs.get(); + s->road_cam.ss[1].bufs = s->focus_bufs; + s->road_cam.ss[2].bufs = s->stats_bufs; + camera_open(&s->road_cam, true); if (getenv("CAMERA_TEST")) { cameras_close(s); @@ -1914,23 +1468,16 @@ void cameras_open(MultiCameraState *s) { } // ISPIF: set vfe info - memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); - ispif_cfg_data.cfg_type = ISPIF_SET_VFE_INFO; - ispif_cfg_data.vfe_info.num_vfe = 2; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); + struct ispif_cfg_data ispif_cfg_data = {.cfg_type = ISPIF_SET_VFE_INFO, .vfe_info.num_vfe = 2}; + int err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); LOG("ispif set vfe info: %d", err); // ISPIF: setup - memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); - ispif_cfg_data.cfg_type = ISPIF_INIT; - ispif_cfg_data.csid_version = 0x30050000; //CSID_VERSION_V35 + ispif_cfg_data = {.cfg_type = ISPIF_INIT, .csid_version = 0x30050000 /* CSID_VERSION_V35*/}; err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); LOG("ispif setup: %d", err); - memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data)); - ispif_cfg_data.cfg_type = ISPIF_CFG; - ispif_cfg_data.params = ispif_params; - + ispif_cfg_data = {.cfg_type = ISPIF_CFG, .params = ispif_params}; err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); LOG("ispif cfg: %d", err); @@ -1938,19 +1485,15 @@ void cameras_open(MultiCameraState *s) { err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); LOG("ispif start_frame_boundary: %d", err); - front_start(&s->front); - rear_start(&s->rear); + driver_camera_start(&s->driver_cam); + road_camera_start(&s->road_cam); } static void camera_close(CameraState *s) { - int err; - - s->buf.stop(); - // ISP: STOP_STREAM s->stream_cfg.cmd = STOP_STREAM; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); + int err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); LOG("isp stop stream: %d", err); for (int i = 0; i < 3; i++) { @@ -2000,10 +1543,10 @@ const char* get_isp_event_name(unsigned int type) { static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) { pthread_mutex_lock(&s->frame_info_lock); - for (int i=0; iframe_metadata[i].frame_id == frame_id) { + for (auto &i : s->frame_metadata) { + if (i.frame_id == frame_id) { pthread_mutex_unlock(&s->frame_info_lock); - return s->frame_metadata[i]; + return i; } } pthread_mutex_unlock(&s->frame_info_lock); @@ -2014,35 +1557,31 @@ static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) { }; } -static void* ops_thread(void* arg) { - MultiCameraState *s = (MultiCameraState*)arg; +static void ops_thread(MultiCameraState *s) { + int last_road_cam_op_id = 0; + int last_driver_cam_op_id = 0; - int rear_op_id_last = 0; - int front_op_id_last = 0; - - CameraExpInfo rear_op; - CameraExpInfo front_op; + CameraExpInfo road_cam_op; + CameraExpInfo driver_cam_op; set_thread_name("camera_settings"); SubMaster sm({"sensorEvents"}); while(!do_exit) { - rear_op = rear_exp.load(); - if (rear_op.op_id != rear_op_id_last) { - do_autoexposure(&s->rear, rear_op.grey_frac); - do_autofocus(&s->rear, &sm); - rear_op_id_last = rear_op.op_id; + road_cam_op = road_cam_exp.load(); + if (road_cam_op.op_id != last_road_cam_op_id) { + do_autoexposure(&s->road_cam, road_cam_op.grey_frac); + do_autofocus(&s->road_cam, &sm); + last_road_cam_op_id = road_cam_op.op_id; } - front_op = front_exp.load(); - if (front_op.op_id != front_op_id_last) { - do_autoexposure(&s->front, front_op.grey_frac); - front_op_id_last = front_op.op_id; + driver_cam_op = driver_cam_exp.load(); + if (driver_cam_op.op_id != last_driver_cam_op_id) { + do_autoexposure(&s->driver_cam, driver_cam_op.grey_frac); + last_driver_cam_op_id = driver_cam_op.op_id; } - usleep(50000); + util::sleep_for(50); } - - return NULL; } static void update_lapmap(MultiCameraState *s, const CameraBuf *b, const int cnt) { @@ -2090,7 +1629,7 @@ static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t la if (self_recover < 2 && (lens_true_pos < (dac_down + 1) || lens_true_pos > (dac_up - 1)) && is_blur(lapres, lapres_size)) { // truly stuck, needs help if (--self_recover < -FOCUS_RECOVER_PATIENCE) { - LOGD("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, self_recover); + LOGD("road camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, self_recover); // parity determined by which end is stuck at self_recover = FOCUS_RECOVER_STEPS + (lens_true_pos < dac_m ? 1 : 0); } @@ -2105,64 +1644,49 @@ static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t la c->self_recover.store(self_recover); } -void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { - common_camera_process_front(s->sm_front, s->pm, c, cnt); +void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { + common_process_driver_camera(s->sm, s->pm, c, cnt); } // called by processing_thread -void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { +void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; update_lapmap(s, b, cnt); setup_self_recover(c, &s->lapres[0], std::size(s->lapres)); MessageBuilder msg; - auto framed = msg.initEvent().initFrame(); - fill_frame_data(framed, b->cur_frame_data, cnt); - if (env_send_rear) { - fill_frame_image(framed, (uint8_t *)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); + auto framed = msg.initEvent().initRoadCameraState(); + fill_frame_data(framed, b->cur_frame_data); + if (env_send_road) { + framed.setImage(get_frame_image(b)); } - framed.setFocusVal(s->rear.focus); - framed.setFocusConf(s->rear.confidence); - framed.setRecoverState(s->rear.self_recover); + framed.setFocusVal(s->road_cam.focus); + framed.setFocusConf(s->road_cam.confidence); + framed.setRecoverState(s->road_cam.self_recover); framed.setSharpnessScore(s->lapres); framed.setTransform(b->yuv_transform.v); - s->pm->send("frame", msg); - - if (cnt % 100 == 3) { - create_thumbnail(s, c, (uint8_t *)b->cur_rgb_buf->addr); - } + s->pm->send("roadCameraState", msg); if (cnt % 3 == 0) { const int x = 290, y = 322, width = 560, height = 314; const int skip = 1; - set_exposure_target(c, (const uint8_t *)b->yuv_bufs[b->cur_yuv_idx].y, x, x + width, skip, y, y + height, skip); + set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x, x + width, skip, y, y + height, skip); } } void cameras_run(MultiCameraState *s) { - int err; - - pthread_t ops_thread_handle; - err = pthread_create(&ops_thread_handle, NULL, - ops_thread, s); - assert(err == 0); std::vector threads; - threads.push_back(start_process_thread(s, "processing", &s->rear, camera_process_frame)); - threads.push_back(start_process_thread(s, "frontview", &s->front, camera_process_front)); + threads.push_back(std::thread(ops_thread, s)); + threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); - CameraState* cameras[2] = {&s->rear, &s->front}; + CameraState* cameras[2] = {&s->road_cam, &s->driver_cam}; while (!do_exit) { - struct pollfd fds[2] = {{0}}; - - fds[0].fd = cameras[0]->isp_fd; - fds[0].events = POLLPRI; - - fds[1].fd = cameras[1]->isp_fd; - fds[1].events = POLLPRI; - + struct pollfd fds[2] = {{.fd = cameras[0]->isp_fd, .events = POLLPRI}, + {.fd = cameras[1]->isp_fd, .events = POLLPRI}}; int ret = poll(fds, ARRAYSIZE(fds), 1000); - if (ret <= 0) { + if (ret < 0) { if (errno == EINTR || errno == EAGAIN) continue; LOGE("poll failed (%d - %d)", ret, errno); break; @@ -2174,93 +1698,62 @@ void cameras_run(MultiCameraState *s) { CameraState *c = cameras[i]; - struct v4l2_event ev; + struct v4l2_event ev = {}; ret = ioctl(c->isp_fd, VIDIOC_DQEVENT, &ev); - struct msm_isp_event_data *isp_event_data = (struct msm_isp_event_data *)ev.u.data; - unsigned int event_type = ev.type; - - uint64_t timestamp = (isp_event_data->mono_timestamp.tv_sec*1000000000ULL - + isp_event_data->mono_timestamp.tv_usec*1000); - - int buf_idx = isp_event_data->u.buf_done.buf_idx; - int stream_id = isp_event_data->u.buf_done.stream_id; - int buffer = (stream_id&0xFFFF) - 1; - - uint64_t t = nanos_since_boot(); - - /*if (i == 1) { - printf("%10.2f: VIDIOC_DQEVENT: %d type:%X (%s)\n", t*1.0/1e6, ret, event_type, get_isp_event_name(event_type)); - }*/ - - // printf("%d: %s\n", i, get_isp_event_name(event_type)); - - switch (event_type) { - case ISP_EVENT_BUF_DIVERT: - - /*if (c->is_samsung) { - printf("write %d\n", c->frame_size); - FILE *f = fopen("/tmp/test", "wb"); - fwrite((void*)c->camera_bufs[i].addr, 1, c->frame_size, f); - fclose(f); - }*/ - //printf("divert: %d %d %d\n", i, buffer, buf_idx); + const msm_isp_event_data *isp_event_data = (const msm_isp_event_data *)ev.u.data; + if (ev.type == ISP_EVENT_BUF_DIVERT) { + const int buf_idx = isp_event_data->u.buf_done.buf_idx; + const int buffer = (isp_event_data->u.buf_done.stream_id & 0xFFFF) - 1; if (buffer == 0) { c->buf.camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id); - tbuffer_dispatch(&c->buf.camera_tb, buf_idx); + c->buf.queue(buf_idx); } else { - uint8_t *d = (uint8_t*)(c->ss[buffer].bufs[buf_idx].addr); + auto &ss = c->ss[buffer]; if (buffer == 1) { - parse_autofocus(c, d); + parse_autofocus(c, (uint8_t *)(ss.bufs[buf_idx].addr)); } - c->ss[buffer].qbuf_info[buf_idx].dirty_buf = 1; - ioctl(c->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &c->ss[buffer].qbuf_info[buf_idx]); + ss.qbuf_info[buf_idx].dirty_buf = 1; + ioctl(c->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &ss.qbuf_info[buf_idx]); } - break; - case ISP_EVENT_EOF: - // printf("ISP_EVENT_EOF delta %f\n", (t-last_t)/1e6); - c->last_t = t; + } else if (ev.type == ISP_EVENT_EOF) { + const uint64_t timestamp = (isp_event_data->mono_timestamp.tv_sec * 1000000000ULL + isp_event_data->mono_timestamp.tv_usec * 1000); pthread_mutex_lock(&c->frame_info_lock); c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){ - .frame_id = isp_event_data->frame_id, - .timestamp_eof = timestamp, - .frame_length = (unsigned int)c->cur_frame_length, - .integ_lines = (unsigned int)c->cur_integ_lines, - .global_gain = (unsigned int)c->cur_gain, - .lens_pos = c->cur_lens_pos, - .lens_sag = c->last_sag_acc_z, - .lens_err = c->focus_err, - .lens_true_pos = c->lens_true_pos, - .gain_frac = c->cur_gain_frac, + .frame_id = isp_event_data->frame_id, + .timestamp_eof = timestamp, + .frame_length = (unsigned int)c->cur_frame_length, + .integ_lines = (unsigned int)c->cur_integ_lines, + .global_gain = (unsigned int)c->cur_gain, + .lens_pos = c->cur_lens_pos, + .lens_sag = c->last_sag_acc_z, + .lens_err = c->focus_err, + .lens_true_pos = c->lens_true_pos, + .gain_frac = c->cur_gain_frac, }; - c->frame_metadata_idx = (c->frame_metadata_idx+1)%METADATA_BUF_COUNT; + c->frame_metadata_idx = (c->frame_metadata_idx + 1) % METADATA_BUF_COUNT; pthread_mutex_unlock(&c->frame_info_lock); - break; - case ISP_EVENT_ERROR: + } else if (ev.type == ISP_EVENT_ERROR) { LOGE("ISP_EVENT_ERROR! err type: 0x%08x", isp_event_data->u.error_info.err_type); - break; } } } LOG(" ************** STOPPING **************"); - err = pthread_join(ops_thread_handle, NULL); - assert(err == 0); + for (auto &t : threads) t.join(); cameras_close(s); - - for (auto &t : threads) t.join(); } void cameras_close(MultiCameraState *s) { - camera_close(&s->rear); - camera_close(&s->front); + camera_close(&s->road_cam); + camera_close(&s->driver_cam); for (int i = 0; i < FRAME_BUF_COUNT; i++) { - visionbuf_free(&s->focus_bufs[i]); - visionbuf_free(&s->stats_bufs[i]); + s->focus_bufs[i].free(); + s->stats_bufs[i].free(); } CL_CHECK(clReleaseMemObject(s->rgb_conv_roi_cl)); CL_CHECK(clReleaseMemObject(s->rgb_conv_result_cl)); @@ -2268,6 +1761,6 @@ void cameras_close(MultiCameraState *s) { CL_CHECK(clReleaseKernel(s->krnl_rgb_laplacian)); CL_CHECK(clReleaseProgram(s->prg_rgb_laplacian)); - delete s->sm_front; + delete s->sm; delete s->pm; } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 7bef442d3..82934ef95 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -12,10 +12,10 @@ #include "msmb_camera.h" #include "msm_cam_sensor.h" +#include "visionbuf.h" + #include "common/mat.h" -#include "common/visionbuf.h" -#include "common/buffering.h" -#include "common/utilpp.h" +#include "common/util.h" #include "camera_common.h" @@ -91,14 +91,12 @@ typedef struct CameraState { StreamState ss[3]; - uint64_t last_t; - camera_apply_exposure_func apply_exposure; int16_t focus[NUM_FOCUS]; uint8_t confidence[NUM_FOCUS]; - float focus_err; + std::atomic focus_err; uint16_t cur_step_pos; uint16_t cur_lens_pos; @@ -109,8 +107,6 @@ typedef struct CameraState { int fps; - mat3 transform; - CameraBuf buf; } CameraState; @@ -131,15 +127,15 @@ typedef struct MultiCameraState { cl_program prg_rgb_laplacian; cl_kernel krnl_rgb_laplacian; - CameraState rear; - CameraState front; + CameraState road_cam; + CameraState driver_cam; - SubMaster *sm_front; + SubMaster *sm; PubMaster *pm; } MultiCameraState; -void cameras_init(MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc new file mode 100644 index 000000000..3fef92675 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -0,0 +1,1179 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/util.h" +#include "common/swaglog.h" +#include "camera_qcom2.h" + +#include "media/cam_defs.h" +#include "media/cam_isp.h" +#include "media/cam_isp_ife.h" +#include "media/cam_sensor_cmn_header.h" +#include "media/cam_sensor.h" +#include "media/cam_sync.h" + +#include "sensor2_i2c.h" + +#define FRAME_WIDTH 1928 +#define FRAME_HEIGHT 1208 +//#define FRAME_STRIDE 1936 // for 8 bit output +#define FRAME_STRIDE 2416 // for 10 bit output + + +extern ExitHandler do_exit; + +// global var for AE ops +std::atomic cam_exp[3] = {{{0}}}; + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_AR0231] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, + .bayer = true, + .bayer_flip = 1, + .hdr = false + }, +}; + +float sensor_analog_gains[ANALOG_GAIN_MAX_IDX] = {3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, + 5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, + 7.0/2.0, 8.0/2.0}; + +// ************** low level camera helpers **************** + +int cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol); + if (ret == -1) { + printf("OP CODE ERR - %d \n", op_code); + perror("wat"); + } + return ret; +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + static struct cam_release_dev_cmd release_dev_cmd; + release_dev_cmd.session_handle = session_handle; + release_dev_cmd.dev_handle = dev_handle; + return cam_control(fd, op_code, &release_dev_cmd, sizeof(release_dev_cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, int align, int flags, uint32_t *handle, int mmu_hdl, int mmu_hdl2) { + struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0}; + mem_mgr_alloc_cmd.len = len; + mem_mgr_alloc_cmd.align = align; + mem_mgr_alloc_cmd.flags = flags; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + // LOGD("alloced: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void *alloc(int video0_fd, int len, int align, int flags, uint32_t *handle) { + return alloc_w_mmu_hdl(video0_fd, len, align, flags, handle, 0, 0); +} + +void release(int video0_fd, uint32_t handle) { + int ret; + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + assert(ret == 0); +} + +void release_fd(int video0_fd, uint32_t handle) { + // handle to fd + close(handle>>16); + release(video0_fd, handle); +} + +void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) { + struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; + req_mgr_flush_request.session_hdl = session_hdl; + req_mgr_flush_request.link_hdl = link_hdl; + req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL; + int ret; + ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); + // LOGD("flushed all req: %d", ret); +} + +// ************** high level camera helpers **************** + +void sensors_poke(struct CameraState *s, int request_id) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet); + struct cam_packet *pkt = (struct cam_packet *)alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 0; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = 0x7f; + pkt->header.request_id = request_id; + + struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->sensor_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + munmap(pkt, size); + release_fd(s->video0_fd, cam_packet_handle); +} + +void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) { + // LOGD("sensors_i2c: %d", len); + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + struct cam_packet *pkt = (struct cam_packet *)alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = op_code; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); + buf_desc[0].type = CAM_CMD_BUF_I2C; + + struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[0].mem_handle); + i2c_random_wr->header.count = len; + i2c_random_wr->header.op_code = 1; + i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; + i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload)); + + struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->sensor_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + munmap(i2c_random_wr, buf_desc[0].size); + release_fd(s->video0_fd, buf_desc[0].mem_handle); + munmap(pkt, size); + release_fd(s->video0_fd, cam_packet_handle); +} + +void sensors_init(int video0_fd, int sensor_fd, int camera_num) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + struct cam_packet *pkt = (struct cam_packet *)alloc(video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = -1; + pkt->header.op_code = 0x1000003; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); + buf_desc[0].type = CAM_CMD_BUF_LEGACY; + struct cam_cmd_i2c_info *i2c_info = (struct cam_cmd_i2c_info *)alloc(video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[0].mem_handle); + struct cam_cmd_probe *probe = (struct cam_cmd_probe *)((uint8_t *)i2c_info) + sizeof(struct cam_cmd_i2c_info); + + switch (camera_num) { + case 0: + // port 0 + i2c_info->slave_addr = 0x20; + probe->camera_id = 0; + break; + case 1: + // port 1 + i2c_info->slave_addr = 0x30; + probe->camera_id = 1; + break; + case 2: + // port 2 + i2c_info->slave_addr = 0x20; + probe->camera_id = 2; + break; + } + + // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz + //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; + i2c_info->i2c_freq_mode = I2C_FAST_MODE; + i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO; + + probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; + probe->op_code = 3; // don't care? + probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; + probe->reg_addr = 0x3000; //0x300a; //0x300b; + probe->expected_data = 0x354; //0x7750; //0x885a; + probe->data_mask = 0; + + //buf_desc[1].size = buf_desc[1].length = 148; + buf_desc[1].size = buf_desc[1].length = 196; + buf_desc[1].type = CAM_CMD_BUF_I2C; + struct cam_cmd_power *power = (struct cam_cmd_power *)alloc(video0_fd, buf_desc[1].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[1].mem_handle); + memset(power, 0, buf_desc[1].size); + struct cam_cmd_unconditional_wait *unconditional_wait; + + //void *ptr = power; + // 7750 + /*power->count = 2; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 8; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/ + + // 885a + power->count = 4; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 3; // clock?? + power->power_settings[1].power_seq_type = 1; // analog + power->power_settings[2].power_seq_type = 2; // digital + power->power_settings[3].power_seq_type = 8; // reset low + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 5; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // set clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 24000000; //Hz + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 10; // ms + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // 8,1 is this reset? + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 100; // ms + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // probe happens here + + // disable clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 0; + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // reset low + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 0; + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + unconditional_wait = (struct cam_cmd_unconditional_wait *)power; + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = 1; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + power = (struct cam_cmd_power *)((char*)power + sizeof(struct cam_cmd_unconditional_wait)); + + // 7750 + /*power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power = (void*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings));*/ + + // 885a + power->count = 3; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 1; + power->power_settings[2].power_seq_type = 3; + power = (struct cam_cmd_power *)((char*)power + (sizeof(struct cam_cmd_power) + (power->count-1)*sizeof(struct cam_power_settings))); + + LOGD("probing the sensor"); + int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); + assert(ret == 0); + + munmap(i2c_info, buf_desc[0].size); + release_fd(video0_fd, buf_desc[0].mem_handle); + munmap(power, buf_desc[1].size); + release_fd(video0_fd, buf_desc[1].mem_handle); + munmap(pkt, size); + release_fd(video0_fd, cam_packet_handle); +} + +void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + if (io_mem_handle != 0) { + size += sizeof(struct cam_buf_io_cfg); + } + struct cam_packet *pkt = (struct cam_packet *)alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = 0; + + if (io_mem_handle != 0) { + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; + pkt->num_io_configs = 1; + } + + if (io_mem_handle != 0) { + pkt->header.op_code = 0xf000001; + pkt->header.request_id = request_id; + } else { + pkt->header.op_code = 0xf000000; + } + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset); + + // TODO: support MMU + buf_desc[0].size = 65624; + buf_desc[0].length = 0; + buf_desc[0].type = CAM_CMD_BUF_DIRECT; + buf_desc[0].meta_data = 3; + buf_desc[0].mem_handle = buf0_mem_handle; + buf_desc[0].offset = buf0_offset; + + buf_desc[1].size = 324; + if (io_mem_handle != 0) { + buf_desc[1].length = 228; // 0 works here too + buf_desc[1].offset = 0x60; + } else { + buf_desc[1].length = 324; + } + buf_desc[1].type = CAM_CMD_BUF_GENERIC; + buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; + uint32_t *buf2 = (uint32_t *)alloc(s->video0_fd, buf_desc[1].size, 0x20, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[1].mem_handle); + + // cam_isp_packet_generic_blob_handler + uint32_t tmp[] = { + // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG) + 0x2000, + 0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0 + // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks + 0x3801, + 0x1, 0x4, // Dual mode, 4 RDI wires + 0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk? + // offset 0x60 + // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth + 0xe002, + 0x1, 0x4, // 4 RDI + 0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote + 0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote + 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + memcpy(buf2, tmp, sizeof(tmp)); + + if (io_mem_handle != 0) { + io_cfg[0].mem_handle[0] = io_mem_handle; + io_cfg[0].planes[0] = (struct cam_plane_cfg){ + .width = FRAME_WIDTH, + .height = FRAME_HEIGHT, + .plane_stride = FRAME_STRIDE, + .slice_height = FRAME_HEIGHT, + .meta_stride = 0x0, + .meta_size = 0x0, + .meta_offset = 0x0, + .packer_config = 0x0, + .mode_config = 0x0, + .tile_config = 0x0, + .h_init = 0x0, + .v_init = 0x0, + }; + io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; + io_cfg[0].color_pattern = 0x5; + io_cfg[0].bpp = 0xc; + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; + io_cfg[0].fence = fence; + io_cfg[0].direction = CAM_BUF_OUTPUT; + io_cfg[0].subsample_pattern = 0x1; + io_cfg[0].framedrop_pattern = 0x1; + } + + struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->isp_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + if (ret != 0) { + printf("ISP CONFIG FAILED\n"); + } + + munmap(buf2, buf_desc[1].size); + release_fd(s->video0_fd, buf_desc[1].mem_handle); + // release_fd(s->video0_fd, buf_desc[0].mem_handle); + munmap(pkt, size); + release_fd(s->video0_fd, cam_packet_handle); +} + +void enqueue_buffer(struct CameraState *s, int i, bool dp) { + int ret; + int request_id = s->request_ids[i]; + + if (s->buf_handle[i]) { + release(s->video0_fd, s->buf_handle[i]); + // wait + struct cam_sync_wait sync_wait = {0}; + sync_wait.sync_obj = s->sync_objs[i]; + sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 + ret = cam_control(s->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj); + + s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof + if (dp) s->buf.queue(i); + + // destroy old output fence + struct cam_sync_info sync_destroy = {0}; + strcpy(sync_destroy.name, "NodeOutputPortFence"); + sync_destroy.sync_obj = s->sync_objs[i]; + ret = cam_control(s->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj); + } + + // do stuff + struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; + req_mgr_sched_request.session_hdl = s->session_handle; + req_mgr_sched_request.link_hdl = s->link_handle; + req_mgr_sched_request.req_id = request_id; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); + // LOGD("sched req: %d %d", ret, request_id); + + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = cam_control(s->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + // LOGD("fence req: %d %d", ret, sync_create.sync_obj); + s->sync_objs[i] = sync_create.sync_obj; + + // configure ISP to put the image in place + struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0}; + mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu; + mem_mgr_map_cmd.num_hdl = 1; + mem_mgr_map_cmd.flags = 1; + mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd)); + // LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + + // poke sensor + sensors_poke(s, request_id); + // LOGD("Poked sensor"); + + // push the buffer + config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1)); +} + +void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) { + for (int i=start;irequest_ids[(i - 1) % FRAME_BUF_COUNT] = i; + enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp); + } +} + +// ******************* camera ******************* + +static void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { + LOGD("camera init %d", camera_num); + + assert(camera_id < ARRAYSIZE(cameras_supported)); + s->ci = cameras_supported[camera_id]; + assert(s->ci.frame_width != 0); + + s->camera_num = camera_num; + + s->dc_gain_enabled = false; + s->analog_gain = 0x5; + s->analog_gain_frac = sensor_analog_gains[s->analog_gain]; + s->exposure_time = 256; + s->exposure_time_max = 1.2 * EXPOSURE_TIME_MAX / 2; + s->exposure_time_min = 0.75 * EXPOSURE_TIME_MIN * 2; + s->request_id_last = 0; + s->skipped = true; + s->ef_filtered = 1.0; + + s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type); +} + +// TODO: refactor this to somewhere nicer, perhaps use in camera_qcom as well +static int open_v4l_by_name_and_index(const char name[], int index, int flags) { + char nbuf[0x100]; + int v4l_index = 0; + int cnt_index = index; + while (1) { + snprintf(nbuf, sizeof(nbuf), "/sys/class/video4linux/v4l-subdev%d/name", v4l_index); + FILE *f = fopen(nbuf, "rb"); + if (f == NULL) return -1; + int len = fread(nbuf, 1, sizeof(nbuf), f); + fclose(f); + + // name ends with '\n', remove it + if (len < 1) return -1; + nbuf[len-1] = '\0'; + + if (strcmp(nbuf, name) == 0) { + if (cnt_index == 0) { + snprintf(nbuf, sizeof(nbuf), "/dev/v4l-subdev%d", v4l_index); + LOGD("open %s for %s index %d", nbuf, name, index); + return open(nbuf, flags); + } + cnt_index--; + } + v4l_index++; + } +} + +static void camera_open(CameraState *s) { + int ret; + s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num, O_RDWR | O_NONBLOCK); + assert(s->sensor_fd >= 0); + LOGD("opened sensor"); + + s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num, O_RDWR | O_NONBLOCK); + assert(s->csiphy_fd >= 0); + LOGD("opened csiphy"); + + // probe the sensor + LOGD("-- Probing sensor %d", s->camera_num); + sensors_init(s->video0_fd, s->sensor_fd, s->camera_num); + + memset(&s->req_mgr_session_info, 0, sizeof(s->req_mgr_session_info)); + ret = cam_control(s->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl); + s->session_handle = s->req_mgr_session_info.session_hdl; + // access the sensor + LOGD("-- Accessing sensor"); + static struct cam_acquire_dev_cmd acquire_dev_cmd = {0}; + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + ret = cam_control(s->sensor_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + LOGD("acquire sensor dev: %d", ret); + s->sensor_dev_handle = acquire_dev_cmd.dev_handle; + + static struct cam_isp_resource isp_resource = {0}; + + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + acquire_dev_cmd.num_resources = 1; + acquire_dev_cmd.resource_hdl = (uint64_t)&isp_resource; + + isp_resource.resource_id = CAM_ISP_RES_ID_PORT; + isp_resource.length = sizeof(struct cam_isp_in_port_info) + sizeof(struct cam_isp_out_port_info)*(1-1); + isp_resource.handle_type = CAM_HANDLE_USER_POINTER; + + struct cam_isp_in_port_info *in_port_info = (struct cam_isp_in_port_info *)malloc(isp_resource.length); + isp_resource.res_hdl = (uint64_t)in_port_info; + + switch (s->camera_num) { + case 0: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_0; + break; + case 1: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_1; + break; + case 2: + in_port_info->res_type = CAM_ISP_IFE_IN_RES_PHY_2; + break; + } + + in_port_info->lane_type = CAM_ISP_LANE_TYPE_DPHY; + in_port_info->lane_num = 4; + in_port_info->lane_cfg = 0x3210; + + in_port_info->vc = 0x0; + //in_port_info->dt = 0x2C; //CSI_RAW12 + //in_port_info->format = CAM_FORMAT_MIPI_RAW_12; + + in_port_info->dt = 0x2B; //CSI_RAW10 + in_port_info->format = CAM_FORMAT_MIPI_RAW_10; + + in_port_info->test_pattern = 0x2; // 0x3? + in_port_info->usage_type = 0x0; + + in_port_info->left_start = 0x0; + in_port_info->left_stop = FRAME_WIDTH - 1; + in_port_info->left_width = FRAME_WIDTH; + + in_port_info->right_start = 0x0; + in_port_info->right_stop = FRAME_WIDTH - 1; + in_port_info->right_width = FRAME_WIDTH; + + in_port_info->line_start = 0x0; + in_port_info->line_stop = FRAME_HEIGHT - 1; + in_port_info->height = FRAME_HEIGHT; + + in_port_info->pixel_clk = 0x0; + in_port_info->batch_size = 0x0; + in_port_info->dsp_mode = 0x0; + in_port_info->hbi_cnt = 0x0; + in_port_info->custom_csid = 0x0; + + in_port_info->num_out_res = 0x1; + in_port_info->data[0] = (struct cam_isp_out_port_info){ + .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, + //.format = CAM_FORMAT_MIPI_RAW_12, + .format = CAM_FORMAT_MIPI_RAW_10, + .width = FRAME_WIDTH, + .height = FRAME_HEIGHT, + .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, + }; + + ret = cam_control(s->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + LOGD("acquire isp dev: %d", ret); + free(in_port_info); + s->isp_dev_handle = acquire_dev_cmd.dev_handle; + + static struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {0}; + csiphy_acquire_dev_info.combo_mode = 0; + + acquire_dev_cmd.session_handle = s->session_handle; + acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER; + acquire_dev_cmd.num_resources = 1; + acquire_dev_cmd.resource_hdl = (uint64_t)&csiphy_acquire_dev_info; + + ret = cam_control(s->csiphy_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd)); + + LOGD("acquire csiphy dev: %d", ret); + s->csiphy_dev_handle = acquire_dev_cmd.dev_handle; + + // acquires done + + // config ISP + alloc_w_mmu_hdl(s->video0_fd, 984480, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&s->buf0_handle, s->device_iommu, s->cdm_iommu); + config_isp(s, 0, 0, 1, s->buf0_handle, 0); + + LOG("-- Configuring sensor"); + sensors_i2c(s, init_array_ar0231, sizeof(init_array_ar0231)/sizeof(struct i2c_random_wr_payload), + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + //sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload), + //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON); + //sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload), + //CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF); + + // config csiphy + LOG("-- Config CSI PHY"); + { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + struct cam_packet *pkt = (struct cam_packet *)alloc(s->video0_fd, size, 8, + CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); + buf_desc[0].type = CAM_CMD_BUF_GENERIC; + + struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc(s->video0_fd, buf_desc[0].size, 8, CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, (uint32_t*)&buf_desc[0].mem_handle); + csiphy_info->lane_mask = 0x1f; + csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? + csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane + csiphy_info->combo_mode = 0x0; + csiphy_info->lane_cnt = 0x4; + csiphy_info->secure_mode = 0x0; + csiphy_info->settle_time = 2800000000; + csiphy_info->data_rate = 44000000; + + static struct cam_config_dev_cmd config_dev_cmd = {}; + config_dev_cmd.session_handle = s->session_handle; + config_dev_cmd.dev_handle = s->csiphy_dev_handle; + config_dev_cmd.offset = 0; + config_dev_cmd.packet_handle = cam_packet_handle; + + int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd)); + assert(ret == 0); + + release(s->video0_fd, buf_desc[0].mem_handle); + release(s->video0_fd, cam_packet_handle); + } + + // link devices + LOG("-- Link devices"); + static struct cam_req_mgr_link_info req_mgr_link_info = {0}; + req_mgr_link_info.session_hdl = s->session_handle; + req_mgr_link_info.num_devices = 2; + req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle; + req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); + LOGD("link: %d", ret); + s->link_handle = req_mgr_link_info.link_hdl; + + static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = 0; + req_mgr_link_control.session_hdl = s->session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control: %d", ret); + + LOGD("start csiphy: %d", ret); + ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("start isp: %d", ret); + ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle); + LOGD("start sensor: %d", ret); + ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle); + + enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0); +} + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { + camera_init(v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, + VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right + printf("road camera initted \n"); + camera_init(v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, + VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE); + printf("wide road camera initted \n"); + camera_init(v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx, + VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT); + printf("driver camera initted \n"); + + s->sm = new SubMaster({"driverState"}); + s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); +} + +void cameras_open(MultiCameraState *s) { + int ret; + + LOG("-- Opening devices"); + // video0 is req_mgr, the target of many ioctls + s->video0_fd = open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK); + assert(s->video0_fd >= 0); + LOGD("opened video0"); + s->road_cam.video0_fd = s->driver_cam.video0_fd = s->wide_road_cam.video0_fd = s->video0_fd; + + // video1 is cam_sync, the target of some ioctls + s->video1_fd = open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK); + assert(s->video1_fd >= 0); + LOGD("opened video1"); + s->road_cam.video1_fd = s->driver_cam.video1_fd = s->wide_road_cam.video1_fd = s->video1_fd; + + // looks like there's only one of these + s->isp_fd = open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK); + assert(s->isp_fd >= 0); + LOGD("opened isp"); + s->road_cam.isp_fd = s->driver_cam.isp_fd = s->wide_road_cam.isp_fd = s->isp_fd; + + // query icp for MMU handles + LOG("-- Query ICP for MMU handles"); + static struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; + static struct cam_query_cap_cmd query_cap_cmd = {0}; + query_cap_cmd.handle_type = 1; + query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; + query_cap_cmd.size = sizeof(isp_query_cap_cmd); + ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); + assert(ret == 0); + LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); + LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); + int device_iommu = isp_query_cap_cmd.device_iommu.non_secure; + int cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; + s->road_cam.device_iommu = s->driver_cam.device_iommu = s->wide_road_cam.device_iommu = device_iommu; + s->road_cam.cdm_iommu = s->driver_cam.cdm_iommu = s->wide_road_cam.cdm_iommu = cdm_iommu; + + // subscribe + LOG("-- Subscribing"); + static struct v4l2_event_subscription sub = {0}; + sub.type = 0x8000000; + sub.id = 2; // should use boot time for sof + ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); + printf("req mgr subscribe: %d\n", ret); + + camera_open(&s->road_cam); + printf("road camera opened \n"); + camera_open(&s->wide_road_cam); + printf("wide road camera opened \n"); + camera_open(&s->driver_cam); + printf("driver camera opened \n"); +} + +static void camera_close(CameraState *s) { + int ret; + + // stop devices + LOG("-- Stop devices"); + // ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle); + // LOGD("stop sensor: %d", ret); + ret = device_control(s->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle); + LOGD("stop isp: %d", ret); + ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("stop csiphy: %d", ret); + // link control stop + LOG("-- Stop link control"); + static struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = 1; + req_mgr_link_control.session_hdl = s->session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control stop: %d", ret); + + // unlink + LOG("-- Unlink"); + static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; + req_mgr_unlink_info.session_hdl = s->session_handle; + req_mgr_unlink_info.link_hdl = s->link_handle; + ret = cam_control(s->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); + LOGD("unlink: %d", ret); + + // release devices + LOGD("-- Release devices"); + ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle); + LOGD("release sensor: %d", ret); + ret = device_control(s->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle); + LOGD("release isp: %d", ret); + ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle); + LOGD("release csiphy: %d", ret); + + ret = cam_control(s->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info)); + LOGD("destroyed session: %d", ret); +} + +static void cameras_close(MultiCameraState *s) { + camera_close(&s->road_cam); + camera_close(&s->wide_road_cam); + camera_close(&s->driver_cam); + + delete s->sm; + delete s->pm; +} + +// ******************* just a helper ******************* + +void handle_camera_event(CameraState *s, void *evdat) { + struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; + + uint64_t timestamp = event_data->u.frame_msg.timestamp; + int main_id = event_data->u.frame_msg.frame_id; + int real_id = event_data->u.frame_msg.request_id; + + if (real_id != 0) { // next ready + if (real_id == 1) {s->idx_offset = main_id;} + int buf_idx = (real_id - 1) % FRAME_BUF_COUNT; + + // check for skipped frames + if (main_id > s->frame_id_last + 1 && !s->skipped) { + // realign + clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); + enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0); + s->skipped = true; + } else if (main_id == s->frame_id_last + 1) { + s->skipped = false; + } + + // check for dropped requests + if (real_id > s->request_id_last + 1) { + enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0); + } + + // metas + s->frame_id_last = main_id; + s->request_id_last = real_id; + s->buf.camera_bufs_metadata[buf_idx].frame_id = main_id - s->idx_offset; + s->buf.camera_bufs_metadata[buf_idx].timestamp_sof = timestamp; + s->buf.camera_bufs_metadata[buf_idx].global_gain = s->analog_gain + (100*s->dc_gain_enabled); + s->buf.camera_bufs_metadata[buf_idx].gain_frac = s->analog_gain_frac; + s->buf.camera_bufs_metadata[buf_idx].integ_lines = s->exposure_time; + + // dispatch + enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1); + } else { // not ready + // reset after half second of no response + if (main_id > s->frame_id_last + 10) { + clear_req_queue(s->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl); + enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0); + s->frame_id_last = main_id; + s->skipped = true; + } + } +} + +// ******************* exposure control helpers ******************* + +void set_exposure_time_bounds(CameraState *s) { + switch (s->analog_gain) { + case 0: { + s->exposure_time_min = EXPOSURE_TIME_MIN; + s->exposure_time_max = EXPOSURE_TIME_MAX; // EXPOSURE_TIME_MIN * 4; + break; + } + case ANALOG_GAIN_MAX_IDX - 1: { + s->exposure_time_min = EXPOSURE_TIME_MIN; // EXPOSURE_TIME_MAX / 4; + s->exposure_time_max = EXPOSURE_TIME_MAX; + break; + } + default: { + // finetune margins on both ends + float k_up = sensor_analog_gains[s->analog_gain+1] / sensor_analog_gains[s->analog_gain]; + float k_down = sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain]; + s->exposure_time_min = k_down * EXPOSURE_TIME_MIN * 2; + s->exposure_time_max = k_up * EXPOSURE_TIME_MAX / 2; + } + } +} + +void switch_conversion_gain(CameraState *s) { + if (!s->dc_gain_enabled) { + s->dc_gain_enabled = true; + s->analog_gain -= 4; + } else { + s->dc_gain_enabled = false; + s->analog_gain += 4; + } +} + +static void set_camera_exposure(CameraState *s, float grey_frac) { + // TODO: get stats from sensor? + float target_grey = 0.4 - ((float)(s->analog_gain + 4*s->dc_gain_enabled) / 48.0f); + float exposure_factor = 1 + 30 * pow((target_grey - grey_frac), 3); + exposure_factor = std::max(exposure_factor, 0.4f); + + if (s->camera_num != 1) { + s->ef_filtered = (1 - EF_LOWPASS_K) * s->ef_filtered + EF_LOWPASS_K * exposure_factor; + exposure_factor = s->ef_filtered; + } + + // always prioritize exposure time adjust + s->exposure_time *= exposure_factor; + + // switch gain if max/min exposure time is reached + // or always switch down to a lower gain when possible + bool kd = false; + if (s->analog_gain > 0) { + kd = 1.1 * s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain]) < EXPOSURE_TIME_MAX / 2; + } + + if (s->exposure_time > s->exposure_time_max) { + if (s->analog_gain < ANALOG_GAIN_MAX_IDX - 1) { + s->exposure_time = EXPOSURE_TIME_MAX / 2; + s->analog_gain += 1; + if (!s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] >= 4.0) { // switch to HCG + switch_conversion_gain(s); + } + set_exposure_time_bounds(s); + } else { + s->exposure_time = s->exposure_time_max; + } + } else if (s->exposure_time < s->exposure_time_min || kd) { + if (s->analog_gain > 0) { + s->exposure_time = std::max(EXPOSURE_TIME_MIN * 2, (int)(s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain]))); + s->analog_gain -= 1; + if (s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] <= 1.25) { // switch back to LCG + switch_conversion_gain(s); + } + set_exposure_time_bounds(s); + } else { + s->exposure_time = s->exposure_time_min; + } + } + + // set up config + uint16_t AG = s->analog_gain + 4; + AG = 0xFF00 + AG * 16 + AG; + s->analog_gain_frac = sensor_analog_gains[s->analog_gain]; + + // printf("cam %d, min %d, max %d \n", s->camera_num, s->exposure_time_min, s->exposure_time_max); + // printf("cam %d, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, AG, s->exposure_time, s->dc_gain_enabled); + + struct i2c_random_wr_payload exp_reg_array[] = { + {0x3366, AG}, // analog gain + {0x3362, (uint16_t)(s->dc_gain_enabled?0x1:0x0)}, // DC_GAIN + {0x305A, 0x00D8}, // red gain + {0x3058, 0x011B}, // blue gain + {0x3056, 0x009A}, // g1 gain + {0x305C, 0x009A}, // g2 gain + {0x3012, (uint16_t)s->exposure_time}, // integ time + }; + //{0x301A, 0x091C}}; // reset + sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), + CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); +} + +void camera_autoexposure(CameraState *s, float grey_frac) { + CameraExpInfo tmp = cam_exp[s->camera_num].load(); + tmp.op_id++; + tmp.grey_frac = grey_frac; + cam_exp[s->camera_num].store(tmp); +} + +static void ae_thread(MultiCameraState *s) { + CameraState *c_handles[3] = {&s->wide_road_cam, &s->road_cam, &s->driver_cam}; + + int op_id_last[3] = {0}; + CameraExpInfo cam_op[3]; + + set_thread_name("camera_settings"); + + while(!do_exit) { + for (int i=0;i<3;i++) { + cam_op[i] = cam_exp[i].load(); + if (cam_op[i].op_id != op_id_last[i]) { + set_camera_exposure(c_handles[i], cam_op[i].grey_frac); + op_id_last[i] = cam_op[i].op_id; + } + } + + util::sleep_for(50); + } +} + +void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { + common_process_driver_camera(s->sm, s->pm, c, cnt); +} + +// called by processing_thread +void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { + const CameraBuf *b = &c->buf; + + MessageBuilder msg; + auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState(); + fill_frame_data(framed, b->cur_frame_data); + if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) { + framed.setImage(get_frame_image(b)); + } + if (c == &s->road_cam) { + framed.setTransform(b->yuv_transform.v); + } + s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); + + if (cnt % 3 == 0) { + const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); + const int skip = 2; + set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, x, x + w, skip, y, y + h, skip); + } +} + +void cameras_run(MultiCameraState *s) { + LOG("-- Starting threads"); + std::vector threads; + threads.push_back(std::thread(ae_thread, s)); + threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); + threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + + // start devices + LOG("-- Starting devices"); + int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload); + sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + + // poll events + LOG("-- Dequeueing Video events"); + while (!do_exit) { + struct pollfd fds[1] = {{0}}; + + fds[0].fd = s->video0_fd; + fds[0].events = POLLPRI; + + int ret = poll(fds, ARRAYSIZE(fds), 1000); + if (ret < 0) { + if (errno == EINTR || errno == EAGAIN) continue; + LOGE("poll failed (%d - %d)", ret, errno); + break; + } + + if (!fds[0].revents) continue; + + struct v4l2_event ev = {0}; + ret = ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev); + if (ev.type == 0x8000000) { + struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data; + // LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + // printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + + if (event_data->session_hdl == s->road_cam.req_mgr_session_info.session_hdl) { + handle_camera_event(&s->road_cam, event_data); + } else if (event_data->session_hdl == s->wide_road_cam.req_mgr_session_info.session_hdl) { + handle_camera_event(&s->wide_road_cam, event_data); + } else if (event_data->session_hdl == s->driver_cam.req_mgr_session_info.session_hdl) { + handle_camera_event(&s->driver_cam, event_data); + } else { + printf("Unknown vidioc event source\n"); + assert(false); + } + } + } + + LOG(" ************** STOPPING **************"); + + for (auto &t : threads) t.join(); + + cameras_close(s); +} diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h new file mode 100644 index 000000000..104ba49d8 --- /dev/null +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include + +#include "camera_common.h" +#include "media/cam_req_mgr.h" + +#define FRAME_BUF_COUNT 4 + +#define ANALOG_GAIN_MAX_IDX 10 // 0xF is bypass +#define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss +#define EXPOSURE_TIME_MAX 1757 // with HDR, slowest ss + +#define HLC_THRESH 222 +#define HLC_A 80 +#define HISTO_CEIL_K 5 + +#define EF_LOWPASS_K 0.35 + +#define DEBAYER_LOCAL_WORKSIZE 16 + +typedef struct CameraState { + CameraInfo ci; + + float analog_gain_frac; + uint16_t analog_gain; + bool dc_gain_enabled; + int exposure_time; + int exposure_time_min; + int exposure_time_max; + float ef_filtered; + + int device_iommu; + int cdm_iommu; + + int video0_fd; + int video1_fd; + int isp_fd; + + int sensor_fd; + int csiphy_fd; + + int camera_num; + + + uint32_t session_handle; + + uint32_t sensor_dev_handle; + uint32_t isp_dev_handle; + uint32_t csiphy_dev_handle; + + int32_t link_handle; + + int buf0_handle; + int buf_handle[FRAME_BUF_COUNT]; + int sync_objs[FRAME_BUF_COUNT]; + int request_ids[FRAME_BUF_COUNT]; + int request_id_last; + int frame_id_last; + int idx_offset; + bool skipped; + + struct cam_req_mgr_session_info req_mgr_session_info; + + CameraBuf buf; +} CameraState; + +typedef struct MultiCameraState { + int device; + + int video0_fd; + int video1_fd; + int isp_fd; + + CameraState road_cam; + CameraState wide_road_cam; + CameraState driver_cam; + + pthread_mutex_t isp_lock; + + SubMaster *sm; + PubMaster *pm; +} MultiCameraState; + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_open(MultiCameraState *s); +void cameras_run(MultiCameraState *s); +void camera_autoexposure(CameraState *s, float grey_frac); diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h new file mode 100644 index 000000000..8abec280c --- /dev/null +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -0,0 +1,137 @@ +struct i2c_random_wr_payload start_reg_array[] = {{0x301a, 0x91c}}; +struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x918}};; + +struct i2c_random_wr_payload init_array_ar0231[] = { + {0x301A, 0x0018}, // RESET_REGISTER + {0x3092, 0x0C24}, // ROW_NOISE_CONTROL + {0x337A, 0x0C80}, // DBLC_SCALE0 + {0x3520, 0x1288}, // RESERVED_MFR_3520 + {0x3522, 0x880C}, // RESERVED_MFR_3522 + {0x3524, 0x0C12}, // RESERVED_MFR_3524 + {0x352C, 0x1212}, // RESERVED_MFR_352C + {0x354A, 0x007F}, // RESERVED_MFR_354A + {0x350C, 0x055C}, // RESERVED_MFR_350C + {0x3506, 0x3333}, // RESERVED_MFR_3506 + {0x3508, 0x3333}, // RESERVED_MFR_3508 + {0x3100, 0x4000}, // DLO_CONTROL0 + {0x3280, 0x0CCC}, // RESERVED_MFR_3280 + {0x3282, 0x0CCC}, // RESERVED_MFR_3282 + {0x3284, 0x0CCC}, // RESERVED_MFR_3284 + {0x3286, 0x0CCC}, // RESERVED_MFR_3286 + {0x3288, 0x0FA0}, // RESERVED_MFR_3288 + {0x328A, 0x0FA0}, // RESERVED_MFR_328A + {0x328C, 0x0FA0}, // RESERVED_MFR_328C + {0x328E, 0x0FA0}, // RESERVED_MFR_328E + {0x3290, 0x0FA0}, // RESERVED_MFR_3290 + {0x3292, 0x0FA0}, // RESERVED_MFR_3292 + {0x3294, 0x0FA0}, // RESERVED_MFR_3294 + {0x3296, 0x0FA0}, // RESERVED_MFR_3296 + {0x3298, 0x0FA0}, // RESERVED_MFR_3298 + {0x329A, 0x0FA0}, // RESERVED_MFR_329A + {0x329C, 0x0FA0}, // RESERVED_MFR_329C + {0x329E, 0x0FA0}, // RESERVED_MFR_329E + {0x32E6, 0x00E0}, // RESERVED_MFR_32E6 + {0x1008, 0x036F}, // RESERVED_PARAM_1008 + {0x100C, 0x058F}, // RESERVED_PARAM_100C + {0x100E, 0x07AF}, // RESERVED_PARAM_100E + {0x1010, 0x014F}, // RESERVED_PARAM_1010 + {0x3230, 0x0312}, // FINE_CORRECTION + {0x3232, 0x0532}, // FINE_CORRECTION2 + {0x3234, 0x0752}, // FINE_CORRECTION3 + {0x3236, 0x00F2}, // FINE_CORRECTION4 + {0x3566, 0x3328}, // RESERVED_MFR_3566 + {0x32D0, 0x3A02}, // RESERVED_MFR_32D0 + {0x32D2, 0x3508}, // RESERVED_MFR_32D2 + {0x32D4, 0x3702}, // RESERVED_MFR_32D4 + {0x32D6, 0x3C04}, // RESERVED_MFR_32D6 + {0x32DC, 0x370A}, // RESERVED_MFR_32DC + {0x30B0, 0x0800}, // DIGITAL_TEST + {0x302A, 0x0006}, // VT_PIX_CLK_DIV + {0x302C, 0x0001}, // VT_SYS_CLK_DIV + {0x302E, 0x0002}, // PRE_PLL_CLK_DIV + {0x3030, 0x0028}, // PLL_MULTIPLIER + {0x3036, 0x000A}, // OP_WORD_CLK_DIV + {0x3038, 0x0001}, // OP_SYS_CLK_DIV + {0x30B0, 0x0800}, // DIGITAL_TEST + {0x30A2, 0x0001}, // X_ODD_INC_ + {0x30A6, 0x0001}, // Y_ODD_INC_ + {0x3040, 0xC000}, // READ_MODE C000 + {0x30BA, 0x11F2}, // DIGITAL_CTRL + {0x3044, 0x0400}, // DARK_CONTROL + {0x3064, 0x1802}, // SMIA_TEST + {0x33E0, 0x0C80}, // TEST_ASIL_ROWS + {0x3180, 0x0080}, // RESERVED_MFR_3180 + {0x33E4, 0x0080}, // RESERVED_MFR_33E4 + {0x33E0, 0x0C80}, // TEST_ASIL_ROWS + {0x33E0, 0x0C80}, // TEST_ASIL_ROWS + {0x3004, 0x0000}, // X_ADDR_START_ + {0x3008, 0x0787}, // X_ADDR_END_ 787 + {0x3002, 0x0000}, // Y_ADDR_START_ + {0x3006, 0x04B7}, // Y_ADDR_END_ 4B7 + {0x3032, 0x0000}, // SCALING_MODE + {0x3400, 0x0010}, // RESERVED_MFR_3400 + {0x3402, 0x0788}, // X_OUTPUT_CONTROL + {0x3402, 0x0F10}, // X_OUTPUT_CONTROL + {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL + {0x3404, 0x0970}, // Y_OUTPUT_CONTROL + {0x30BA, 0x11F2}, // DIGITAL_CTRL + + // SLAV* MODE + {0x30CE, 0x0120}, + {0x340A, 0xE6}, // E6 // 0000 1110 0110 + {0x340C, 0x802}, // 2 // 0000 0000 0010 + + // Readout timing + {0x300C, 0x074B}, // LINE_LENGTH_PCK: min for 2-exposure HDR + {0x300A, 0x085E}, // FRAME_LENGTH_LINES_ 6EB + {0x3042, 0x0000}, // EXTRA_DELAY + + // Readout Settings + {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI + {0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10 + {0x3342, 0x122B}, // MIPI_F1_PDT_EDT + {0x3346, 0x122B}, // MIPI_F2_PDT_EDT + {0x334A, 0x122B}, // MIPI_F3_PDT_EDT + {0x334E, 0x122B}, // MIPI_F4_PDT_EDT + {0x3344, 0x0011}, // MIPI_F1_VDT_VC + {0x3348, 0x0111}, // MIPI_F2_VDT_VC + {0x334C, 0x0211}, // MIPI_F3_VDT_VC + {0x3350, 0x0311}, // MIPI_F4_VDT_VC + {0x31B0, 0x0049}, // FRAME_PREAMBLE + {0x31B2, 0x0033}, // LINE_PREAMBLE + {0x31B4, 0x2185}, // RESERVED_MFR_31B4 + {0x31B6, 0x1146}, // RESERVED_MFR_31B6 + {0x31B8, 0x3047}, // RESERVED_MFR_31B8 + {0x31BA, 0x0186}, // RESERVED_MFR_31BA + {0x31BC, 0x0805}, // RESERVED_MFR_31BC + {0x301A, 0x01C}, // RESET_REGISTER + + // HDR Settings + {0x3082, 0x0004}, // OPERATION_MODE_CTRL + {0x3238, 0x0004}, // EXPOSURE_RATIO + {0x3014, 0x098E}, // FINE_INTEGRATION_TIME_ + {0x321E, 0x098E}, // FINE_INTEGRATION_TIME2 + {0x30B0, 0x0800}, // DIGITAL_TEST + {0x32EA, 0x3C0E}, // RESERVED_MFR_32EA + {0x32EC, 0x72A1}, // RESERVED_MFR_32EC + {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? + {0x33DA, 0x0000}, // COMPANDING + {0x3370, 0x03B1}, // DBLC + {0x31E0, 0x0001}, // PDC + {0x318E, 0x0200}, // PRE_HDR_GAIN_EN + + // Initial Gains + {0x3022, 0x01}, // GROUPED_PARAMETER_HOLD_ + {0x3366, 0x5555}, // ANALOG_GAIN + {0x3060, 0x3333}, // ANALOG_COLOR_GAIN + {0x3362, 0x0000}, // DC GAIN + {0x305A, 0x00D8}, // RED_GAIN + {0x3058, 0x011B}, // BLUE_GAIN + {0x3056, 0x009A}, // GREEN1_GAIN + {0x305C, 0x009A}, // GREEN2_GAIN + {0x3022, 0x00}, // GROUPED_PARAMETER_HOLD_ + + // Initial Integration Time + {0x3012, 0x256}, +}; + diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index b6b948fe0..0c6d54d92 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -1,6 +1,5 @@ #include #include -#include #include #include #include @@ -19,304 +18,23 @@ #include #include "clutil.h" -#include "common/ipc.h" #include "common/params.h" #include "common/swaglog.h" #include "common/util.h" -#include "common/visionipc.h" +#include "visionipc_server.h" -#define MAX_CLIENTS 6 - -volatile sig_atomic_t do_exit = 0; - -static void set_do_exit(int sig) { - do_exit = 1; -} - -struct VisionState; - -struct VisionClientState { - VisionState *s; - int fd; - pthread_t thread_handle; - bool running; -}; - -struct VisionClientStreamState { - bool subscribed; - int bufs_outstanding; - bool tb; - TBuffer* tbuffer; - PoolQueue* queue; -}; - -struct VisionState { - MultiCameraState cameras; - pthread_mutex_t clients_lock; - VisionClientState clients[MAX_CLIENTS]; -}; - -static CameraBuf *get_camerabuf_by_type(VisionState *s, VisionStreamType type) { - assert(type >= 0 && type < VISION_STREAM_MAX); - if (type == VISION_STREAM_RGB_BACK || type == VISION_STREAM_YUV) { - return &s->cameras.rear.buf; - } else if (type == VISION_STREAM_RGB_FRONT || type == VISION_STREAM_YUV_FRONT) { - return &s->cameras.front.buf; - } -#ifdef QCOM2 - return &s->cameras.wide.buf; -#endif - assert(0); -} - -// visionserver -void* visionserver_client_thread(void* arg) { - int err; - VisionClientState *client = (VisionClientState*)arg; - VisionState *s = client->s; - int fd = client->fd; - - set_thread_name("clientthread"); - - VisionClientStreamState streams[VISION_STREAM_MAX] = {{0}}; - - LOGW("client start fd %d", fd); - - while (true) { - struct pollfd polls[1+VISION_STREAM_MAX] = {{0}}; - polls[0].fd = fd; - polls[0].events = POLLIN; - - int poll_to_stream[1+VISION_STREAM_MAX] = {0}; - int num_polls = 1; - for (int i=0; i= 2) { - continue; - } - if (streams[i].tb) { - polls[num_polls].fd = tbuffer_efd(streams[i].tbuffer); - } else { - polls[num_polls].fd = poolq_efd(streams[i].queue); - } - poll_to_stream[num_polls] = i; - num_polls++; - } - int ret = poll(polls, num_polls, -1); - if (ret < 0) { - if (errno == EINTR || errno == EAGAIN) continue; - LOGE("poll failed (%d - %d)", ret, errno); - break; - } - if (do_exit) break; - if (polls[0].revents) { - VisionPacket p; - err = vipc_recv(fd, &p); - if (err <= 0) { - break; - } else if (p.type == VIPC_STREAM_SUBSCRIBE) { - VisionStreamType stream_type = p.d.stream_sub.type; - VisionPacket rep = { - .type = VIPC_STREAM_BUFS, - .d = { .stream_bufs = { .type = stream_type }, }, - }; - - VisionClientStreamState *stream = &streams[stream_type]; - stream->tb = p.d.stream_sub.tbuffer; - - VisionStreamBufs *stream_bufs = &rep.d.stream_bufs; - CameraBuf *b = get_camerabuf_by_type(s, stream_type); - if (stream_type == VISION_STREAM_RGB_BACK || - stream_type == VISION_STREAM_RGB_FRONT || - stream_type == VISION_STREAM_RGB_WIDE) { - stream_bufs->width = b->rgb_width; - stream_bufs->height = b->rgb_height; - stream_bufs->stride = b->rgb_stride; - stream_bufs->buf_len = b->rgb_bufs[0].len; - rep.num_fds = UI_BUF_COUNT; - for (int i = 0; i < rep.num_fds; i++) { - rep.fds[i] = b->rgb_bufs[i].fd; - } - if (stream->tb) { - stream->tbuffer = &b->ui_tb; - } else { - assert(false); - } - } else { - stream_bufs->width = b->yuv_width; - stream_bufs->height = b->yuv_height; - stream_bufs->stride = b->yuv_width; - stream_bufs->buf_len = b->yuv_buf_size; - rep.num_fds = YUV_COUNT; - for (int i = 0; i < rep.num_fds; i++) { - rep.fds[i] = b->yuv_ion[i].fd; - } - if (stream->tb) { - stream->tbuffer = b->yuv_tb; - } else { - stream->queue = pool_get_queue(&b->yuv_pool); - } - } - vipc_send(fd, &rep); - streams[stream_type].subscribed = true; - } else if (p.type == VIPC_STREAM_RELEASE) { - int si = p.d.stream_rel.type; - assert(si < VISION_STREAM_MAX); - if (streams[si].tb) { - tbuffer_release(streams[si].tbuffer, p.d.stream_rel.idx); - } else { - poolq_release(streams[si].queue, p.d.stream_rel.idx); - } - streams[p.d.stream_rel.type].bufs_outstanding--; - } else { - assert(false); - } - } else { - int stream_i = VISION_STREAM_MAX; - for (int i=1; iyuv_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_sof = b->yuv_metas[idx].timestamp_sof; - rep.d.stream_acq.extra.timestamp_eof = b->yuv_metas[idx].timestamp_eof; - } - vipc_send(fd, &rep); - } - } - } - - LOGW("client end fd %d", fd); - - for (int i=0; iclients_lock); - client->running = false; - pthread_mutex_unlock(&s->clients_lock); - - return NULL; -} - -void* visionserver_thread(void* arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("visionserver"); - - int sock = ipc_bind(VIPC_SOCKET_PATH); - while (!do_exit) { - struct pollfd polls[1] = {{0}}; - polls[0].fd = sock; - polls[0].events = POLLIN; - - int ret = poll(polls, ARRAYSIZE(polls), 1000); - if (ret < 0) { - if (errno == EINTR || errno == EAGAIN) continue; - LOGE("poll failed (%d - %d)", ret, errno); - break; - } - if (do_exit) break; - if (!polls[0].revents) { - continue; - } - - int fd = accept(sock, NULL, NULL); - assert(fd >= 0); - - pthread_mutex_lock(&s->clients_lock); - - int client_idx = 0; - for (; client_idx < MAX_CLIENTS; client_idx++) { - if (!s->clients[client_idx].running) break; - } - - if (client_idx >= MAX_CLIENTS) { - LOG("ignoring visionserver connection, max clients connected"); - close(fd); - - pthread_mutex_unlock(&s->clients_lock); - continue; - } - - VisionClientState *client = &s->clients[client_idx]; - client->s = s; - client->fd = fd; - client->running = true; - - err = pthread_create(&client->thread_handle, NULL, - visionserver_client_thread, client); - assert(err == 0); - - pthread_mutex_unlock(&s->clients_lock); - } - - for (int i=0; iclients_lock); - bool running = s->clients[i].running; - pthread_mutex_unlock(&s->clients_lock); - if (running) { - err = pthread_join(s->clients[i].thread_handle, NULL); - assert(err == 0); - } - } - - close(sock); - - return NULL; -} +ExitHandler do_exit; void party(cl_device_id device_id, cl_context context) { - VisionState state = {}; - VisionState *s = &state; + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); - cameras_init(&s->cameras, device_id, context); - cameras_open(&s->cameras); + cameras_init(&vipc_server, &cameras, device_id, context); + cameras_open(&cameras); - std::thread server_thread(visionserver_thread, s); + vipc_server.start_listener(); - // priority for cameras - int err = set_realtime_priority(53); - LOG("setpriority returns %d", err); - - cameras_run(&s->cameras); - - server_thread.join(); + cameras_run(&cameras); } #ifdef QCOM @@ -331,9 +49,6 @@ int main(int argc, char *argv[]) { set_core_affinity(6); #endif - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); - cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); // TODO: do this for QCOM2 too diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index addf293cc..60883608f 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -3,10 +3,15 @@ import os import signal import subprocess import time + +import numpy as np from PIL import Image + +import cereal.messaging as messaging from common.basedir import BASEDIR from common.params import Params -from selfdrive.camerad.snapshot.visionipc import VisionIPC +from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size +from selfdrive.hardware import TICI from selfdrive.controls.lib.alertmanager import set_offroad_alert @@ -15,58 +20,79 @@ def jpeg_write(fn, dat): img.save(fn, "JPEG") +def extract_image(dat, frame_sizes): + img = np.frombuffer(dat, dtype=np.uint8) + w, h = frame_sizes[len(img) // 3] + b = img[::3].reshape(h, w) + g = img[1::3].reshape(h, w) + r = img[2::3].reshape(h, w) + return np.dstack([r, g, b]) + + +def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"): + frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size] + frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes} + + sm = messaging.SubMaster([frame, front_frame]) + while min(sm.logMonoTime.values()) == 0: + sm.update() + + rear = extract_image(sm[frame].image, frame_sizes) + front = extract_image(sm[front_frame].image, frame_sizes) + return rear, front + + def snapshot(): params = Params() front_camera_allowed = int(params.get("RecordFront")) if params.get("IsOffroad") != b"1" or params.get("IsTakingSnapshot") == b"1": - return None + print("Already taking snapshot") + return None, None params.put("IsTakingSnapshot", "1") set_offroad_alert("Offroad_IsTakingSnapshot", True) time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start # Check if camerad is already started - ps = subprocess.Popen("ps | grep camerad", shell=True, stdout=subprocess.PIPE) - ret = list(filter(lambda x: 'grep ' not in x, ps.communicate()[0].decode('utf-8').strip().split("\n"))) - if len(ret) > 0: + try: + subprocess.check_call(["pgrep", "camerad"]) + + print("Camerad already running") params.put("IsTakingSnapshot", "0") params.delete("Offroad_IsTakingSnapshot") - return None + return None, None + except subprocess.CalledProcessError: + pass - proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad")) + env = os.environ.copy() + env["SEND_ROAD"] = "1" + env["SEND_WIDE_ROAD"] = "1" + env["SEND_DRIVER"] = "1" + proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), + cwd=os.path.join(BASEDIR, "selfdrive/camerad"), env=env) time.sleep(3.0) - ret = None - start_time = time.time() - while time.time() - start_time < 5.0: - try: - ipc = VisionIPC() - pic = ipc.get() - del ipc - - if front_camera_allowed: - ipc_front = VisionIPC(front=True) - fpic = ipc_front.get() - del ipc_front - else: - fpic = None - - ret = pic, fpic - break - except Exception: - time.sleep(1) + frame = "wideRoadCameraState" if TICI else "roadCameraState" + rear, front = get_snapshots(frame) proc.send_signal(signal.SIGINT) proc.communicate() params.put("IsTakingSnapshot", "0") set_offroad_alert("Offroad_IsTakingSnapshot", False) - return ret + + if not front_camera_allowed: + front = None + + return rear, front if __name__ == "__main__": pic, fpic = snapshot() - print(pic.shape) - jpeg_write("/tmp/back.jpg", pic) - jpeg_write("/tmp/front.jpg", fpic) + if pic is not None: + print(pic.shape) + jpeg_write("/tmp/back.jpg", pic) + jpeg_write("/tmp/front.jpg", fpic) + else: + print("Error taking snapshot") diff --git a/selfdrive/camerad/snapshot/visionipc.py b/selfdrive/camerad/snapshot/visionipc.py deleted file mode 100644 index 4df79e2cc..000000000 --- a/selfdrive/camerad/snapshot/visionipc.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python3 -import os -from cffi import FFI - -import numpy as np - -gf_dir = os.path.dirname(os.path.abspath(__file__)) - -ffi = FFI() -ffi.cdef(""" - -typedef enum VisionStreamType { - VISION_STREAM_RGB_BACK, - VISION_STREAM_RGB_FRONT, - VISION_STREAM_YUV, - VISION_STREAM_YUV_FRONT, - VISION_STREAM_MAX, -} VisionStreamType; - -typedef struct VisionUIInfo { - int big_box_x, big_box_y; - int big_box_width, big_box_height; - int transformed_width, transformed_height; - - int front_box_x, front_box_y; - int front_box_width, front_box_height; -} VisionUIInfo; - -typedef struct VisionStreamBufs { - VisionStreamType type; - - int width, height, stride; - size_t buf_len; - - union { - VisionUIInfo ui_info; - } buf_info; -} VisionStreamBufs; - -typedef struct VIPCBuf { - int fd; - size_t len; - void* addr; -} VIPCBuf; - -typedef struct VIPCBufExtra { - // only for yuv - uint32_t frame_id; - uint64_t timestamp_eof; -} VIPCBufExtra; - -typedef struct VisionStream { - int ipc_fd; - int last_idx; - int last_type; - int num_bufs; - VisionStreamBufs bufs_info; - VIPCBuf *bufs; -} VisionStream; - -int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); -VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra); -void visionstream_destroy(VisionStream *s); - -""" -) - - -class VisionIPCError(Exception): - pass - - -class VisionIPC(): - def __init__(self, front=False): - self.clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) - - self.s = ffi.new("VisionStream*") - self.buf_info = ffi.new("VisionStreamBufs*") - - err = self.clib.visionstream_init(self.s, self.clib.VISION_STREAM_RGB_FRONT if front else self.clib.VISION_STREAM_RGB_BACK, True, self.buf_info) - - if err != 0: - self.clib.visionstream_destroy(self.s) - raise VisionIPCError - - def __del__(self): - self.clib.visionstream_destroy(self.s) - - def get(self): - buf = self.clib.visionstream_get(self.s, ffi.NULL) - pbuf = ffi.buffer(buf.addr, buf.len) - ret = np.frombuffer(pbuf, dtype=np.uint8).reshape((-1, self.buf_info.stride//3, 3)) - return ret[:self.buf_info.height, :self.buf_info.width, [2, 1, 0]] diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index b81526e9c..3ab2ff842 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,7 +1,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ create_wheel_buttons -from selfdrive.car.chrysler.values import CAR, SteerLimitParams +from selfdrive.car.chrysler.values import CAR, CarControllerParams from opendbc.can.packer import CANPacker class CarController(): @@ -24,9 +24,9 @@ class CarController(): # *** compute control surfaces *** # steer torque - new_steer = actuators.steer * SteerLimitParams.STEER_MAX + new_steer = actuators.steer * CarControllerParams.STEER_MAX apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorqueEps, SteerLimitParams) + CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index b6fdb1131..2d3c1a2ec 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -42,13 +42,15 @@ class CarState(CarStateBase): ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 - ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE'] - ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE'] + ret.steeringAngleDeg = cp.vl["STEERING"]['STEER_ANGLE'] + ret.steeringRateDeg = cp.vl["STEERING"]['STEERING_RATE'] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None)) ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green. ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled ret.cruiseState.speed = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH'] * CV.KPH_TO_MS + # CRUISE_STATE is a three bit msg, 0 is off, 1 and 2 are Non-ACC mode, 3 and 4 are ACC mode, find if there are other states too + ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]['CRUISE_STATE'] in [1, 2] ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"] ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"] @@ -87,6 +89,7 @@ class CarState(CarStateBase): ("ACC_STATUS_2", "ACC_2", 0), ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), + ("CRUISE_STATE", "DASHBOARD", 0), ("TORQUE_DRIVER", "EPS_STATUS", 0), ("TORQUE_MOTOR", "EPS_STATUS", 0), ("LKAS_STATE", "EPS_STATUS", 1), diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 88110f053..6cb3ff8bb 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -4,7 +4,7 @@ from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu -class SteerLimitParams: +class CarControllerParams: STEER_MAX = 261 # 262 faults STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems STEER_DELTA_DOWN = 3 # no faults on the way down it seems @@ -33,7 +33,7 @@ class CAR: FINGERPRINTS = { CAR.PACIFICA_2017_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: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 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: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 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, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 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, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 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: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 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, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8}, ], 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, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 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, 656: 4, 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, 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, 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, 1537: 8, 1538: 8, 1562: 8}, @@ -41,7 +41,7 @@ FINGERPRINTS = { ], CAR.PACIFICA_2020: [ { - 55: 8, 179: 8, 181: 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: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 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, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2016: 8, 2024: 8 + 55: 8, 179: 8, 181: 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, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 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, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8 } ], CAR.PACIFICA_2018_HYBRID: [ @@ -64,7 +64,7 @@ FINGERPRINTS = { }, # Based on "8190c7275a24557b|2020-02-24--09-57-23" { - 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 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, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 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, 886: 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, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 } ], CAR.JEEP_CHEROKEE: [ diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 648fa0774..41ce2fd8a 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -33,7 +33,7 @@ class CarController(): if (frame % 3) == 0: - curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.out.vEgo) + curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*3.1415/180., CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: @@ -43,7 +43,7 @@ class CarController(): self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append(create_steer_command(self.packer, apply_steer, enabled, - CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action)) + CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0: diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 7f34acc9d..2a776b45d 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -17,7 +17,7 @@ class CarState(CarStateBase): ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl]) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = not ret.vEgoRaw > 0.001 - ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] + ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1 ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 29d5db5a9..2584618eb 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -128,7 +128,7 @@ def match_fw_to_car(fw_versions): continue # TODO: on some toyota, the engine can show on two different addresses - if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS] and found_version is None: + if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR, TOYOTA.LEXUS_IS, TOYOTA.AVALON] and found_version is None: continue # ignore non essential ecus diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 9b8e68fee..dcd300c70 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,40 +4,12 @@ from common.numpy_fast import interp from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import DBC, CanBus +from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarControllerParams(): - def __init__(self): - self.STEER_MAX = 300 - self.STEER_STEP = 2 # how often we update the steer cmd - self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) - self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero - self.MIN_STEER_SPEED = 3. - self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting - self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily - self.STEER_DRIVER_FACTOR = 100 # from dbc - self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop - - # Takes case of "Service Adaptive Cruise" and "Service Front Camera" - # dashboard messages. - self.ADAS_KEEPALIVE_STEP = 100 - self.CAMERA_KEEPALIVE_STEP = 100 - - # pedal lookups, only for Volt - MAX_GAS = 3072 # Only a safety limit - ZERO_GAS = 2048 - MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen - self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle - self.GAS_LOOKUP_BP = [-0.25, 0., 0.5] - self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] - self.BRAKE_LOOKUP_BP = [-1., -0.25] - self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0] - - class CarController(): def __init__(self, dbc_name, CP, VM): self.start_time = 0. diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7f45e7ea9..7ec7679f8 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -28,7 +28,7 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw < 0.01 - ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] + ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0 # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 5e50e6166..9f8d9fa75 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -4,6 +4,33 @@ from cereal import car from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu +class CarControllerParams(): + def __init__(self): + self.STEER_MAX = 300 + self.STEER_STEP = 2 # how often we update the steer cmd + self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) + self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero + self.MIN_STEER_SPEED = 3. + self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily + self.STEER_DRIVER_FACTOR = 100 # from dbc + self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop + + # Takes case of "Service Adaptive Cruise" and "Service Front Camera" + # dashboard messages. + self.ADAS_KEEPALIVE_STEP = 100 + self.CAMERA_KEEPALIVE_STEP = 100 + + # pedal lookups, only for Volt + MAX_GAS = 3072 # Only a safety limit + ZERO_GAS = 2048 + MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen + self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle + self.GAS_LOOKUP_BP = [-0.25, 0., 0.5] + self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] + self.BRAKE_LOOKUP_BP = [-1., -0.25] + self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0] + class CAR: HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" VOLT = "CHEVROLET VOLT PREMIER 2017" diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b73af39a6..4d1c60d4a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -5,7 +5,7 @@ from selfdrive.controls.lib.drive_helpers import rate_limit from common.numpy_fast import clip, interp from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan -from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH +from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -74,15 +74,6 @@ HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "car", "lanes", "fcw", "acc_alert", "steer_required"]) -class CarControllerParams(): - def __init__(self, CP): - self.BRAKE_MAX = 1024//4 - self.STEER_MAX = CP.lateralParams.torqueBP[-1] - # mirror of list (assuming first item is zero) for interp of signed request values - assert(CP.lateralParams.torqueBP[0] == 0) - assert(CP.lateralParams.torqueBP[0] == 0) - self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) - self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) class CarController(): def __init__(self, dbc_name, CP, VM): diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 16d85892a..410d34a73 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -227,8 +227,8 @@ class CarState(CarStateBase): ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] - ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] + ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 73dd2ca39..112bfdbc2 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -8,7 +8,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.events import ET from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint -from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING +from selfdrive.controls.lib.longitudinal_planner import _A_CRUISE_MAX_V_FOLLOWING from selfdrive.car.interfaces import CarInterfaceBase A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) @@ -286,7 +286,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.06 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.75 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -343,7 +343,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.35 # as spec ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -357,7 +357,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.35 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -447,7 +447,7 @@ class CarInterface(CarInterfaceBase): ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid) - ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) + ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo) # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index e4e0dd409..2fc52fc91 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -6,6 +6,16 @@ from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu VisualAlert = car.CarControl.HUDControl.VisualAlert +class CarControllerParams(): + def __init__(self, CP): + self.BRAKE_MAX = 1024//4 + self.STEER_MAX = CP.lateralParams.torqueBP[-1] + # mirror of list (assuming first item is zero) for interp of signed request values + assert(CP.lateralParams.torqueBP[0] == 0) + assert(CP.lateralParams.torqueBP[0] == 0) + self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) + self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) + # Car button codes class CruiseButtons: RES_ACCEL = 4 @@ -182,7 +192,7 @@ FW_VERSIONS = { b'57114-TVA-C060\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TVA,A150\x00\x00', + b'39990-TVA,A150\x00\x00', # modified firmware b'39990-TVA-A150\x00\x00', b'39990-TVA-A160\x00\x00', b'39990-TVA-X030\x00\x00', @@ -380,7 +390,7 @@ FW_VERSIONS = { b'57114-TEA-Q220\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TBA,A030\x00\x00', + b'39990-TBA,A030\x00\x00', # modified firmware b'39990-TBA-A030\x00\x00', b'39990-TBG-A030\x00\x00', b'39990-TEG-A010\x00\x00', @@ -440,6 +450,10 @@ FW_VERSIONS = { b'37805-5AN-AJ30\x00\x00', b'37805-5AN-AK20\x00\x00', b'37805-5AN-AR20\x00\x00', + b'37805-5AN-CH20\x00\x00', + b'37805-5AN-E630\x00\x00', + b'37805-5AN-L840\x00\x00', + b'37805-5AN-L930\x00\x00', b'37805-5AN-L940\x00\x00', b'37805-5AN-LF20\x00\x00', b'37805-5AN-LH20\x00\x00', @@ -447,6 +461,7 @@ FW_VERSIONS = { b'37805-5AN-LR20\x00\x00', b'37805-5AN-LS20\x00\x00', b'37805-5AW-G720\x00\x00', + b'37805-5AN-E820\x00\x00', b'37805-5AZ-E850\x00\x00', b'37805-5BB-A630\x00\x00', b'37805-5BB-A640\x00\x00', @@ -469,6 +484,7 @@ FW_VERSIONS = { b'28101-5CK-C130\x00\x00', b'28101-5CK-C140\x00\x00', b'28101-5CK-C150\x00\x00', + b'28101-5CK-G210\x00\x00', b'28101-5DJ-A610\x00\x00', b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', @@ -489,6 +505,7 @@ FW_VERSIONS = { b'39990-TEZ-T020\x00\x00', b'39990-TGG-A020\x00\x00', b'39990-TGG-A120\x00\x00', + b'39990-TGN-E120\x00\x00', b'39990-TGL-E130\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ @@ -514,8 +531,11 @@ FW_VERSIONS = { b'78109-TGG-A620\x00\x00', b'78109-TGG-A810\x00\x00', b'78109-TGG-A820\x00\x00', + b'78109-TGG-C220\x00\x00', + b'78109-TGG-G030\x00\x00', b'78109-TGL-G120\x00\x00', b'78109-TGL-G130\x00\x00', + b'78109-TGG-G410\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', @@ -523,6 +543,7 @@ FW_VERSIONS = { b'36802-TGG-A050\x00\x00', b'36802-TGG-A060\x00\x00', b'36802-TGG-A130\x00\x00', + b'36802-TGG-G040\x00\x00', b'36802-TGL-G040\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ @@ -532,6 +553,7 @@ FW_VERSIONS = { b'36161-TGG-A060\x00\x00', b'36161-TGG-A080\x00\x00', b'36161-TGG-A120\x00\x00', + b'36161-TGG-G050\x00\x00', b'36161-TGL-G050\x00\x00', b'36161-TGL-G070\x00\x00', ], @@ -541,16 +563,42 @@ FW_VERSIONS = { ], }, CAR.CIVIC_BOSCH_DIESEL: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-59N-G830\x00\x00'], - (Ecu.transmission, 0x18da1ef1, None): [b'28101-59Y-G620\x00\x00'], - (Ecu.vsa, 0x18da28f1, None): [b'57114-TGN-E320\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TFK-G020\x00\x00'], - (Ecu.srs, 0x18da53f1, None): [b'77959-TFK-G210\x00\x00'], - (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-TFK-G020\x00\x00'], - (Ecu.fwdRadar, 0x18dab0f1, None): [b'36802-TFK-G130\x00\x00'], - (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-TGN-E010\x00\x00'], - (Ecu.fwdCamera, 0x18dab5f1, None): [b'36161-TFK-G130\x00\x00'], - (Ecu.gateway, 0x18daeff1, None): [b'38897-TBA-A020\x00\x00'], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ + b'37805-59N-G630\x00\x00', + b'37805-59N-G830\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-59Y-G220\x00\x00', + b'28101-59Y-G620\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TGN-E320\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TFK-G020\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TFK-G210\x00\x00', + b'77959-TGN-G220\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TFK-G020\x00\x00', + b'78109-TGN-G120\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TFK-G130\x00\x00', + b'36802-TGN-G130\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TGN-E010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TFK-G130\x00\x00', + b'36161-TGN-G130\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A020\x00\x00', + ], }, CAR.CRV: { (Ecu.vsa, 0x18da28f1, None): [b'57114-T1W-A230\x00\x00',], @@ -598,7 +646,7 @@ FW_VERSIONS = { b'57114-TMC-Z050\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ - b'39990-TLA,A040\x00\x00', + b'39990-TLA,A040\x00\x00', # modified firmware b'39990-TLA-A040\x00\x00', b'39990-TLA-A110\x00\x00', b'39990-TLA-A220\x00\x00', @@ -643,6 +691,7 @@ FW_VERSIONS = { b'77959-TLA-A240\x00\x00', b'77959-TLA-A250\x00\x00', b'77959-TLA-A320\x00\x00', + b'77959-TLA-A410\x00\x00', b'77959-TLA-Q040\x00\x00', ], }, @@ -883,6 +932,7 @@ FW_VERSIONS = { (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5YF-A230\x00\x00', b'37805-5YF-A420\x00\x00', + b'37805-5YF-A430\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TJB-A040\x00\x00', @@ -978,6 +1028,8 @@ FW_VERSIONS = { b'78109-TXM-A010\x00\x00', b'78109-TXM-A020\x00\x00', b'78109-TXM-A110\x00\x00', + b'78109-TXM-C010\x00\x00', + b'78109-TXM-A030\x00\x00', ], }, CAR.HRV: { @@ -1014,8 +1066,9 @@ FW_VERSIONS = { ], (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-T3R-A120\x00\x00', + b'78109-T3R-A410\x00\x00', ], - }, + }, } DBC = { diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4483d7b21..e71ba6d6e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,8 +1,8 @@ from cereal import car from common.realtime import DT_CTRL from selfdrive.car import apply_std_steer_torque_limits -from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa -from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR +from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc +from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -34,7 +34,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane, class CarController(): def __init__(self, dbc_name, CP, VM): - self.p = SteerLimitParams(CP) + self.p = CarControllerParams(CP) self.packer = CANPacker(dbc_name) self.apply_steer_last = 0 @@ -50,7 +50,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # disable if steer angle reach 90 deg, otherwise mdps fault in some models - lkas_active = enabled and abs(CS.out.steeringAngle) < 90. + lkas_active = enabled and abs(CS.out.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg # fix for Genesis hard fault at low speed if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: @@ -76,11 +76,12 @@ class CarController(): elif CS.out.cruiseState.standstill: # send resume at a max freq of 10Hz if (frame - self.last_resume_frame)*DT_CTRL > 0.1: - can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 20) + # send 25 messages at a time to increases the likelihood of resume being accepted + can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25) self.last_resume_frame = frame # 20 Hz LFA MFA message if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.IONIQ_EV_2020]: - can_sends.append(create_lfa_mfa(self.packer, frame, enabled)) + can_sends.append(create_lfahda_mfc(self.packer, enabled)) return can_sends diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index b799a13fe..b0803698b 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -26,8 +26,8 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw < 0.1 - ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] - ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] + ret.steeringAngleDeg = cp.vl["SAS11"]['SAS_Angle'] + ret.steeringRateDeg = cp.vl["SAS11"]['SAS_Speed'] ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'], cp.vl["CGW1"]['CF_Gway_TurnSigRh']) @@ -37,9 +37,14 @@ class CarState(CarStateBase): ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 # cruise state - ret.cruiseState.available = True - ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 - ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. + if self.CP.openpilotLongitudinalControl: + ret.cruiseState.available = cp.vl["TCS13"]['ACCEnable'] == 0 + ret.cruiseState.enabled = cp.vl["TCS13"]['ACC_REQ'] == 1 + ret.cruiseState.standstill = cp.vl["TCS13"]['StandStill'] == 1 + else: + ret.cruiseState.available = cp.vl["SCC11"]['MainMode_ACC'] == 1 + ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 + ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. if ret.cruiseState.enabled: speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS @@ -127,9 +132,13 @@ class CarState(CarStateBase): # save the entire LKAS11 and CLU11 self.lkas11 = copy.copy(cp_cam.vl["LKAS11"]) self.clu11 = copy.copy(cp.vl["CLU11"]) - self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] + self.park_brake = cp.vl["TCS13"]['PBRAKE_ACT'] == 1 self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist'] + self.brake_hold = cp.vl["TCS15"]['AVH_LAMP'] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY + self.brake_error = cp.vl["TCS13"]['ACCEnable'] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED + self.prev_cruise_buttons = self.cruise_buttons + self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"] return ret @@ -171,10 +180,14 @@ class CarState(CarStateBase): ("CF_Clu_AliveCnt1", "CLU11", 0), ("ACCEnable", "TCS13", 0), + ("ACC_REQ", "TCS13", 0), ("BrakeLight", "TCS13", 0), ("DriverBraking", "TCS13", 0), + ("StandStill", "TCS13", 0), + ("PBRAKE_ACT", "TCS13", 0), ("ESC_Off_Step", "TCS15", 0), + ("AVH_LAMP", "TCS15", 0), ("CF_Lvr_GearInf", "LVR11", 0), # Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) @@ -205,10 +218,14 @@ class CarState(CarStateBase): ("CGW4", 5), ("WHL_SPD11", 50), ("SAS11", 100), - ("SCC11", 50), - ("SCC12", 50), ] + if not CP.openpilotLongitudinalControl: + checks += [ + ("SCC11", 50), + ("SCC12", 50), + ] + if CP.carFingerprint in FEATURES["use_bsm"]: signals += [ ("CF_Lca_IndLeft", "LCA11", 0), @@ -266,7 +283,8 @@ class CarState(CarStateBase): ("FCA_CmdAct", "FCA11", 0), ("CF_VSM_Warn", "FCA11", 0), ] - checks += [("FCA11", 50)] + if not CP.openpilotLongitudinalControl: + checks += [("FCA11", 50)] else: signals += [ ("AEB_CmdAct", "SCC12", 0), @@ -294,7 +312,7 @@ class CarState(CarStateBase): ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), ("CF_Lkas_FusionState", "LKAS11", 0), ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), - ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0) + ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0), ] checks = [ diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 9f064edff..b6f8d4126 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -67,21 +67,83 @@ def create_clu11(packer, frame, clu11, button): return packer.make_can_msg("CLU11", 0, values) -def create_lfa_mfa(packer, frame, enabled): +def create_lfahda_mfc(packer, enabled, hda_set_speed=0): values = { - "ACTIVE": enabled, + "LFA_Icon_State": 2 if enabled else 0, + "HDA_Active": 1 if hda_set_speed else 0, + "HDA_Icon_State": 2 if hda_set_speed else 0, + "HDA_VSetReq": hda_set_speed, } - - # ACTIVE 1 = Green steering wheel icon - - # LFA_USM 2 & 3 = LFA cancelled, fast loud beeping - # LFA_USM 0 & 1 = No mesage - - # LFA_SysWarning 1 = "Switching to HDA", short beep - # LFA_SysWarning 2 = "Switching to Smart Cruise control", short beep - # LFA_SysWarning 3 = LFA error - - # ACTIVE2: nothing - # HDA_USM: nothing - return packer.make_can_msg("LFAHDA_MFC", 0, values) + +def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, stopping): + commands = [] + + scc11_values = { + "MainMode_ACC": 1, + "TauGapSet": 4, + "VSetDis": set_speed if enabled else 0, + "AliveCounterACC": idx % 0x10, + } + commands.append(packer.make_can_msg("SCC11", 0, scc11_values)) + + scc12_values = { + "ACCMode": 1 if enabled else 0, + "StopReq": 1 if stopping else 0, + "aReqRaw": accel, + "aReqValue": accel, # stock ramps up at 1.0/s and down at 0.5/s until it reaches aReqRaw + "CR_VSM_Alive": idx % 0xF, + } + scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2] + scc12_values["CR_VSM_ChkSum"] = 0x10 - sum([sum(divmod(i, 16)) for i in scc12_dat]) % 0x10 + + commands.append(packer.make_can_msg("SCC12", 0, scc12_values)) + + scc14_values = { + "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values + "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values + "JerkUpperLimit": 1.0 if enabled else 0, # stock usually is 1.0 but sometimes uses higher values + "JerkLowerLimit": 0.5 if enabled else 0, # stock usually is 0.5 but sometimes uses higher values + "ACCMode": 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage + "ObjGap": 3 if lead_visible else 0, # TODO: 1-5 based on distance to lead vehicle + } + commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) + + fca11_values = { + # seems to count 2,1,0,3,2,1,0,3,2,1,0,3,2,1,0,repeat... + # (where first value is aligned to Supplemental_Counter == 0) + # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)] + "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1, + "Supplemental_Counter": idx % 0xF, + } + fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2] + fca11_values["CR_FCA_ChkSum"] = 0x10 - sum([sum(divmod(i, 16)) for i in fca11_dat]) % 0x10 + commands.append(packer.make_can_msg("FCA11", 0, fca11_values)) + + return commands + +def create_acc_opt(packer): + commands = [] + + scc13_values = { + "SCCDrvModeRValue": 2, + "SCC_Equip": 1, + "Lead_Veh_Dep_Alert_USM": 2, + } + commands.append(packer.make_can_msg("SCC13", 0, scc13_values)) + + fca12_values = { + # stock values may be needed if openpilot has vision based AEB some day + # for now we are not setting these because there is no AEB for vision only + # "FCA_USM": 3, + # "FCA_DrvSetState": 2, + } + commands.append(packer.make_can_msg("FCA12", 0, fca12_values)) + + return commands + +def create_frt_radar_opt(packer): + frt_radar11_values = { + "CF_FCA_Equip_Front_Radar": 1, + } + return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 47c468dde..e8965a2e2 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -27,6 +27,13 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. + ret.maxSteeringAngleDeg = 90. + + eps_modified = False + for fw in car_fw: + if fw.ecu == "eps" and b"," in fw.fwVersion: + eps_modified = True + if candidate == CAR.SANTA_FE: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG @@ -44,7 +51,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.65 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] - elif candidate == CAR.SONATA_2019: + elif candidate == CAR.SONATA_LF: ret.lateralTuning.pid.kf = 0.00005 ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 @@ -57,7 +64,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.steerRatio = 13.75 * 1.15 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + if eps_modified: + ret.maxSteeringAngleDeg = 1000. elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG @@ -73,10 +82,14 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.01 ret.steerRatio = 16.5 ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 3.5 - ret.lateralTuning.indi.outerLoopGain = 2.0 - ret.lateralTuning.indi.timeConstant = 1.4 - ret.lateralTuning.indi.actuatorEffectiveness = 2.3 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [3.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [2.0] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [2.3] ret.minSteerSpeed = 60 * CV.KPH_TO_MS elif candidate == CAR.KONA: ret.lateralTuning.pid.kf = 0.00005 @@ -157,10 +170,14 @@ class CarInterface(CarInterfaceBase): # Genesis elif candidate == CAR.GENESIS_G70: ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 2.5 - ret.lateralTuning.indi.outerLoopGain = 3.5 - ret.lateralTuning.indi.timeConstant = 1.4 - ret.lateralTuning.indi.actuatorEffectiveness = 1.8 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [2.5] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.5] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.4] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.8] ret.steerActuatorDelay = 0.1 ret.mass = 1640.0 + STD_CARGO_KG ret.wheelbase = 2.84 @@ -180,7 +197,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] # these cars require a special panda safety mode due to missing counters and checksums in the messages - if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, + if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]: ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 94712a0c7..1f8279797 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -5,7 +5,7 @@ from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu # Steer torque limits -class SteerLimitParams: +class CarControllerParams: def __init__(self, CP): if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020]: self.STEER_MAX = 384 @@ -30,12 +30,12 @@ class CAR: KONA_EV = "HYUNDAI KONA ELECTRIC 2019" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" SONATA = "HYUNDAI SONATA 2020" - SONATA_2019 = "HYUNDAI SONATA 2019" + SONATA_LF = "HYUNDAI SONATA 2019" PALISADE = "HYUNDAI PALISADE 2020" VELOSTER = "HYUNDAI VELOSTER 2019" # Kia - KIA_FORTE = "KIA FORTE E 2018" + KIA_FORTE = "KIA FORTE E 2018 & GT 2021" KIA_NIRO_EV = "KIA NIRO EV 2020" KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016" KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" @@ -52,6 +52,7 @@ class Buttons: NONE = 0 RES_ACCEL = 1 SET_DECEL = 2 + GAP_DIST = 3 CANCEL = 4 FINGERPRINTS = { @@ -94,7 +95,7 @@ FINGERPRINTS = { CAR.SONATA: [ {67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8}, ], - CAR.SONATA_2019: [ + CAR.SONATA_LF: [ {66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8}, ], CAR.KIA_OPTIMA: [{ @@ -123,7 +124,7 @@ FINGERPRINTS = { }], CAR.IONIQ_EV_2020: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 - }], + }], CAR.IONIQ_EV_LTD: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1507: 8, 1535: 8 }], @@ -140,7 +141,7 @@ FINGERPRINTS = { 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], CAR.KIA_FORTE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1427: 6, 1456: 4, 1470: 8 + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], CAR.KIA_OPTIMA_H: [{ 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1236: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 @@ -160,6 +161,28 @@ FINGERPRINTS = { IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA] FW_VERSIONS = { + CAR.IONIQ_EV_2020: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 99110-G7200 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7560 4APEC101', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEE MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', + ], + }, + CAR.IONIQ_EV_LTD: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7000 ', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.02 56310G7300\x00 4AEEC102', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.00 95740-G7200 160418', + ], + }, CAR.SONATA: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DN8_ SCC FHCUP 1.00 1.01 99110-L1000 ', @@ -172,6 +195,7 @@ FW_VERSIONS = { b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04', b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04', + b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100\xf1\xa01.06', ], (Ecu.engine, 0x7e0, None): [ b'HM6M2_0a0_BD0', @@ -193,15 +217,53 @@ FW_VERSIONS = { b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', + b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.SONATA_LF: { + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00LF__ SCC F-CUP 1.00 1.00 96401-C2200 ', + ], + (Ecu.esp, 0x7d1, None): [ + b'\xf1\x00LF ESC \t 11 \x17\x01\x13 58920-C2610', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606D5K51\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51', + b'\xf1\xa01.01', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', + b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\x00\x00\x00\x00', + b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', ], }, CAR.SANTA_FE: { - (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 \xf1\xa01.02'], - (Ecu.esp, 0x7d1, None): [b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600\xf1\xa01.00',], - (Ecu.engine, 0x7e0, None): [b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00'], - (Ecu.eps, 0x7d4, None): [b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409'], - (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409'], - (Ecu.transmission, 0x7e1, None): [b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x00\x00\x00\x00'], + (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TM__ SCC F-CUP 1.00 1.03 99110-S2000 \xf1\xa01.03', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.02 99110-S2000 \xf1\xa01.02', + ], + (Ecu.esp, 0x7d1, None): [ + b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650\xf1\xa01.04', + b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600\xf1\xa01.00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00TM MDPS C 1.00 1.01 56340-S2000 9129', + b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', + ], + (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x87LDLUEA6159884HG1\x88\x87hv\x99\x99y\x97\x89\xaa\xb8\x9ax\x99\x87\x89y\x99\xb7\x99\xa7?\xf7\xff\x97wo\xff\xf3\x05\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', + b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x00\x00\x00\x00', + ], }, CAR.KIA_STINGER: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CK__ SCC F_CUP 1.00 1.01 96400-J5100 \xf1\xa01.01'], @@ -235,13 +297,17 @@ FW_VERSIONS = { b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x81640K0051\x00\x00\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7d4, None): [b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103',], + (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', # modified firmware + b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', + ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', + b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', ], @@ -273,7 +339,10 @@ FW_VERSIONS = { (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ], }, CAR.KONA_EV: { - (Ecu.esp, 0x7D1, None): [b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', ], + (Ecu.esp, 0x7D1, None): [ + b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05', + b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000\xf1\xa02.12', + ], (Ecu.fwdCamera, 0x7C4, None): [b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', ], (Ecu.eps, 0x7D4, None): [b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ], (Ecu.fwdRadar, 0x7D0, None): [b'\xf1\x00OSev SCC F-CUP 1.00 1.01 99110-K4000 \xf1\xa01.01', ], @@ -314,13 +383,15 @@ CHECKSUM = { FEATURES = { # which message has the gear "use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]), - "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]), + "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]), "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020]), # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12 - "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]), + "use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE]), - "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA, CAR.IONIQ_EV_2020]), + "use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, + CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA, CAR.IONIQ_EV_2020, + CAR.SANTA_FE, CAR.KIA_NIRO_EV]), } EV_HYBRID = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV]) @@ -345,7 +416,7 @@ DBC = { CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None), CAR.SONATA: dbc_dict('hyundai_kia_generic', None), - CAR.SONATA_2019: dbc_dict('hyundai_kia_generic', None), + CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), CAR.PALISADE: dbc_dict('hyundai_kia_generic', None), CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), } diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 746dabb9e..4d2de08e8 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -13,7 +13,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName -MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 144 + 4 = 92 mph +MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 135 + 4 = 86 mph # generic car and radar interfaces @@ -53,7 +53,6 @@ class CarInterfaceBase(): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.isPandaBlack = True # TODO: deprecate this field # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 20d80e519..36a0495fd 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,5 +1,5 @@ from selfdrive.car.mazda import mazdacan -from selfdrive.car.mazda.values import SteerLimitParams, Buttons +from selfdrive.car.mazda.values import CarControllerParams, Buttons from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits @@ -18,9 +18,9 @@ class CarController(): if enabled: # calculate steer and also set limits due to driver torque - new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) + new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorque, SteerLimitParams) + CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer if CS.out.standstill and frame % 20 == 0: diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index bc6c6d0eb..7b8293570 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -38,12 +38,12 @@ class CarState(CarStateBase): ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1 ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1 - ret.steeringAngle = cp.vl["STEER"]['STEER_ANGLE'] + ret.steeringAngleDeg = cp.vl["STEER"]['STEER_ANGLE'] ret.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR'] ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR'] - ret.steeringRate = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE'] + ret.steeringRateDeg = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE'] ret.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1 ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE'] diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 81c2b2b4a..3060cef5f 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu # Steer torque limits -class SteerLimitParams: +class CarControllerParams: STEER_MAX = 600 # max_steer 2048 STEER_STEP = 1 # how often we update the steer cmd STEER_DELTA_UP = 10 # torque increase per refresh diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index f75d1c58f..8fe7c74d9 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase): # TODO: subscribe to phone sensor self.sensor = messaging.sub_sock('sensorEvents') - self.gps = messaging.sub_sock('gpsLocation') + self.gps = messaging.sub_sock('gpsLocationExternal') self.speed = 0. self.prev_speed = 0. @@ -59,7 +59,7 @@ class CarInterface(CarInterfaceBase): gps = messaging.recv_sock(self.gps) if gps is not None: self.prev_speed = self.speed - self.speed = gps.gpsLocation.speed + self.speed = gps.gpsLocationExternal.speed # create message ret = car.CarState.new_message() @@ -81,7 +81,7 @@ class CarInterface(CarInterfaceBase): self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate curvature = self.yaw_rate / max(self.speed, 1.) - ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG + ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG return ret.as_reader() diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 5838d67ae..8f763f358 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -2,13 +2,8 @@ from cereal import car from common.numpy_fast import clip, interp from selfdrive.car.nissan import nissancan from opendbc.can.packer import CANPacker -from selfdrive.car.nissan.values import CAR, STEER_THRESHOLD +from selfdrive.car.nissan.values import CAR, CarControllerParams -# Steer angle limits -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit -LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -34,33 +29,33 @@ class CarController(): acc_active = bool(CS.out.cruiseState.enabled) lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg - apply_angle = actuators.steerAngle + apply_angle = actuators.steeringAngleDeg steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0 if enabled: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): - angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V) + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) else: - angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU) + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) # Max torque from driver before EPS will give up and not apply torque if not bool(CS.out.steeringPressed): - self.lkas_max_torque = LKAS_MAX_TORQUE + self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE else: # Scale max torque based on how much torque the driver is applying to the wheel self.lkas_max_torque = max( # Scale max torque down to half LKAX_MAX_TORQUE as a minimum - LKAS_MAX_TORQUE * 0.5, + CarControllerParams.LKAS_MAX_TORQUE * 0.5, # Start scaling torque at STEER_THRESHOLD - LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - STEER_THRESHOLD) + CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD) ) else: - apply_angle = CS.out.steeringAngle + apply_angle = CS.out.steeringAngleDeg self.lkas_max_torque = 0 self.last_angle = apply_angle @@ -69,26 +64,27 @@ class CarController(): # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated cruise_cancel = 1 - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL] and cruise_cancel: - can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, CS.cruise_throttle_msg, frame)) + if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA] and cruise_cancel: + can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame)) # TODO: Find better way to cancel! # For some reason spamming the cancel button is unreliable on the Leaf # We now cancel by making propilot think the seatbelt is unlatched, # this generates a beep and a warning message every time you disengage - if self.CP.carFingerprint == CAR.LEAF and frame % 2 == 0: + if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC] and frame % 2 == 0: can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel)) can_sends.append(nissancan.create_steering_control( - self.packer, self.car_fingerprint, apply_angle, frame, enabled, self.lkas_max_torque)) + self.packer, apply_angle, frame, enabled, self.lkas_max_torque)) - if frame % 2 == 0: - can_sends.append(nissancan.create_lkas_hud_msg( - self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart)) + if lkas_hud_msg and lkas_hud_info_msg: + if frame % 2 == 0: + can_sends.append(nissancan.create_lkas_hud_msg( + self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart)) - if frame % 50 == 0: - can_sends.append(nissancan.create_lkas_hud_info_msg( - self.packer, lkas_hud_info_msg, steer_hud_alert - )) + if frame % 50 == 0: + can_sends.append(nissancan.create_lkas_hud_info_msg( + self.packer, lkas_hud_info_msg, steer_hud_alert + )) return can_sends diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index f2076183d..67ee422bc 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine from selfdrive.car.interfaces import CarStateBase from selfdrive.config import Conversions as CV from opendbc.can.parser import CANParser -from selfdrive.car.nissan.values import CAR, DBC, STEER_THRESHOLD +from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams TORQUE_SAMPLES = 12 @@ -14,25 +14,28 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.lkas_hud_msg = None + self.lkas_hud_info_msg = None + self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] def update(self, cp, cp_adas, cp_cam): ret = car.CarState.new_message() - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: + if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] - elif self.CP.carFingerprint == CAR.LEAF: + elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] ret.gasPressed = bool(ret.gas > 3) - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: + if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) - elif self.CP.carFingerprint == CAR.LEAF: + elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: ret.brakePressed = bool(cp.vl["BRAKE_PEDAL"]["BRAKE_PEDAL"] > 3) - if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: + if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: ret.brakeLights = bool(cp.vl["DOORS_LIGHTS"]["BRAKE_LIGHT"]) ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS @@ -45,29 +48,47 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw < 0.01 - ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + if self.CP.carFingerprint == CAR.ALTIMA: + ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + else: + ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + if self.CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) - elif self.CP.carFingerprint == CAR.LEAF: - ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 - ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) - - speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] - if speed != 255: + elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: if self.CP.carFingerprint == CAR.LEAF: + ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 + elif self.CP.carFingerprint == CAR.LEAF_IC: + ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1 + ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) + elif self.CP.carFingerprint == CAR.ALTIMA: + ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 + ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"]) + + if self.CP.carFingerprint == CAR.ALTIMA: + speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"] + else: + speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] + + if speed != 255: + if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS else: conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS speed -= 1 # Speed on HUD is always 1 lower than actually sent on can bus ret.cruiseState.speed = speed * conversion - ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + if self.CP.carFingerprint == CAR.ALTIMA: + ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + else: + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + self.steeringTorqueSamples.append(ret.steeringTorque) # Filtering driver torque to prevent steeringPressed false positives - ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > STEER_THRESHOLD) + ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD) - ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) @@ -82,15 +103,19 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + if self.CP.carFingerprint == CAR.ALTIMA: + self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + else: + self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) - if self.CP.carFingerprint == CAR.LEAF: + if self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) - self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) - self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) + if self.CP.carFingerprint != CAR.ALTIMA: + self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) + self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) return ret @@ -104,8 +129,6 @@ class CarState(CarStateBase): ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR", 0), ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR", 0), - - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), ("DOOR_OPEN_FR", "DOORS_LIGHTS", 1), @@ -125,12 +148,11 @@ class CarState(CarStateBase): # sig_address, frequency ("WHEEL_SPEEDS_REAR", 50), ("WHEEL_SPEEDS_FRONT", 50), - ("STEER_TORQUE_SENSOR", 100), ("STEER_ANGLE_SENSOR", 100), ("DOORS_LIGHTS", 10), ] - if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: + if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA]: signals += [ ("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1), ("BRAKE_LIGHT", "DOORS_LIGHTS", 1), @@ -156,10 +178,12 @@ class CarState(CarStateBase): ] checks += [ - ("GAS_PEDAL", 50), + ("GAS_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ("HUD", 25), ] - elif CP.carFingerprint == CAR.LEAF: + elif CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]: signals += [ ("BRAKE_PEDAL", "BRAKE_PEDAL", 0), @@ -179,108 +203,153 @@ class CarState(CarStateBase): ("CRUISE_THROTTLE", 50), ] + if CP.carFingerprint == CAR.ALTIMA: + signals += [ + ("LKAS_ENABLED", "LKAS_SETTINGS", 0), + ("CRUISE_ENABLED", "CRUISE_STATE", 0), + ("SET_SPEED", "PROPILOT_HUD", 0), + ] + checks += [ + ("CRUISE_STATE", 10), + ("LKAS_SETTINGS", 10), + ("PROPILOT_HUD", 50), + ] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1) + + signals += [ + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), + ] + checks += [ + ("STEER_TORQUE_SENSOR", 100), + ] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) @staticmethod def get_adas_can_parser(CP): # this function generates lists for signal, messages and initial values - signals = [ - # sig_name, sig_address, default - ("LKAS_ENABLED", "LKAS_SETTINGS", 0), - ("CRUISE_ENABLED", "CRUISE_STATE", 0), + if CP.carFingerprint == CAR.ALTIMA: + signals = [ + ("DESIRED_ANGLE", "LKAS", 0), + ("SET_0x80_2", "LKAS", 0), + ("MAX_TORQUE", "LKAS", 0), + ("SET_0x80", "LKAS", 0), + ("COUNTER", "LKAS", 0), + ("LKA_ACTIVE", "LKAS", 0), - ("DESIRED_ANGLE", "LKAS", 0), - ("SET_0x80_2", "LKAS", 0), - ("MAX_TORQUE", "LKAS", 0), - ("SET_0x80", "LKAS", 0), - ("COUNTER", "LKAS", 0), - ("LKA_ACTIVE", "LKAS", 0), + ("CRUISE_ON", "PRO_PILOT", 0), + ] + checks = [ + ("PRO_PILOT", 100), + ] + else: + signals = [ + # sig_name, sig_address, default + ("LKAS_ENABLED", "LKAS_SETTINGS", 0), - # Below are the HUD messages. We copy the stock message and modify - ("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0), - ("LEAD_CAR", "PROPILOT_HUD", 0), - ("LEAD_CAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0), - ("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0), - ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("FOLLOW_DISTANCE", "PROPILOT_HUD", 0), - ("AUDIBLE_TONE", "PROPILOT_HUD", 0), - ("SPEED_SET_ICON", "PROPILOT_HUD", 0), - ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0), - ("unknown59", "PROPILOT_HUD", 0), - ("unknown55", "PROPILOT_HUD", 0), - ("unknown26", "PROPILOT_HUD", 0), - ("unknown28", "PROPILOT_HUD", 0), - ("unknown31", "PROPILOT_HUD", 0), - ("SET_SPEED", "PROPILOT_HUD", 0), - ("unknown43", "PROPILOT_HUD", 0), - ("unknown08", "PROPILOT_HUD", 0), - ("unknown05", "PROPILOT_HUD", 0), - ("unknown02", "PROPILOT_HUD", 0), + ("CRUISE_ENABLED", "CRUISE_STATE", 0), - ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0), - ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0), - ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0), - ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0), - ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0), - ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown07", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown10", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown15", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown23", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown19", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown31", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown32", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown46", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown61", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown55", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown50", "PROPILOT_HUD_INFO_MSG", 0), - ] + ("DESIRED_ANGLE", "LKAS", 0), + ("SET_0x80_2", "LKAS", 0), + ("MAX_TORQUE", "LKAS", 0), + ("SET_0x80", "LKAS", 0), + ("COUNTER", "LKAS", 0), + ("LKA_ACTIVE", "LKAS", 0), - checks = [ - ("CRUISE_STATE", 50), - ] + # Below are the HUD messages. We copy the stock message and modify + ("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0), + ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0), + ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0), + ("LEAD_CAR", "PROPILOT_HUD", 0), + ("LEAD_CAR_ERROR", "PROPILOT_HUD", 0), + ("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0), + ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0), + ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0), + ("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0), + ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0), + ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), + ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), + ("FOLLOW_DISTANCE", "PROPILOT_HUD", 0), + ("AUDIBLE_TONE", "PROPILOT_HUD", 0), + ("SPEED_SET_ICON", "PROPILOT_HUD", 0), + ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0), + ("unknown59", "PROPILOT_HUD", 0), + ("unknown55", "PROPILOT_HUD", 0), + ("unknown26", "PROPILOT_HUD", 0), + ("unknown28", "PROPILOT_HUD", 0), + ("unknown31", "PROPILOT_HUD", 0), + ("SET_SPEED", "PROPILOT_HUD", 0), + ("unknown43", "PROPILOT_HUD", 0), + ("unknown08", "PROPILOT_HUD", 0), + ("unknown05", "PROPILOT_HUD", 0), + ("unknown02", "PROPILOT_HUD", 0), + + ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0), + ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0), + ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0), + ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0), + ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), + ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), + ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), + ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0), + ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0), + ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0), + ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0), + ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0), + ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), + ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), + ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), + ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown07", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown10", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown15", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown23", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown19", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown31", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown32", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown46", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown61", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown55", "PROPILOT_HUD_INFO_MSG", 0), + ("unknown50", "PROPILOT_HUD_INFO_MSG", 0), + ] + + checks = [ + ("CRUISE_STATE", 50), + ] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) @staticmethod def get_cam_can_parser(CP): signals = [] + checks = [] + if CP.carFingerprint in [CAR.ROGUE, CAR.XTRAIL]: signals += [ ("CRUISE_ON", "PRO_PILOT", 0), ] - checks = [ - ] + elif CP.carFingerprint == CAR.ALTIMA: + signals += [ + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), + ] + checks += [ + ("STEER_TORQUE_SENSOR", 100), + ] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 1a003a882..1ad4a93c7 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -4,7 +4,6 @@ from selfdrive.car.nissan.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase - class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) @@ -40,12 +39,19 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 17 - elif candidate == CAR.LEAF: + elif candidate in [CAR.LEAF, CAR.LEAF_IC]: ret.mass = 1610 + STD_CARGO_KG ret.wheelbase = 2.705 ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 17 - + elif candidate == CAR.ALTIMA: + # Altima has EPS on C-CAN unlike the others that have it on V-CAN + ret.safetyParam = 1 # EPS is on alternate bus + ret.mass = 1492 + STD_CARGO_KG + ret.wheelbase = 2.824 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 17 + ret.steerControlType = car.CarParams.SteerControlType.angle ret.radarOffCan = True diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index a264442e6..ceace5088 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -1,10 +1,11 @@ import copy import crcmod +from selfdrive.car.nissan.values import CAR nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) -def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_on, lkas_max_torque): +def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque): idx = (frame % 16) values = { "DESIRED_ANGLE": apply_steer, @@ -21,8 +22,12 @@ def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_o return packer.make_can_msg("LKAS", 0, values) -def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame): +def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg, frame): values = copy.copy(cruise_throttle_msg) + can_bus = 2 + + if car_fingerprint == CAR.ALTIMA: + can_bus = 1 values["CANCEL_BUTTON"] = 1 values["NO_BUTTON_PRESSED"] = 0 @@ -32,7 +37,7 @@ def create_acc_cancel_cmd(packer, cruise_throttle_msg, frame): values["FOLLOW_DISTANCE_BUTTON"] = 0 values["COUNTER"] = (frame % 4) - return packer.make_can_msg("CRUISE_THROTTLE", 2, values) + return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values) def create_cancel_msg(packer, cancel_msg, cruise_cancel): diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index ea4af219e..39ce707d5 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -2,12 +2,22 @@ from selfdrive.car import dbc_dict -STEER_THRESHOLD = 1.0 + +class CarControllerParams: + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower + STEER_THRESHOLD = 1.0 class CAR: XTRAIL = "NISSAN X-TRAIL 2017" LEAF = "NISSAN LEAF 2018" + # Leaf with ADAS ECU found behind instrument cluster instead of glovebox + # Currently the only known difference between them is the inverted seatbelt signal. + LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" ROGUE = "NISSAN ROGUE 2019" + ALTIMA = "NISSAN ALTIMA 2020" FINGERPRINTS = { @@ -28,15 +38,30 @@ FINGERPRINTS = { 2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 }, ], + CAR.LEAF_IC: [ + { + 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1001: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6 + }, + { + 2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8 + }, + ], CAR.ROGUE: [ { 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }, + ], + CAR.ALTIMA: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 386: 8, 397: 8, 398: 8, 520: 2, 523: 6, 548: 8, 634: 7, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1266: 8, 1273: 7, 1306: 1, 1342: 1, 1376: 8, 1401: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, ] } DBC = { CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None), CAR.LEAF: dbc_dict('nissan_leaf_2018', None), + CAR.LEAF_IC: dbc_dict('nissan_leaf_2018', None), CAR.ROGUE: dbc_dict('nissan_x_trail_2017', None), + CAR.ALTIMA: dbc_dict('nissan_x_trail_2017', None), } diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 744fd0bcc..a15e03b58 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,20 +1,9 @@ from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS +from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS, CarControllerParams from opendbc.can.packer import CANPacker -class CarControllerParams(): - def __init__(self): - 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 - 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(): def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 @@ -24,7 +13,6 @@ class CarController(): self.fake_button_prev = 0 self.steer_rate_limited = False - self.params = CarControllerParams() self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): @@ -32,23 +20,23 @@ class CarController(): can_sends = [] # *** steering *** - if (frame % self.params.STEER_STEP) == 0: + if (frame % CarControllerParams.STEER_STEP) == 0: - apply_steer = int(round(actuators.steer * self.params.STEER_MAX)) + apply_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) # limits due to driver torque new_steer = int(round(apply_steer)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: - can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP)) + can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) else: - can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP)) + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP)) self.apply_steer_last = apply_steer diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 3be516ed3..1ad6bc75f 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -43,7 +43,7 @@ class CarState(CarStateBase): can_gear = int(cp.vl["Transmission"]['Gear']) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle'] + ret.steeringAngleDeg = cp.vl["Steering_Torque"]['Steering_Angle'] ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint] diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 88c4ec690..9b3ff060d 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -4,6 +4,15 @@ from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu +class CarControllerParams: + STEER_MAX = 2047 # max_steer 4095 + STEER_STEP = 2 # how often we update the steer cmd + STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max + STEER_DELTA_DOWN = 70 # torque decrease per refresh + STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting + STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily + STEER_DRIVER_FACTOR = 1 # from dbc + class CAR: ASCENT = "SUBARU ASCENT LIMITED 2019" IMPREZA = "SUBARU IMPREZA LIMITED 2019" diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index be05c9d06..9eb6f275f 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -39,7 +39,7 @@ class TestCarInterfaces(unittest.TestCase): elif tuning == 'lqr': self.assertTrue(len(car_params.lateralTuning.lqr.a)) elif tuning == 'indi': - self.assertGreater(car_params.lateralTuning.indi.outerLoopGain, 1e-3) + self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) # Run car interface CC = car.CarControl.new_message() diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b76b66b3e..7152aa73a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,16 +4,11 @@ from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ create_fcw_command -from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, SteerLimitParams +from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -# Accel limits -ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value -ACCEL_MAX = 1.5 # 1.5 m/s2 -ACCEL_MIN = -3.0 # 3 m/s2 -ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) def accel_hysteresis(accel, accel_steady, enabled): @@ -21,10 +16,10 @@ def accel_hysteresis(accel, accel_steady, enabled): if not enabled: # send 0 when disabled, otherwise acc faults accel_steady = 0. - elif accel > accel_steady + ACCEL_HYST_GAP: - accel_steady = accel - ACCEL_HYST_GAP - elif accel < accel_steady - ACCEL_HYST_GAP: - accel_steady = accel + ACCEL_HYST_GAP + elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP: + accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP + elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP: + accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP accel = accel_steady return accel, accel_steady @@ -65,11 +60,11 @@ class CarController(): apply_accel = actuators.gas - actuators.brake apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) - apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) + apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) # steer torque - new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) + new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams) self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) @@ -108,7 +103,7 @@ class CarController(): # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if frame % 2 == 0: # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) - # can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2)) + # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus): diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 4cf84904e..42bce1c48 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -13,13 +13,11 @@ class CarState(CarStateBase): can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR'] - # All TSS2 car have the accurate sensor - self.accurate_steer_angle_seen = CP.carFingerprint in TSS2_CAR - - # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - # is zeroed to where the steering angle is at start. + # On cars with cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] + # the signal is zeroed to where the steering angle is at start. # Need to apply an offset as soon as the steering angle measurements are both received - self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR + self.needs_angle_offset = True + self.accurate_steer_angle_seen = False self.angle_offset = 0. def update(self, cp, cp_cam): @@ -52,17 +50,17 @@ class CarState(CarStateBase): self.accurate_steer_angle_seen = True if self.accurate_steer_angle_seen: - ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset - + ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset if self.needs_angle_offset: angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3: + if abs(angle_wheel) > 1e-3: self.needs_angle_offset = False - self.angle_offset = ret.steeringAngle - angle_wheel + self.angle_offset = ret.steeringAngleDeg - angle_wheel else: - ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + + ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] - ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 @@ -90,8 +88,7 @@ class CarState(CarStateBase): else: ret.cruiseState.standstill = self.pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) - # TODO: CRUISE_STATE is a 4 bit signal, find any other non-adaptive cruise states - ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] in [5] + ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] in [1, 2, 3, 4, 5, 6] if self.CP.carFingerprint == CAR.PRIUS: ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b2a3c70b2..082284a44 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS +from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, NO_DSU_CAR, FINGERPRINTS, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase @@ -12,7 +12,7 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): @staticmethod def compute_gb(accel, speed): - return float(accel) / 3.0 + return float(accel) / CarControllerParams.ACCEL_SCALE @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value @@ -24,6 +24,26 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 + # Improved longitudinal tune + if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2]: + ret.longitudinalTuning.deadzoneBP = [0., 8.05] + ret.longitudinalTuning.deadzoneV = [.0, .14] + ret.longitudinalTuning.kpBP = [0., 5., 20.] + ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] + ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.] + ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1] + ret.stoppingBrakeRate = 0.1 # reach stopping target smoothly + ret.startingBrakeRate = 2.0 # release brakes fast + ret.startAccel = 1.2 # Accelerate from 0 faster + else: + # Default longitudinal tune + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiV = [0.54, 0.36] + if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -37,11 +57,15 @@ class CarInterface(CarInterfaceBase): ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG ret.lateralTuning.init('indi') - ret.lateralTuning.indi.innerLoopGain = 4.0 - ret.lateralTuning.indi.outerLoopGain = 3.0 - ret.lateralTuning.indi.timeConstant = 1.0 - ret.lateralTuning.indi.actuatorEffectiveness = 1.0 - ret.steerActuatorDelay = 0.5 + ret.lateralTuning.indi.innerLoopGainBP = [0.] + ret.lateralTuning.indi.innerLoopGainV = [4.0] + ret.lateralTuning.indi.outerLoopGainBP = [0.] + ret.lateralTuning.indi.outerLoopGainV = [3.0] + ret.lateralTuning.indi.timeConstantBP = [0.] + ret.lateralTuning.indi.timeConstantV = [1.0] + ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] + ret.lateralTuning.indi.actuatorEffectivenessV = [1.0] + ret.steerActuatorDelay = 0.3 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -122,7 +146,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]] ret.lateralTuning.pid.kf = 0.00006 - elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2]: + elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]: stop_and_go = True ret.safetyParam = 73 ret.wheelbase = 2.82448 @@ -196,9 +220,8 @@ class CarInterface(CarInterfaceBase): elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True - ret.minSpeedCan = 0.375 ret.safetyParam = 73 - ret.wheelbase = 2.63906 + ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG @@ -215,6 +238,16 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kf = 0.00007818594 + elif candidate == CAR.LEXUS_ESH: + stop_and_go = True + ret.safetyParam = 73 + ret.wheelbase = 2.8190 + ret.steerRatio = 16.06 + tire_stiffness_factor = 0.444 # not optimized yet + ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] + ret.lateralTuning.pid.kf = 0.00007818594 + elif candidate == CAR.SIENNA: stop_and_go = True ret.safetyParam = 73 @@ -280,8 +313,9 @@ class CarInterface(CarInterfaceBase): ret.enableCamera = True # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] + # TODO: use FW query for the enableDsu flag # In TSS2 cars the camera does long control - ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR + ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in NO_DSU_CAR ret.enableGasInterceptor = 0x201 in fingerprint[0] # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR) @@ -297,21 +331,11 @@ class CarInterface(CarInterfaceBase): # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kiBP = [0., 35.] - if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] ret.gasMaxV = [0.2, 0.5, 0.7] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiV = [0.18, 0.12] - else: - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiV = [0.54, 0.36] return ret diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 6ef056ff2..9d782cbd1 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -4,8 +4,12 @@ from selfdrive.car import dbc_dict from cereal import car Ecu = car.CarParams.Ecu -# Steer torque limits -class SteerLimitParams: +class CarControllerParams: + ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value + ACCEL_MAX = 1.5 # 1.5 m/s2 + ACCEL_MIN = -3.0 # 3 m/s2 + ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) + STEER_MAX = 1500 STEER_DELTA_UP = 10 # 1.5s time to peak torque STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) @@ -26,6 +30,7 @@ class CAR: CAMRY = "TOYOTA CAMRY 2018" CAMRYH = "TOYOTA CAMRY HYBRID 2018" CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 + CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021" HIGHLANDER = "TOYOTA HIGHLANDER 2017" HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" @@ -36,6 +41,7 @@ class CAR: COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019" LEXUS_ES_TSS2 = "LEXUS ES 2019" LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019" + LEXUS_ESH = "LEXUS ES 300H 2018" SIENNA = "TOYOTA SIENNA XLE 2018" LEXUS_IS = "LEXUS IS300 2018" LEXUS_CTH = "LEXUS CT 200H 2018" @@ -46,23 +52,23 @@ class CAR: # addr: (ecu, cars, bus, 1/freq*100, vl) STATIC_MSGS = [ (0x128, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), - (0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), - (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x128, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), + (0x141, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), + (0x160, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), (0x161, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), - (0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0X161, Ecu.dsu, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), - (0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, Ecu.dsu, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), (0x366, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, Ecu.dsu, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), - (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] ECU_FINGERPRINT = { @@ -179,6 +185,9 @@ FINGERPRINTS = { CAR.CAMRY_TSS2: [{ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 791: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1594: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1677: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1800: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1841: 8, 1848: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1940: 8, 1941: 8, 1945: 8, 1948: 8, 1949: 8, 1952: 8, 1953: 8, 1956: 8, 1960: 8, 1961: 8, 1964: 8, 1968: 8, 1973: 8, 1976: 8, 1981: 8, 1986: 8, 1988: 8, 1990: 8, 1994: 8, 1996: 8, 1998: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2012: 8, 2016: 8, 2017: 8 }], + CAR.CAMRYH_TSS2: [{ + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 791: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1593: 8, 1594: 8, 1595: 8, 1649: 8, 1653: 8, 1654: 8, 1655: 8, 1677: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], CAR.HIGHLANDER: [{ 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8 }, @@ -242,6 +251,10 @@ FINGERPRINTS = { { 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.LEXUS_ESH: [ + { + 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 710: 8, 713: 8, 740: 5, 800: 8, 832: 8, 835: 8, 836: 8, 845: 7, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 2, 916: 2, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }], CAR.SIENNA: [ { 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767: 4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 @@ -281,27 +294,38 @@ FINGERPRINTS = { # Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2, - CAR.LEXUS_NX, CAR.CAMRY_TSS2] + CAR.LEXUS_NX, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, CAR.LEXUS_ESH] FW_VERSIONS = { CAR.AVALON: { - (Ecu.esp, 0x7b0, None): [b'F152607060\x00\x00\x00\x00\x00\x00'], + (Ecu.esp, 0x7b0, None): [ + b'F152607110\x00\x00\x00\x00\x00\x00', + b'F152607180\x00\x00\x00\x00\x00\x00', + ], (Ecu.dsu, 0x791, None): [ b'881510705200\x00\x00\x00\x00', b'881510701300\x00\x00\x00\x00', + b'881510703200\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B41090\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7a1, None): [b'8965B41051\x00\x00\x00\x00\x00\x00'], (Ecu.engine, 0x7e0, None): [ b'\x0230721100\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230721200\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', ], + (Ecu.engine, 0x700, None): [ + b'\x01896630738000\x00\x00\x00\x00', + ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702000\x00\x00\x00\x00', b'8821F4702100\x00\x00\x00\x00', + b'8821F4702300\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646F0701100\x00\x00\x00\x00', b'8646F0703000\x00\x00\x00\x00', + b'8646F0702100\x00\x00\x00\x00', ], }, CAR.CAMRY: { @@ -309,6 +333,7 @@ FW_VERSIONS = { b'\x018966306L3100\x00\x00\x00\x00', b'\x018966306L4200\x00\x00\x00\x00', b'\x018966306L5200\x00\x00\x00\x00', + b'\x018966306P8000\x00\x00\x00\x00', b'\x018966306Q3100\x00\x00\x00\x00', b'\x018966306Q4000\x00\x00\x00\x00', b'\x018966306Q4100\x00\x00\x00\x00', @@ -328,6 +353,7 @@ FW_VERSIONS = { (Ecu.dsu, 0x791, None): [ b'8821F0601200 ', b'8821F0601300 ', + b'8821F0602000 ', b'8821F0603300 ', b'8821F0607200 ', b'8821F0608000 ', @@ -337,6 +363,7 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'F152606210\x00\x00\x00\x00\x00\x00', b'F152606230\x00\x00\x00\x00\x00\x00', + b'F152606270\x00\x00\x00\x00\x00\x00', b'F152606290\x00\x00\x00\x00\x00\x00', b'F152633540\x00\x00\x00\x00\x00\x00', b'F152633A20\x00\x00\x00\x00\x00\x00', @@ -351,6 +378,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601200 ', b'8821F0601300 ', + b'8821F0602000 ', b'8821F0603300 ', b'8821F0607200 ', b'8821F0608000 ', @@ -443,10 +471,29 @@ FW_VERSIONS = { b'8965B33630\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ + b'\x01F152606370\x00\x00\x00\x00\x00\x00', b'\x01F152606400\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x018966306Q5000\x00\x00\x00\x00', + b'\x018966306T3100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 15): [ + b'\x018821F6201200\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 109): [ + b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', + ], + }, + CAR.CAMRYH_TSS2: { + (Ecu.eps, 0x7a1, None): [ + b'8965B33630\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152633D00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x700, None): [ + b'\x018966306Q6000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 15): [ b'\x018821F6201200\x00\x00\x00\x00', @@ -469,11 +516,13 @@ FW_VERSIONS = { b'8821FF404100 ', b'8821FF405100 ', b'8821FF406000 ', + b'8821FF407100 ', b'8821F0W01100 ', ], (Ecu.esp, 0x7b0, None): [ b'F152610020\x00\x00\x00\x00\x00\x00', b'F152610153\x00\x00\x00\x00\x00\x00', + b'F152610210\x00\x00\x00\x00\x00\x00', b'F1526F4034\x00\x00\x00\x00\x00\x00', b'F1526F4044\x00\x00\x00\x00\x00\x00', b'F1526F4073\x00\x00\x00\x00\x00\x00', @@ -483,11 +532,14 @@ FW_VERSIONS = { (Ecu.eps, 0x7a1, None): [ b'8965B10011\x00\x00\x00\x00\x00\x00', b'8965B10040\x00\x00\x00\x00\x00\x00', + b'8965B10070\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00', b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', + b'\x0331036000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F0W01000 ', @@ -496,12 +548,14 @@ FW_VERSIONS = { b'8821FF404100 ', b'8821FF405100 ', b'8821FF406000 ', + b'8821FF407100 ', b'8821F0W01100 ', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'8646FF401800 ', b'8646FF404000 ', b'8646FF406000 ', + b'8646FF407000 ', ], }, CAR.CHRH: { @@ -572,6 +626,7 @@ FW_VERSIONS = { b'\x01896630ZG5100\x00\x00\x00\x00', b'\x01896630ZG5200\x00\x00\x00\x00', b'\x01896630ZG5300\x00\x00\x00\x00', + b'\x01896630ZP2000\x00\x00\x00\x00', b'\x01896630ZQ5000\x00\x00\x00\x00', b'\x018966312L8000\x00\x00\x00\x00', b'\x018966312P9000\x00\x00\x00\x00', @@ -581,6 +636,7 @@ FW_VERSIONS = { b'\x018966312R1000\x00\x00\x00\x00', b'\x018966312R1100\x00\x00\x00\x00', b'\x018966312R3100\x00\x00\x00\x00', + b'\x018966312S7000\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -595,6 +651,7 @@ FW_VERSIONS = { b'8965B12361\x00\x00\x00\x00\x00\x00', b'\x018965B12350\x00\x00\x00\x00\x00\x00', b'\x018965B12470\x00\x00\x00\x00\x00\x00', + b'\x018965B12490\x00\x00\x00\x00\x00\x00', b'\x018965B12500\x00\x00\x00\x00\x00\x00', b'\x018965B12520\x00\x00\x00\x00\x00\x00', b'\x018965B12530\x00\x00\x00\x00\x00\x00', @@ -603,10 +660,12 @@ FW_VERSIONS = { b'F152602191\x00\x00\x00\x00\x00\x00', b'\x01F152602280\x00\x00\x00\x00\x00\x00', b'\x01F152602560\x00\x00\x00\x00\x00\x00', + b'\x01F152602590\x00\x00\x00\x00\x00\x00', b'\x01F152602650\x00\x00\x00\x00\x00\x00', b'\x01F152612641\x00\x00\x00\x00\x00\x00', b'\x01F152612651\x00\x00\x00\x00\x00\x00', b'\x01F152612B10\x00\x00\x00\x00\x00\x00', + b'\x01F152612B51\x00\x00\x00\x00\x00\x00', b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', @@ -624,14 +683,17 @@ FW_VERSIONS = { b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, CAR.COROLLAH_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630ZJ1000\x00\x00\x00\x00', + b'\x01896630ZU8000\x00\x00\x00\x00', b'\x018966342M5000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', + b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00', b'\x038966312N1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', @@ -650,6 +712,7 @@ FW_VERSIONS = { b'F152612691\x00\x00\x00\x00\x00\x00', b'F152612692\x00\x00\x00\x00\x00\x00', b'F152612700\x00\x00\x00\x00\x00\x00', + b'F152612790\x00\x00\x00\x00\x00\x00', b'F152612800\x00\x00\x00\x00\x00\x00', b'F152612840\x00\x00\x00\x00\x00\x00', b'F152612A10\x00\x00\x00\x00\x00\x00', @@ -665,6 +728,7 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F1201400\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', @@ -685,6 +749,7 @@ FW_VERSIONS = { b'\x01896630E84000\x00\x00\x00\x00', b'\x01896630E85000\x00\x00\x00\x00', b'\x01896630E88000\x00\x00\x00\x00', + b'\x01896630EA0000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48140\x00\x00\x00\x00\x00\x00', @@ -741,6 +806,9 @@ FW_VERSIONS = { b'\x01896630E64100\x00\x00\x00\x00', b'\x01896630E64200\x00\x00\x00\x00', b'\x01896630EB2000\x00\x00\x00\x00', + b'\x01896630EB2100\x00\x00\x00\x00', + b'\x01896630EB1100\x00\x00\x00\x00', + b'\x01896630EB1000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301400\x00\x00\x00\x00', @@ -759,6 +827,7 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'\x01F15264872300\x00\x00\x00\x00', b'\x01F15264872400\x00\x00\x00\x00', + b'\x01F15264872500\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -984,16 +1053,17 @@ FW_VERSIONS = { b'\x01896634A44000\x00\x00\x00\x00', b'\x01896634A45000\x00\x00\x00\x00', b'\x01896634A46000\x00\x00\x00\x00', - b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x028966342T0000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x028966342V1000\x00\x00\x00\x00897CF1202001\x00\x00\x00\x00', b'\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', + b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'\x01F15260R210\x00\x00\x00\x00\x00\x00', b'\x01F15260R220\x00\x00\x00\x00\x00\x00', + b'\x01F15260R290\x00\x00\x00\x00\x00\x00', b'\x01F15260R300\x00\x00\x00\x00\x00\x00', b'\x01F152642551\x00\x00\x00\x00\x00\x00', b'\x01F152642561\x00\x00\x00\x00\x00\x00', @@ -1035,12 +1105,14 @@ FW_VERSIONS = { b'\x01896634A25000\x00\x00\x00\x00', b'\x018966342W5000\x00\x00\x00\x00', b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', + b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642291\x00\x00\x00\x00\x00\x00', + b'F152642290\x00\x00\x00\x00\x00\x00', b'F152642330\x00\x00\x00\x00\x00\x00', b'F152642331\x00\x00\x00\x00\x00\x00', b'F152642531\x00\x00\x00\x00\x00\x00', @@ -1145,6 +1217,23 @@ FW_VERSIONS = { b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, + CAR.LEXUS_ESH: { + (Ecu.engine, 0x7e0, None): [ + b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152633171\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33512\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4701300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F3302200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_NX: { (Ecu.engine, 0x700, None): [ b'\x01896637851000\x00\x00\x00\x00', @@ -1169,10 +1258,12 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'\x0237882000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0237886000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152678160\x00\x00\x00\x00\x00\x00', b'F152678170\x00\x00\x00\x00\x00\x00', + b'F152678171\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881517804300\x00\x00\x00\x00', @@ -1202,6 +1293,7 @@ FW_VERSIONS = { b'\x018966348R8500\x00\x00\x00\x00', b'\x018966348W1300\x00\x00\x00\x00', b'\x01896630EA4300\x00\x00\x00\x00', + b'\x01896630EA6300\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152648472\x00\x00\x00\x00\x00\x00', @@ -1210,6 +1302,7 @@ FW_VERSIONS = { b'F152648493\x00\x00\x00\x00\x00\x00', b'F152648474\x00\x00\x00\x00\x00\x00', b'F152648630\x00\x00\x00\x00\x00\x00', + b'F152648494\x00\x00\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'881514810300\x00\x00\x00\x00', @@ -1277,14 +1370,18 @@ FW_VERSIONS = { }, CAR.LEXUS_RX_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x01896630EC9000\x00\x00\x00\x00', b'\x01896634D12000\x00\x00\x00\x00', b'\x01896630EB0000\x00\x00\x00\x00', b'\x01896630EA9000\x00\x00\x00\x00', + b'\x01896630ED0000\x00\x00\x00\x00', + b'\x018966348W9000\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'\x01F152648801\x00\x00\x00\x00\x00\x00', b'\x01F15260E031\x00\x00\x00\x00\x00\x00', b'\x01F15260E041\x00\x00\x00\x00\x00\x00', + b'\x01F152648781\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48271\x00\x00\x00\x00\x00\x00', @@ -1302,9 +1399,11 @@ FW_VERSIONS = { CAR.LEXUS_RXH_TSS2: { (Ecu.engine, 0x7e0, None): [ b'\x02348X8000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0234D16000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152648831\x00\x00\x00\x00\x00\x00', + b'F152648D60\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48271\x00\x00\x00\x00\x00\x00', @@ -1313,14 +1412,17 @@ FW_VERSIONS = { b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', ], }, CAR.PRIUS_TSS2: { (Ecu.engine, 0x700, None): [ b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00', + b'\x038966347C5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707101\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ + b'F152647500\x00\x00\x00\x00\x00\x00', b'F152647520\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ @@ -1351,6 +1453,7 @@ DBC = { CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'), CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'), @@ -1361,6 +1464,7 @@ DBC = { CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ESH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'), CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), @@ -1373,9 +1477,9 @@ DBC = { # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, - CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2]) + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]) NO_DSU_CAR = TSS2_CAR | set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]) # no resume button press required -NO_STOP_TIMER_CAR = TSS2_CAR | set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA]) +NO_STOP_TIMER_CAR = TSS2_CAR | set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH]) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a99413092..73a83bb6c 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -28,8 +28,8 @@ class CarState(CarStateBase): # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringAngleDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1, -1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 63da6672b..24c078d7a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -25,7 +25,7 @@ class CarInterface(CarInterfaceBase): # VW port is a community feature, since we don't own one to test ret.communityFeature = True - if candidate == CAR.GOLF: + if candidate in [CAR.GOLF, CAR.AUDI_A3]: # Set common MQB parameters that will apply globally ret.carName = "volkswagen" ret.radarOffCan = True diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index d99d78102..f20c781c1 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -47,14 +47,18 @@ MQB_LDW_MESSAGES = { class CAR: GOLF = "VOLKSWAGEN GOLF" + AUDI_A3 = "AUDI A3" FINGERPRINTS = { - CAR.GOLF: [ - # 76b83eb0245de90e|2019-10-21--17-40-42 - jyoung8607 car - {64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 + CAR.GOLF: [{ + 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 + }], + CAR.AUDI_A3: [{ + 64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 278: 8, 279: 8, 283: 8, 285: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 295: 8, 299: 8, 302: 8, 346: 8, 418: 8, 427: 8, 506: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 787: 8, 788: 8, 789: 8, 792: 8, 802: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 846: 8, 847: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1624: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1792: 8, 1872: 8, 1976: 8, 1977: 8, 1982: 8, 1985: 8 }], } DBC = { CAR.GOLF: dbc_dict('vw_mqb_2010', None), + CAR.AUDI_A3: dbc_dict('vw_mqb_2010', None), } diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc index d9fa9eda3..32d8c508c 100644 --- a/selfdrive/clocksd/clocksd.cc +++ b/selfdrive/clocksd/clocksd.cc @@ -1,21 +1,25 @@ -#include -#include - #include #include #include #include #include -#include -#include "messaging.hpp" -#include "common/timing.h" - // Apple doesn't have timerfd -#ifndef __APPLE__ +#ifdef __APPLE__ +#include +#else #include #endif +#include +#include + +#include "messaging.hpp" +#include "common/timing.h" +#include "common/util.h" + +ExitHandler do_exit; + #ifdef QCOM namespace { int64_t arm_cntpct() { @@ -28,7 +32,6 @@ namespace { int main() { setpriority(PRIO_PROCESS, 0, -13); - PubMaster pm({"clocks"}); #ifndef __APPLE__ @@ -45,12 +48,12 @@ int main() { assert(err == 0); uint64_t expirations = 0; - while ((err = read(timerfd, &expirations, sizeof(expirations)))) { + while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) { if (err < 0) break; #else // Just run at 1Hz on apple - while (true){ - std::this_thread::sleep_for(std::chrono::seconds(1)); + while (!do_exit){ + util::sleep_for(1000); #endif uint64_t boottime = nanos_since_boot(); diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index fdca02e83..ec3603d3a 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -5,16 +5,13 @@ if SHARED: else: fxn = env.Library -common_libs = ['params.cc', 'swaglog.cc', 'util.c', 'cqueue.c', 'gpio.cc', 'i2c.cc'] +common_libs = ['params.cc', 'swaglog.cc', 'util.cc', 'gpio.cc', 'i2c.cc'] _common = fxn('common', common_libs, LIBS="json11") -_visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) files = [ - 'buffering.c', 'clutil.cc', - 'efd.c', - 'glutil.c', + 'glutil.cc', 'visionimg.cc', ] @@ -23,21 +20,9 @@ if arch == "aarch64": 'framebuffer.cc', 'touch.c', ] - if QCOM_REPLAY: - files += ['visionbuf_cl.c'] - else: - files += ['visionbuf_ion.c'] _gpu_libs = ['gui', 'adreno_utils'] -elif arch == "larch64": - files += [ - 'visionbuf_ion.c', - ] - _gpu_libs = ['GL'] else: - files += [ - 'visionbuf_cl.c', - ] _gpu_libs = ["GL"] _gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs) -Export('_common', '_visionipc', '_gpucommon', '_gpu_libs') +Export('_common', '_gpucommon', '_gpu_libs') diff --git a/selfdrive/common/buffering.c b/selfdrive/common/buffering.c deleted file mode 100644 index bba2e82ab..000000000 --- a/selfdrive/common/buffering.c +++ /dev/null @@ -1,438 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "common/efd.h" - -#include "buffering.h" - -void tbuffer_init(TBuffer *tb, int num_bufs, const char* name) { - assert(num_bufs >= 3); - - memset(tb, 0, sizeof(TBuffer)); - tb->reading = (bool*)calloc(num_bufs, sizeof(bool)); - assert(tb->reading); - tb->pending_idx = -1; - tb->num_bufs = num_bufs; - tb->name = name; - - pthread_mutex_init(&tb->lock, NULL); - pthread_cond_init(&tb->cv, NULL); - tb->efd = efd_init(); - assert(tb->efd >= 0); -} - -void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name, - void (*release_cb)(void* c, int idx), - void* cb_cookie) { - - tbuffer_init(tb, num_bufs, name); - - tb->release_cb = release_cb; - tb->cb_cookie = cb_cookie; -} - -int tbuffer_efd(TBuffer *tb) { - return tb->efd; -} - -int tbuffer_select(TBuffer *tb) { - pthread_mutex_lock(&tb->lock); - - int i; - for (i=0; inum_bufs; i++) { - if (!tb->reading[i] && i != tb->pending_idx) { - break; - } - } - assert(i < tb->num_bufs); - - pthread_mutex_unlock(&tb->lock); - return i; -} - -void tbuffer_dispatch(TBuffer *tb, int idx) { - pthread_mutex_lock(&tb->lock); - - if (tb->pending_idx != -1) { - //printf("tbuffer (%s) dropped!\n", tb->name ? tb->name : "?"); - if (tb->release_cb) { - tb->release_cb(tb->cb_cookie, tb->pending_idx); - } - tb->pending_idx = -1; - } - - tb->pending_idx = idx; - - efd_write(tb->efd); - pthread_cond_signal(&tb->cv); - - pthread_mutex_unlock(&tb->lock); -} - -int tbuffer_acquire(TBuffer *tb) { - pthread_mutex_lock(&tb->lock); - - if (tb->stopped) { - pthread_mutex_unlock(&tb->lock); - return -1; - } - - while (tb->pending_idx == -1) { - pthread_cond_wait(&tb->cv, &tb->lock); - - if (tb->stopped) { - pthread_mutex_unlock(&tb->lock); - return -1; - } - } - - efd_clear(tb->efd); - - int ret = tb->pending_idx; - assert(ret < tb->num_bufs); - - tb->reading[ret] = true; - tb->pending_idx = -1; - - pthread_mutex_unlock(&tb->lock); - - return ret; -} - -static void tbuffer_release_locked(TBuffer *tb, int idx) { - assert(idx < tb->num_bufs); - if (!tb->reading[idx]) { - printf("!! releasing tbuffer we aren't reading %d\n", idx); - } - - if (tb->release_cb) { - tb->release_cb(tb->cb_cookie, idx); - } - - tb->reading[idx] = false; -} - -void tbuffer_release(TBuffer *tb, int idx) { - pthread_mutex_lock(&tb->lock); - tbuffer_release_locked(tb, idx); - pthread_mutex_unlock(&tb->lock); -} - -void tbuffer_release_all(TBuffer *tb) { - pthread_mutex_lock(&tb->lock); - for (int i=0; inum_bufs; i++) { - if (tb->reading[i]) { - tbuffer_release_locked(tb, i); - } - } - pthread_mutex_unlock(&tb->lock); -} - -void tbuffer_stop(TBuffer *tb) { - pthread_mutex_lock(&tb->lock); - tb->stopped = true; - efd_write(tb->efd); - pthread_cond_signal(&tb->cv); - pthread_mutex_unlock(&tb->lock); -} - - - - - - - - - - - -void pool_init(Pool *s, int num_bufs) { - assert(num_bufs > 3); - - memset(s, 0, sizeof(*s)); - s->num_bufs = num_bufs; - - s->refcnt = (int*)calloc(num_bufs, sizeof(int)); - s->ts = (int*)calloc(num_bufs, sizeof(int)); - - s->counter = 1; - - pthread_mutex_init(&s->lock, NULL); -} - -void pool_init2(Pool *s, int num_bufs, - void (*release_cb)(void* c, int idx), void* cb_cookie) { - - pool_init(s, num_bufs); - s->cb_cookie = cb_cookie; - s->release_cb = release_cb; - -} - - -void pool_acquire(Pool *s, int idx) { - pthread_mutex_lock(&s->lock); - - assert(idx >= 0 && idx < s->num_bufs); - - s->refcnt[idx]++; - - pthread_mutex_unlock(&s->lock); -} - -static void pool_release_locked(Pool *s, int idx) { - // printf("release %d refcnt %d\n", idx, s->refcnt[idx]); - - assert(idx >= 0 && idx < s->num_bufs); - - assert(s->refcnt[idx] > 0); - s->refcnt[idx]--; - - // printf("release %d -> %d, %p\n", idx, s->refcnt[idx], s->release_cb); - if (s->refcnt[idx] == 0 && s->release_cb) { - // printf("call %p\b", s->release_cb); - s->release_cb(s->cb_cookie, idx); - } -} - -void pool_release(Pool *s, int idx) { - pthread_mutex_lock(&s->lock); - pool_release_locked(s, idx); - pthread_mutex_unlock(&s->lock); -} - -TBuffer* pool_get_tbuffer(Pool *s) { - pthread_mutex_lock(&s->lock); - - assert(s->num_tbufs < POOL_MAX_TBUFS); - TBuffer* tbuf = &s->tbufs[s->num_tbufs]; - s->num_tbufs++; - tbuffer_init2(tbuf, s->num_bufs, - "pool", (void (*)(void *, int))pool_release, s); - - bool stopped = s->stopped; - pthread_mutex_unlock(&s->lock); - - // Stop the tbuffer so we can return a valid object. - // We must stop here because the pool_stop may have already been called, - // in which case tbuffer_stop may never be called again. - if (stopped) { - tbuffer_stop(tbuf); - } - return tbuf; -} - -PoolQueue* pool_get_queue(Pool *s) { - pthread_mutex_lock(&s->lock); - - int i; - for (i = 0; i < POOL_MAX_QUEUES; i++) { - if (!s->queues[i].inited) { - break; - } - } - assert(i < POOL_MAX_QUEUES); - - PoolQueue *c = &s->queues[i]; - memset(c, 0, sizeof(*c)); - - c->pool = s; - c->inited = true; - - c->efd = efd_init(); - assert(c->efd >= 0); - - c->num_bufs = s->num_bufs; - c->num = c->num_bufs+1; - c->idx = (int*)malloc(sizeof(int)*c->num); - memset(c->idx, -1, sizeof(int)*c->num); - - pthread_mutex_init(&c->lock, NULL); - pthread_cond_init(&c->cv, NULL); - - pthread_mutex_unlock(&s->lock); - return c; -} - -void pool_release_queue(PoolQueue *c) { - Pool *s = c->pool; - - pthread_mutex_lock(&s->lock); - pthread_mutex_lock(&c->lock); - - for (int i=0; inum; i++) { - if (c->idx[i] != -1) { - pool_release_locked(s, c->idx[i]); - } - } - - close(c->efd); - free(c->idx); - - c->inited = false; - - pthread_mutex_unlock(&c->lock); - - pthread_mutex_destroy(&c->lock); - pthread_cond_destroy(&c->cv); - - pthread_mutex_unlock(&s->lock); -} - -int pool_select(Pool *s) { - pthread_mutex_lock(&s->lock); - - int i; - for (i=0; inum_bufs; i++) { - if (s->refcnt[i] == 0) { - break; - } - } - - if (i >= s->num_bufs) { - // overwrite the oldest - // still being using in a queue or tbuffer :/ - - int min_k = 0; - int min_ts = s->ts[0]; - for (int k=1; knum_bufs; k++) { - if (s->ts[k] < min_ts) { - min_ts = s->ts[k]; - min_k = k; - } - } - i = min_k; - printf("pool is full! evicted %d\n", min_k); - - // might be really bad if the user is doing pointery stuff - if (s->release_cb) { - s->release_cb(s->cb_cookie, min_k); - } - } - - s->refcnt[i]++; - - s->ts[i] = s->counter; - s->counter++; - - pthread_mutex_unlock(&s->lock); - - return i; -} - -void pool_push(Pool *s, int idx) { - pthread_mutex_lock(&s->lock); - - // printf("push %d head %d tail %d\n", idx, s->head, s->tail); - - assert(idx >= 0 && idx < s->num_bufs); - - s->ts[idx] = s->counter; - s->counter++; - - assert(s->refcnt[idx] > 0); - s->refcnt[idx]--; //push is a implcit release - - int num_tbufs = s->num_tbufs; - s->refcnt[idx] += num_tbufs; - - // dispatch pool queues - for (int i=0; iqueues[i]; - if (!c->inited) continue; - - pthread_mutex_lock(&c->lock); - if (((c->head+1) % c->num) == c->tail) { - // queue is full. skip for now - pthread_mutex_unlock(&c->lock); - continue; - } - - s->refcnt[idx]++; - - c->idx[c->head] = idx; - c->head = (c->head+1) % c->num; - assert(c->head != c->tail); - pthread_mutex_unlock(&c->lock); - - efd_write(c->efd); - pthread_cond_signal(&c->cv); - } - - pthread_mutex_unlock(&s->lock); - - for (int i=0; itbufs[i], idx); - } -} - -int poolq_pop(PoolQueue *c) { - pthread_mutex_lock(&c->lock); - - if (c->stopped) { - pthread_mutex_unlock(&c->lock); - return -1; - } - - while (c->head == c->tail) { - pthread_cond_wait(&c->cv, &c->lock); - - if (c->stopped) { - pthread_mutex_unlock(&c->lock); - return -1; - } - } - - // printf("pop head %d tail %d\n", s->head, s->tail); - - assert(c->head != c->tail); - - int r = c->idx[c->tail]; - c->idx[c->tail] = -1; - c->tail = (c->tail+1) % c->num; - - // queue event is level triggered - if (c->head == c->tail) { - efd_clear(c->efd); - } - - // printf("pop %d head %d tail %d\n", r, s->head, s->tail); - - assert(r >= 0 && r < c->num_bufs); - - pthread_mutex_unlock(&c->lock); - - return r; -} - -int poolq_efd(PoolQueue *c) { - return c->efd; -} - -void poolq_release(PoolQueue *c, int idx) { - pool_release(c->pool, idx); -} - -void pool_stop(Pool *s) { - for (int i=0; inum_tbufs; i++) { - tbuffer_stop(&s->tbufs[i]); - } - - pthread_mutex_lock(&s->lock); - s->stopped = true; - for (int i=0; iqueues[i]; - if (!c->inited) continue; - - pthread_mutex_lock(&c->lock); - c->stopped = true; - pthread_mutex_unlock(&c->lock); - efd_write(c->efd); - pthread_cond_signal(&c->cv); - } - pthread_mutex_unlock(&s->lock); -} diff --git a/selfdrive/common/buffering.h b/selfdrive/common/buffering.h deleted file mode 100644 index fda4c6449..000000000 --- a/selfdrive/common/buffering.h +++ /dev/null @@ -1,123 +0,0 @@ -#ifndef BUFFERING_H -#define BUFFERING_H - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -// Tripple buffering helper - -typedef struct TBuffer { - pthread_mutex_t lock; - pthread_cond_t cv; - int efd; - - bool* reading; - int pending_idx; - - int num_bufs; - const char* name; - - void (*release_cb)(void* c, int idx); - void *cb_cookie; - - bool stopped; -} TBuffer; - -// num_bufs must be at least the number of buffers that can be acquired simultaniously plus two -void tbuffer_init(TBuffer *tb, int num_bufs, const char* name); - -void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name, - void (*release_cb)(void* c, int idx), - void* cb_cookie); - -// returns an eventfd that signals if a buffer is ready and tbuffer_acquire shouldn't to block. -// useful to polling on multiple tbuffers. -int tbuffer_efd(TBuffer *tb); - -// Chooses a buffer that's not reading or pending -int tbuffer_select(TBuffer *tb); - -// Called when the writer is done with their buffer -// - Wakes up the reader if it's waiting -// - releases the pending buffer if the reader's too slow -void tbuffer_dispatch(TBuffer *tb, int idx); - -// Called when the reader wants a new buffer, will return -1 when stopped -int tbuffer_acquire(TBuffer *tb); - -// Called when the reader is done with their buffer -void tbuffer_release(TBuffer *tb, int idx); - -void tbuffer_release_all(TBuffer *tb); - -void tbuffer_stop(TBuffer *tb); - - - - -// pool: buffer pool + queue thing... - -#define POOL_MAX_TBUFS 8 -#define POOL_MAX_QUEUES 8 - -typedef struct Pool Pool; - -typedef struct PoolQueue { - pthread_mutex_t lock; - pthread_cond_t cv; - Pool* pool; - bool inited; - bool stopped; - int efd; - int num_bufs; - int num; - int head, tail; - int* idx; -} PoolQueue; - -int poolq_pop(PoolQueue *s); -int poolq_efd(PoolQueue *s); -void poolq_release(PoolQueue *c, int idx); - -typedef struct Pool { - pthread_mutex_t lock; - bool stopped; - int num_bufs; - int counter; - - int* ts; - int* refcnt; - - void (*release_cb)(void* c, int idx); - void *cb_cookie; - - int num_tbufs; - TBuffer tbufs[POOL_MAX_TBUFS]; - PoolQueue queues[POOL_MAX_QUEUES]; -} Pool; - -void pool_init(Pool *s, int num_bufs); -void pool_init2(Pool *s, int num_bufs, - void (*release_cb)(void* c, int idx), void* cb_cookie); - -TBuffer* pool_get_tbuffer(Pool *s); - -PoolQueue* pool_get_queue(Pool *s); -void pool_release_queue(PoolQueue *q); - -int pool_select(Pool *s); -void pool_push(Pool *s, int idx); -void pool_acquire(Pool *s, int idx); -void pool_release(Pool *s, int idx); -void pool_stop(Pool *s); - - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc index 45767fc50..c6a2b49dc 100644 --- a/selfdrive/common/clutil.cc +++ b/selfdrive/common/clutil.cc @@ -7,7 +7,6 @@ #include #include #include "util.h" -#include "utilpp.h" namespace { // helper functions diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h index 793d6e7f3..71523c273 100644 --- a/selfdrive/common/clutil.h +++ b/selfdrive/common/clutil.h @@ -10,10 +10,6 @@ #include #endif -#ifdef __cplusplus -extern "C" { -#endif - #define CL_CHECK(_expr) \ do { \ assert(CL_SUCCESS == _expr); \ @@ -30,7 +26,3 @@ extern "C" { cl_device_id cl_get_device_id(cl_device_type device_type); cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args); const char* cl_get_error_string(int err); - -#ifdef __cplusplus -} -#endif diff --git a/selfdrive/common/cqueue.c b/selfdrive/common/cqueue.c deleted file mode 100644 index fe2c6f4eb..000000000 --- a/selfdrive/common/cqueue.c +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include - -#include "cqueue.h" - -void queue_init(Queue *q) { - memset(q, 0, sizeof(*q)); - TAILQ_INIT(&q->q); - pthread_mutex_init(&q->lock, NULL); - pthread_cond_init(&q->cv, NULL); -} - -void* queue_pop(Queue *q) { - pthread_mutex_lock(&q->lock); - while (TAILQ_EMPTY(&q->q)) { - pthread_cond_wait(&q->cv, &q->lock); - } - QueueEntry *entry = TAILQ_FIRST(&q->q); - TAILQ_REMOVE(&q->q, entry, entries); - pthread_mutex_unlock(&q->lock); - - void* r = entry->data; - free(entry); - return r; -} - -void* queue_try_pop(Queue *q) { - pthread_mutex_lock(&q->lock); - - void* r = NULL; - if (!TAILQ_EMPTY(&q->q)) { - QueueEntry *entry = TAILQ_FIRST(&q->q); - TAILQ_REMOVE(&q->q, entry, entries); - r = entry->data; - free(entry); - } - - pthread_mutex_unlock(&q->lock); - return r; -} - -void queue_push(Queue *q, void *data) { - QueueEntry *entry = calloc(1, sizeof(QueueEntry)); - assert(entry); - entry->data = data; - - pthread_mutex_lock(&q->lock); - TAILQ_INSERT_TAIL(&q->q, entry, entries); - pthread_cond_signal(&q->cv); - pthread_mutex_unlock(&q->lock); -} diff --git a/selfdrive/common/cqueue.h b/selfdrive/common/cqueue.h deleted file mode 100644 index f2613660b..000000000 --- a/selfdrive/common/cqueue.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef COMMON_CQUEUE_H -#define COMMON_CQUEUE_H - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -// a blocking queue - -typedef struct QueueEntry { - TAILQ_ENTRY(QueueEntry) entries; - void *data; -} QueueEntry; - -typedef struct Queue { - TAILQ_HEAD(queue, QueueEntry) q; - pthread_mutex_t lock; - pthread_cond_t cv; -} Queue; - -void queue_init(Queue *q); -void* queue_pop(Queue *q); -void* queue_try_pop(Queue *q); -void queue_push(Queue *q, void *data); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/selfdrive/common/efd.c b/selfdrive/common/efd.c deleted file mode 100644 index 78a7c0989..000000000 --- a/selfdrive/common/efd.c +++ /dev/null @@ -1,56 +0,0 @@ -#include -#include - -#ifdef __linux__ -#include -#else -#include -#include -#define EVENT_IDENT 42 -#endif - -#include "efd.h" - - -int efd_init() { -#ifdef __linux__ - return eventfd(0, EFD_CLOEXEC); -#else - int fd = kqueue(); - assert(fd >= 0); - - struct kevent kev; - EV_SET(&kev, EVENT_IDENT, EVFILT_USER, EV_ADD | EV_CLEAR, 0, 0, NULL); - - struct timespec timeout = {0, 0}; - int err = kevent(fd, &kev, 1, NULL, 0, &timeout); - assert(err != -1); - - return fd; -#endif -} - -void efd_write(int fd) { -#ifdef __linux__ - eventfd_write(fd, 1); -#else - struct kevent kev; - EV_SET(&kev, EVENT_IDENT, EVFILT_USER, 0, NOTE_TRIGGER, 0, NULL); - - struct timespec timeout = {0, 0}; - int err = kevent(fd, &kev, 1, NULL, 0, &timeout); - assert(err != -1); -#endif -} - -void efd_clear(int fd) { -#ifdef __linux__ - eventfd_t efd_cnt; - eventfd_read(fd, &efd_cnt); -#else - struct kevent kev; - struct timespec timeout = {0, 0}; - int nfds = kevent(fd, NULL, 0, &kev, 1, &timeout); - assert(nfds != -1); -#endif -} diff --git a/selfdrive/common/efd.h b/selfdrive/common/efd.h deleted file mode 100644 index 056482ffa..000000000 --- a/selfdrive/common/efd.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef EFD_H -#define EFD_H - -#ifdef __cplusplus -extern "C" { -#endif - -// event fd: a semaphore that can be poll()'d -int efd_init(); -void efd_write(int fd); -void efd_clear(int fd); - -#ifdef __cplusplus -} -#endif - -#endif \ No newline at end of file diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc index 30ef7ae8e..b006479af 100644 --- a/selfdrive/common/framebuffer.cc +++ b/selfdrive/common/framebuffer.cc @@ -1,6 +1,6 @@ +#include "framebuffer.h" #include "util.h" #include -#include #include #include @@ -32,28 +32,26 @@ struct FramebufferState { EGLContext context; }; -extern "C" void framebuffer_swap(FramebufferState *s) { +void FrameBuffer::swap() { eglSwapBuffers(s->display, s->surface); assert(glGetError() == GL_NO_ERROR); } -extern "C" bool set_brightness(int brightness) { +bool set_brightness(int brightness) { char bright[64]; snprintf(bright, sizeof(bright), "%d", brightness); return 0 == write_file("/sys/class/leds/lcd-backlight/brightness", bright, strlen(bright)); } -extern "C" void framebuffer_set_power(FramebufferState *s, int mode) { +void FrameBuffer::set_power(int mode) { SurfaceComposerClient::setDisplayPowerMode(s->dtoken, mode); } -extern "C" FramebufferState* framebuffer_init( - const char* name, int32_t layer, int alpha, - int *out_w, int *out_h) { +FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h) { status_t status; int success; - FramebufferState *s = new FramebufferState; + s = new FramebufferState; s->session = new SurfaceComposerClient(); assert(s->session != NULL); @@ -141,6 +139,11 @@ extern "C" FramebufferState* framebuffer_init( if (out_w) *out_w = w; if (out_h) *out_h = h; - - return s; +} + +FrameBuffer::~FrameBuffer() { + eglDestroyContext(s->display, s->context); + eglDestroySurface(s->display, s->surface); + eglTerminate(s->display); + delete s; } diff --git a/selfdrive/common/framebuffer.h b/selfdrive/common/framebuffer.h index 45053bbb1..1285b67c6 100644 --- a/selfdrive/common/framebuffer.h +++ b/selfdrive/common/framebuffer.h @@ -1,17 +1,7 @@ #pragma once -#ifdef __cplusplus -extern "C" { -#endif +#include -typedef struct FramebufferState FramebufferState; - -FramebufferState* framebuffer_init( - const char* name, int32_t layer, int alpha, - int *out_w, int *out_h); - -void framebuffer_set_power(FramebufferState *s, int mode); -void framebuffer_swap(FramebufferState *s); bool set_brightness(int brightness); /* Display power modes */ @@ -40,6 +30,13 @@ enum { HWC_POWER_MODE_DOZE_SUSPEND = 3, }; -#ifdef __cplusplus -} -#endif +struct FramebufferState; +class FrameBuffer { + public: + FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h); + ~FrameBuffer(); + void set_power(int mode); + void swap(); +private: + FramebufferState *s; +}; diff --git a/selfdrive/common/glutil.c b/selfdrive/common/glutil.c deleted file mode 100644 index e208891e2..000000000 --- a/selfdrive/common/glutil.c +++ /dev/null @@ -1,69 +0,0 @@ -#include -#include - -#include "glutil.h" - -GLuint load_shader(GLenum shaderType, const char *src) { - GLint status = 0, len = 0; - GLuint shader; - - if (!(shader = glCreateShader(shaderType))) - return 0; - - glShaderSource(shader, 1, &src, NULL); - glCompileShader(shader); - glGetShaderiv(shader, GL_COMPILE_STATUS, &status); - - if (status) - return shader; - - glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &len); - if (len) { - char *msg = (char*)malloc(len); - if (msg) { - glGetShaderInfoLog(shader, len, NULL, msg); - msg[len-1] = 0; - fprintf(stderr, "error compiling shader:\n%s\n", msg); - free(msg); - } - } - glDeleteShader(shader); - return 0; -} - -GLuint load_program(const char *vert_src, const char *frag_src) { - GLuint vert, frag, prog; - GLint status = 0, len = 0; - - if (!(vert = load_shader(GL_VERTEX_SHADER, vert_src))) - return 0; - if (!(frag = load_shader(GL_FRAGMENT_SHADER, frag_src))) - goto fail_frag; - if (!(prog = glCreateProgram())) - goto fail_prog; - - glAttachShader(prog, vert); - glAttachShader(prog, frag); - glLinkProgram(prog); - - glGetProgramiv(prog, GL_LINK_STATUS, &status); - if (status) - return prog; - - glGetProgramiv(prog, GL_INFO_LOG_LENGTH, &len); - if (len) { - char *buf = (char*) malloc(len); - if (buf) { - glGetProgramInfoLog(prog, len, NULL, buf); - buf[len-1] = 0; - fprintf(stderr, "error linking program:\n%s\n", buf); - free(buf); - } - } - glDeleteProgram(prog); -fail_prog: - glDeleteShader(frag); -fail_frag: - glDeleteShader(vert); - return 0; -} diff --git a/selfdrive/common/glutil.cc b/selfdrive/common/glutil.cc new file mode 100644 index 000000000..77cb82627 --- /dev/null +++ b/selfdrive/common/glutil.cc @@ -0,0 +1,63 @@ +#include +#include +#include +#include + +#include "glutil.h" + +static GLuint load_shader(GLenum shaderType, const char *src) { + GLint status = 0, len = 0; + GLuint shader = glCreateShader(shaderType); + assert(shader != 0); + + glShaderSource(shader, 1, &src, NULL); + glCompileShader(shader); + glGetShaderiv(shader, GL_COMPILE_STATUS, &status); + if (!status) { + glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &len); + if (len) { + std::string msg(len, '\0'); + glGetShaderInfoLog(shader, len, NULL, msg.data()); + fprintf(stderr, "error compiling shader:\n%s\n", msg.c_str()); + } + assert(0); + } + return shader; +} + +GLShader::GLShader(const char *vert_src, const char *frag_src) { + GLint status = 0, len = 0; + prog = glCreateProgram(); + assert(prog != 0); + + vert = load_shader(GL_VERTEX_SHADER, vert_src); + frag = load_shader(GL_FRAGMENT_SHADER, frag_src); + glAttachShader(prog, vert); + glAttachShader(prog, frag); + glLinkProgram(prog); + + glGetProgramiv(prog, GL_LINK_STATUS, &status); + if (!status) { + glGetProgramiv(prog, GL_INFO_LOG_LENGTH, &len); + if (len) { + std::string msg(len, '\0'); + glGetProgramInfoLog(prog, len, NULL, msg.data()); + fprintf(stderr, "error linking program:\n%s\n", msg.c_str()); + } + assert(0); + } +} + +GLShader::~GLShader() { + glDeleteProgram(prog); + glDeleteShader(frag); + glDeleteShader(vert); +} + +GLuint GLShader::getUniformLocation(const char *name) { + auto it = uniform_loc_map.find(name); + if (it == uniform_loc_map.end()) { + it = uniform_loc_map.insert(it, {name, glGetUniformLocation(prog, name)}); + } + return it->second; +} diff --git a/selfdrive/common/glutil.h b/selfdrive/common/glutil.h index d907b09e7..dff87821f 100644 --- a/selfdrive/common/glutil.h +++ b/selfdrive/common/glutil.h @@ -1,21 +1,20 @@ -#ifndef COMMON_GLUTIL_H -#define COMMON_GLUTIL_H +#pragma once #ifdef __APPLE__ #include #else #include #endif +#include -#ifdef __cplusplus -extern "C" { -#endif +class GLShader { +public: + GLShader(const char *vert_src, const char *frag_src); + ~GLShader(); + GLuint getUniformLocation(const char * name); + GLuint prog = 0; -GLuint load_shader(GLenum shaderType, const char *src); -GLuint load_program(const char *vert_src, const char *frag_src); - -#ifdef __cplusplus -} -#endif - -#endif +private: + GLuint vert = 0, frag = 0; + std::map uniform_loc_map; +}; diff --git a/selfdrive/common/gpio.h b/selfdrive/common/gpio.h index 479b4f7e0..f50134269 100644 --- a/selfdrive/common/gpio.h +++ b/selfdrive/common/gpio.h @@ -1,5 +1,4 @@ -#ifndef GPIO_H -#define GPIO_H +#pragma once #include @@ -20,16 +19,5 @@ #define GPIO_STM_BOOT0 0 #endif - -#ifdef __cplusplus -extern "C" { -#endif - int gpio_init(int pin_nr, bool output); int gpio_set(int pin_nr, bool high); - -#ifdef __cplusplus -} // extern "C" -#endif - -#endif diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 9946b40f0..8d41b2b38 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,10 +1,18 @@ #pragma once +const int TRAJECTORY_SIZE = 33; +const float MIN_DRAW_DISTANCE = 10.0; +const float MAX_DRAW_DISTANCE = 100.0; -constexpr int MODEL_PATH_DISTANCE = 192; -constexpr int TRAJECTORY_SIZE = 33; -constexpr float MIN_DRAW_DISTANCE = 10.0; -constexpr float MAX_DRAW_DISTANCE = 100.0; -constexpr int POLYFIT_DEGREE = 4; -constexpr int SPEED_PERCENTILES = 10; -constexpr int DESIRE_PRED_SIZE = 32; -constexpr int OTHER_META_SIZE = 4; +const double T_IDXS[TRAJECTORY_SIZE] = {0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 , + 0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562, + 0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 , + 2.19726562, 2.5 , 2.82226562, 3.1640625 , 3.52539062, + 3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 , + 6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062, + 8.7890625 , 9.38476562, 10.}; +const double X_IDXS[TRAJECTORY_SIZE] = { 0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875, + 6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875, + 27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875, + 60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875, + 108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875, + 168.75 , 180.1875, 192.}; diff --git a/selfdrive/common/mutex.h b/selfdrive/common/mutex.h deleted file mode 100644 index ef0135935..000000000 --- a/selfdrive/common/mutex.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef COMMON_MUTEX_H -#define COMMON_MUTEX_H - -#include - -static inline void mutex_init_reentrant(pthread_mutex_t *mutex) { - pthread_mutexattr_t attr; - pthread_mutexattr_init(&attr); - pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); - pthread_mutex_init(mutex, &attr); -} - -#endif diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index ce079a996..930f3f0df 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -18,22 +18,12 @@ #include #include "common/util.h" -#include "common/utilpp.h" -std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) { - const char* env_val = getenv(env_var); - if (env_val != NULL){ - return std::string(env_val) + std::string(suffix); - } else { - return std::string(default_val); - } -} - #if defined(QCOM) || defined(QCOM2) const std::string default_params_path = "/data/params"; #else -const std::string default_params_path = getenv_default("HOME", "/.comma/params", "/data/params"); +const std::string default_params_path = util::getenv_default("HOME", "/.comma/params", "/data/params"); #endif #if defined(QCOM) || defined(QCOM2) @@ -266,7 +256,7 @@ std::string Params::get(std::string key, bool block){ size_t size; int r; - if (block){ + if (block) { r = read_db_value_blocking((const char*)key.c_str(), &value, &size); } else { r = read_db_value((const char*)key.c_str(), &value, &size); @@ -300,7 +290,7 @@ int Params::read_db_value_blocking(const char* key, char** value, size_t* value_ if (result == 0) { break; } else { - usleep(100000); // 0.1 s + util::sleep_for(100); // 0.1 s } } diff --git a/selfdrive/common/queue.h b/selfdrive/common/queue.h new file mode 100644 index 000000000..b3558b11a --- /dev/null +++ b/selfdrive/common/queue.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include + +template +class SafeQueue { +public: + SafeQueue() = default; + + void push(const T& v) { + { + std::unique_lock lk(m); + q.push(v); + } + cv.notify_one(); + } + + T pop() { + std::unique_lock lk(m); + cv.wait(lk, [this] { return !q.empty(); }); + T v = q.front(); + q.pop(); + return v; + } + + bool try_pop(T& v, int timeout_ms = 0) { + std::unique_lock lk(m); + if (!cv.wait_for(lk, std::chrono::milliseconds(timeout_ms), [this] { return !q.empty(); })) { + return false; + } + v = q.front(); + q.pop(); + return true; + } + + bool empty() const { + std::scoped_lock lk(m); + return q.empty(); + } + + size_t size() const { + std::scoped_lock lk(m); + return q.size(); + } + +private: + mutable std::mutex m; + std::condition_variable cv; + std::queue q; +}; diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index 9d9034754..5c69c0a94 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -6,29 +6,35 @@ #include #include -#include +#include #include #include "json11.hpp" -#include "common/timing.h" +#include "common/util.h" #include "common/version.h" #include "swaglog.h" -typedef struct LogState { - pthread_mutex_t lock; +class LogState { +public: + LogState() = default; + ~LogState(); + std::mutex lock; bool inited; json11::Json::object ctx_j; void *zctx; void *sock; int print_level; -} LogState; - -static LogState s = { - .lock = PTHREAD_MUTEX_INITIALIZER, }; +LogState::~LogState() { + zmq_close(sock); + zmq_ctx_destroy(zctx); +} + +static LogState s = {}; + static void cloudlog_bind_locked(const char* k, const char* v) { s.ctx_j[k] = v; } @@ -38,6 +44,10 @@ static void cloudlog_init() { s.ctx_j = json11::Json::object {}; s.zctx = zmq_ctx_new(); s.sock = zmq_socket(s.zctx, ZMQ_PUSH); + + int timeout = 100; // 100 ms timeout on shutdown for messages to be received by logmessaged + zmq_setsockopt(s.sock, ZMQ_LINGER, &timeout, sizeof(timeout)); + zmq_connect(s.sock, "ipc:///tmp/logmessage"); s.print_level = CLOUDLOG_WARNING; @@ -60,28 +70,38 @@ static void cloudlog_init() { cloudlog_bind_locked("version", COMMA_VERSION); s.ctx_j["dirty"] = !getenv("CLEAN"); + // device type + if (util::file_exists("/EON")) { + cloudlog_bind_locked("device", "eon"); + } else if (util::file_exists("/TICI")) { + cloudlog_bind_locked("device", "tici"); + } else { + cloudlog_bind_locked("device", "pc"); + } + s.inited = true; } +void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { + std::lock_guard lk(s.lock); + cloudlog_init(); + if (levelnum >= s.print_level) { + printf("%s: %s\n", filename, msg); + } + char levelnum_c = levelnum; + zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE); + zmq_send(s.sock, log_s.c_str(), log_s.length(), ZMQ_NOBLOCK); +} + void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) { - pthread_mutex_lock(&s.lock); - cloudlog_init(); - - char* msg_buf = NULL; + char* msg_buf = nullptr; va_list args; va_start(args, fmt); vasprintf(&msg_buf, fmt, args); va_end(args); - if (!msg_buf) { - pthread_mutex_unlock(&s.lock); - return; - } - - if (levelnum >= s.print_level) { - printf("%s: %s\n", filename, msg_buf); - } + if (!msg_buf) return; json11::Json log_j = json11::Json::object { {"msg", msg_buf}, @@ -92,21 +112,13 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func {"funcname", func}, {"created", seconds_since_epoch()} }; - std::string log_s = log_j.dump(); - + log(levelnum, filename, lineno, func, msg_buf, log_s); free(msg_buf); - - char levelnum_c = levelnum; - zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE); - zmq_send(s.sock, log_s.c_str(), log_s.length(), ZMQ_NOBLOCK); - - pthread_mutex_unlock(&s.lock); } void cloudlog_bind(const char* k, const char* v) { - pthread_mutex_lock(&s.lock); + std::lock_guard lk(s.lock); cloudlog_init(); cloudlog_bind_locked(k, v); - pthread_mutex_unlock(&s.lock); } diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index 3a828ed49..fae44152d 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -1,5 +1,4 @@ -#ifndef SWAGLOG_H -#define SWAGLOG_H +#pragma once #include "selfdrive/common/timing.h" @@ -9,19 +8,11 @@ #define CLOUDLOG_ERROR 40 #define CLOUDLOG_CRITICAL 50 -#ifdef __cplusplus -extern "C" { -#endif - void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; void cloudlog_bind(const char* k, const char* v); -#ifdef __cplusplus -} -#endif - #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \ __func__, \ fmt, ## __VA_ARGS__) @@ -64,5 +55,3 @@ void cloudlog_bind(const char* k, const char* v); #define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__) #define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) #define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__) - -#endif diff --git a/selfdrive/common/timing.h b/selfdrive/common/timing.h index 1a30ad6e1..48d031e51 100644 --- a/selfdrive/common/timing.h +++ b/selfdrive/common/timing.h @@ -23,7 +23,7 @@ static inline double millis_since_boot() { static inline double seconds_since_boot() { struct timespec t; clock_gettime(CLOCK_BOOTTIME, &t); - return (double)t.tv_sec + t.tv_nsec * 1e-9;; + return (double)t.tv_sec + t.tv_nsec * 1e-9; } static inline uint64_t nanos_since_epoch() { diff --git a/selfdrive/common/util.c b/selfdrive/common/util.cc similarity index 100% rename from selfdrive/common/util.c rename to selfdrive/common/util.cc diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index c7a1dbfc8..b3dfae7e7 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -1,35 +1,25 @@ -#ifndef COMMON_UTIL_H -#define COMMON_UTIL_H +#pragma once #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #ifndef sighandler_t typedef void (*sighandler_t)(int sig); #endif -#ifndef __cplusplus - -#define min(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - _a < _b ? _a : _b; }) - -#define max(a,b) \ - ({ __typeof__ (a) _a = (a); \ - __typeof__ (b) _b = (b); \ - _a > _b ? _a : _b; }) - -#endif - #define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) #undef ALIGN #define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1)) -#ifdef __cplusplus -extern "C" { -#endif - // Reads a file into a newly allocated buffer. // // Returns NULL on failure, otherwise the NULL-terminated file contents. @@ -42,8 +32,114 @@ void set_thread_name(const char* name); int set_realtime_priority(int level); int set_core_affinity(int core); -#ifdef __cplusplus -} -#endif +namespace util { +inline bool starts_with(const std::string &s, const std::string &prefix) { + return s.compare(0, prefix.size(), prefix) == 0; +} + +template +inline std::string string_format(const std::string& format, Args... args) { + size_t size = snprintf(nullptr, 0, format.c_str(), args...) + 1; + std::unique_ptr buf(new char[size]); + snprintf(buf.get(), size, format.c_str(), args...); + return std::string(buf.get(), buf.get() + size - 1); +} + +inline std::string read_file(const std::string &fn) { + std::ifstream t(fn); + std::stringstream buffer; + buffer << t.rdbuf(); + return buffer.str(); +} + +inline std::string tohex(const uint8_t* buf, size_t buf_size) { + std::unique_ptr hexbuf(new char[buf_size*2+1]); + for (size_t i=0; i < buf_size; i++) { + sprintf(&hexbuf[i*2], "%02x", buf[i]); + } + hexbuf[buf_size*2] = 0; + return std::string(hexbuf.get(), hexbuf.get() + buf_size*2); +} + +inline std::string base_name(std::string const & path) { + size_t pos = path.find_last_of("/"); + if (pos == std::string::npos) return path; + return path.substr(pos + 1); +} + +inline std::string dir_name(std::string const & path) { + size_t pos = path.find_last_of("/"); + if (pos == std::string::npos) return ""; + return path.substr(0, pos); +} + +inline std::string readlink(const std::string &path) { + char buff[4096]; + ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1); + if (len != -1) { + buff[len] = '\0'; + return std::string(buff); + } + return ""; +} + +inline std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) { + const char* env_val = getenv(env_var); + if (env_val != NULL){ + return std::string(env_val) + std::string(suffix); + } else { + return std::string(default_val); + } +} + +inline void sleep_for(const int milliseconds) { + std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); +} + +inline bool file_exists(const std::string& fn) { + std::ifstream f(fn); + return f.good(); +} + +} + +class ExitHandler { +public: + ExitHandler() { + std::signal(SIGINT, (sighandler_t)set_do_exit); + std::signal(SIGTERM, (sighandler_t)set_do_exit); + +#ifndef __APPLE__ + std::signal(SIGPWR, (sighandler_t)set_do_exit); #endif + }; + inline static std::atomic power_failure = false; + inline operator bool() { return do_exit; } + inline ExitHandler& operator=(bool v) { + do_exit = v; + return *this; + } +private: + static void set_do_exit(int sig) { +#ifndef __APPLE__ + power_failure = (sig == SIGPWR); +#endif + do_exit = true; + } + inline static std::atomic do_exit = false; +}; + +struct unique_fd { + unique_fd(int fd = -1) : fd_(fd) {} + unique_fd& operator=(unique_fd&& uf) { + fd_ = uf.fd_; + uf.fd_ = -1; + return *this; + } + ~unique_fd() { + if (fd_ != -1) close(fd_); + } + operator int() const { return fd_; } + int fd_; +}; diff --git a/selfdrive/common/utilpp.h b/selfdrive/common/utilpp.h deleted file mode 100644 index e0547e0c1..000000000 --- a/selfdrive/common/utilpp.h +++ /dev/null @@ -1,77 +0,0 @@ -#pragma once - -#include -#include - -#include -#include -#include -#include - -namespace util { - -inline bool starts_with(std::string s, std::string prefix) { - return s.compare(0, prefix.size(), prefix) == 0; -} - -template -inline std::string string_format( const std::string& format, Args ... args ) { - size_t size = snprintf( nullptr, 0, format.c_str(), args ... ) + 1; - std::unique_ptr buf( new char[ size ] ); - snprintf( buf.get(), size, format.c_str(), args ... ); - return std::string( buf.get(), buf.get() + size - 1 ); -} - -inline std::string read_file(std::string fn) { - std::ifstream t(fn); - std::stringstream buffer; - buffer << t.rdbuf(); - return buffer.str(); -} - -inline std::string tohex(const uint8_t* buf, size_t buf_size) { - std::unique_ptr hexbuf(new char[buf_size*2+1]); - for (size_t i=0; i < buf_size; i++) { - sprintf(&hexbuf[i*2], "%02x", buf[i]); - } - hexbuf[buf_size*2] = 0; - return std::string(hexbuf.get(), hexbuf.get() + buf_size*2); -} - -inline std::string base_name(std::string const & path) { - size_t pos = path.find_last_of("/"); - if (pos == std::string::npos) return path; - return path.substr(pos + 1); -} - -inline std::string dir_name(std::string const & path) { - size_t pos = path.find_last_of("/"); - if (pos == std::string::npos) return ""; - return path.substr(0, pos); -} - -inline std::string readlink(std::string path) { - char buff[4096]; - ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1); - if (len != -1) { - buff[len] = '\0'; - return std::string(buff); - } - return ""; -} - -} - -struct unique_fd { - unique_fd(int fd = -1) : fd_(fd) {} - unique_fd& operator=(unique_fd&& uf) { - fd_ = uf.fd_; - uf.fd_ = -1; - return *this; - } - ~unique_fd() { - if (fd_ != -1) close(fd_); - } - operator int() const { return fd_; } - int fd_; -}; diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 2958ce25f..978ae6351 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.8.1-80393615-2020-12-18T10:57:03" +#define COMMA_VERSION "0.8.2-280192ed-2021-02-23T16:46:20" diff --git a/selfdrive/common/visionbuf.h b/selfdrive/common/visionbuf.h deleted file mode 100644 index 8716b456c..000000000 --- a/selfdrive/common/visionbuf.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#define CL_USE_DEPRECATED_OPENCL_1_2_APIS -#ifdef __APPLE__ -#include -#else -#include -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -typedef struct VisionBuf { - size_t len; - size_t mmap_len; - void* addr; - int handle; - int fd; - - cl_context ctx; - cl_device_id device_id; - cl_mem buf_cl; - cl_command_queue copy_q; -} VisionBuf; - -#define VISIONBUF_SYNC_FROM_DEVICE 0 -#define VISIONBUF_SYNC_TO_DEVICE 1 - -VisionBuf visionbuf_allocate(size_t len); -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx); -void visionbuf_sync(const VisionBuf* buf, int dir); -void visionbuf_free(const VisionBuf* buf); - -#ifdef __cplusplus -} -#endif diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c deleted file mode 100644 index 1cf6ad6b8..000000000 --- a/selfdrive/common/visionbuf_cl.c +++ /dev/null @@ -1,101 +0,0 @@ -#include "visionbuf.h" - -#include -#include -#include -#include -#include -#include -#include -#include "common/clutil.h" - -#define CL_USE_DEPRECATED_OPENCL_1_2_APIS -#ifdef __APPLE__ -#include -#else -#include -#endif - -int offset = 0; -void *malloc_with_fd(size_t len, int *fd) { - char full_path[0x100]; -#ifdef __APPLE__ - snprintf(full_path, sizeof(full_path)-1, "/tmp/visionbuf_%d_%d", getpid(), offset++); -#else - snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++); -#endif - *fd = open(full_path, O_RDWR | O_CREAT, 0777); - assert(*fd >= 0); - unlink(full_path); - ftruncate(*fd, len); - void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0); - assert(addr != MAP_FAILED); - return addr; -} - -VisionBuf visionbuf_allocate(size_t len) { - // const size_t alignment = 4096; - // void* addr = aligned_alloc(alignment, alignment * ((len - 1) / alignment + 1)); - //void* addr = calloc(1, len); - - int fd; - void *addr = malloc_with_fd(len, &fd); - - return (VisionBuf){ - .len = len, .addr = addr, .handle = 1, .fd = fd, - }; -} - -VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx) { -#if __OPENCL_VERSION__ >= 200 - void* host_ptr = - clSVMAlloc(ctx, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, len, 0); - assert(host_ptr); -#else - int fd; - void* host_ptr = malloc_with_fd(len, &fd); - - cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(ctx, device_id, 0, &err)); -#endif - - cl_mem mem = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err)); - - return (VisionBuf){ - .len = len, .addr = host_ptr, .handle = 0, .fd = fd, - .device_id = device_id, .ctx = ctx, .buf_cl = mem, - -#if __OPENCL_VERSION__ < 200 - .copy_q = q, -#endif - - }; -} - -void visionbuf_sync(const VisionBuf* buf, int dir) { - if (!buf->buf_cl) return; - -#if __OPENCL_VERSION__ < 200 - if (dir == VISIONBUF_SYNC_FROM_DEVICE) { - CL_CHECK(clEnqueueReadBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL)); - } else { - CL_CHECK(clEnqueueWriteBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL)); - } - clFinish(buf->copy_q); -#endif -} - -void visionbuf_free(const VisionBuf* buf) { - if (buf->handle) { - munmap(buf->addr, buf->len); - close(buf->fd); - } else { - CL_CHECK(clReleaseMemObject(buf->buf_cl)); -#if __OPENCL_VERSION__ >= 200 - clSVMFree(buf->ctx, buf->addr); -#else - CL_CHECK(clReleaseCommandQueue(buf->copy_q)); - munmap(buf->addr, buf->len); - close(buf->fd); -#endif - } -} diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc index 5f3002992..fdd7d07c7 100644 --- a/selfdrive/common/visionimg.cc +++ b/selfdrive/common/visionimg.cc @@ -1,121 +1,62 @@ #include +#include "common/visionimg.h" #ifdef QCOM #include #include #include #include - -#include #define GL_GLEXT_PROTOTYPES #include - -#include -#define EGL_EGLEXT_PROTOTYPES -#include - -#endif - -#include "common/util.h" -#include "common/visionbuf.h" - -#include "common/visionimg.h" - -#ifdef QCOM - using namespace android; -// from libadreno_utils.so -extern "C" void compute_aligned_width_and_height(int width, - int height, - int bpp, - int tile_mode, - int raster_mode, - int padding_threshold, - int *aligned_w, - int *aligned_h); -#endif +EGLImageTexture::EGLImageTexture(const VisionBuf *buf) { + const int bpp = 3; + assert((buf->len % buf->stride) == 0); + assert((buf->stride % bpp) == 0); -void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) { -#if defined(QCOM) && !defined(QCOM_REPLAY) - compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h); -#else - *aligned_w = width; *aligned_h = height; -#endif -} - -VisionImg visionimg_alloc_rgb24(cl_device_id device_id, cl_context ctx, int width, int height, VisionBuf *out_buf) { - assert(out_buf != nullptr); - int aligned_w = 0, aligned_h = 0; - visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h); - - int stride = aligned_w * 3; - size_t size = (size_t) aligned_w * aligned_h * 3; - - *out_buf = visionbuf_allocate_cl(size, device_id, ctx); - - return (VisionImg){ - .fd = out_buf->fd, - .format = VISIONIMG_FORMAT_RGB24, - .width = width, - .height = height, - .stride = stride, - .size = size, - .bpp = 3, - }; -} - -#ifdef QCOM - -EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph) { - assert((img->size % img->stride) == 0); - assert((img->stride % img->bpp) == 0); - - int format = 0; - if (img->format == VISIONIMG_FORMAT_RGB24) { - format = HAL_PIXEL_FORMAT_RGB_888; - } else { - assert(false); - } - - private_handle_t* hnd = new private_handle_t(img->fd, img->size, + const int format = HAL_PIXEL_FORMAT_RGB_888; + private_handle = new private_handle_t(buf->fd, buf->len, private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER, 0, format, - img->stride/img->bpp, img->size/img->stride, - img->width, img->height); + buf->stride/bpp, buf->len/buf->stride, + buf->width, buf->height); - 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, EGLImageKHR *pkhr, void **pph) { - - EGLClientBuffer buf = visionimg_to_egl(img, pph); + // GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release. + GraphicBuffer* gb = new GraphicBuffer(buf->width, buf->height, (PixelFormat)format, + GraphicBuffer::USAGE_HW_TEXTURE, buf->stride/bpp, (private_handle_t*)private_handle, false); EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); assert(display != EGL_NO_DISPLAY); - EGLint img_attrs[] = { EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE }; - EGLImageKHR image = eglCreateImageKHR(display, EGL_NO_CONTEXT, - EGL_NATIVE_BUFFER_ANDROID, buf, img_attrs); - assert(image != EGL_NO_IMAGE_KHR); + EGLint img_attrs[] = {EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE}; + img_khr = eglCreateImageKHR(display, EGL_NO_CONTEXT, + EGL_NATIVE_BUFFER_ANDROID, gb->getNativeBuffer(), img_attrs); + assert(img_khr != EGL_NO_IMAGE_KHR); - GLuint tex = 0; - glGenTextures(1, &tex); - glBindTexture(GL_TEXTURE_2D, tex); - glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image); - *pkhr = image; - return tex; + glGenTextures(1, &frame_tex); + glBindTexture(GL_TEXTURE_2D, frame_tex); + glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, img_khr); } -void visionimg_destroy_gl(EGLImageKHR khr, void *ph) { +EGLImageTexture::~EGLImageTexture() { + glDeleteTextures(1, &frame_tex); EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); assert(display != EGL_NO_DISPLAY); - eglDestroyImageKHR(display, khr); - delete (private_handle_t*)ph; + eglDestroyImageKHR(display, img_khr); + delete (private_handle_t*)private_handle; } -#endif +#else // ifdef QCOM + +EGLImageTexture::EGLImageTexture(const VisionBuf *buf) { + glGenTextures(1, &frame_tex); + glBindTexture(GL_TEXTURE_2D, frame_tex); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, buf->width, buf->height, 0, GL_RGB, GL_UNSIGNED_BYTE, buf->addr); + glGenerateMipmap(GL_TEXTURE_2D); +} + +EGLImageTexture::~EGLImageTexture() { + glDeleteTextures(1, &frame_tex); +} +#endif // ifdef QCOM diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h index 14ee9f863..20a476c1a 100644 --- a/selfdrive/common/visionimg.h +++ b/selfdrive/common/visionimg.h @@ -1,41 +1,27 @@ -#ifndef VISIONIMG_H -#define VISIONIMG_H +#pragma once -#include "common/visionbuf.h" -#include "common/glutil.h" +#include "visionbuf.h" + +#ifdef __APPLE__ + #include +#else + #include +#endif #ifdef QCOM #include +#define EGL_EGLEXT_PROTOTYPES #include #undef Status -#else -typedef int EGLImageKHR; -typedef void *EGLClientBuffer; -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -#define VISIONIMG_FORMAT_RGB24 1 - -typedef struct VisionImg { - int fd; - int format; - int width, height, stride; - int bpp; - size_t size; -} VisionImg; - -void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h); -VisionImg visionimg_alloc_rgb24(cl_device_id device_id, cl_context ctx, int width, int height, VisionBuf *out_buf); - -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); - -#ifdef __cplusplus -} // extern "C" #endif +class EGLImageTexture { + public: + EGLImageTexture(const VisionBuf *buf); + ~EGLImageTexture(); + GLuint frame_tex = 0; +#ifdef QCOM + void *private_handle = nullptr; + EGLImageKHR img_khr = 0; #endif +}; diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c deleted file mode 100644 index 682049115..000000000 --- a/selfdrive/common/visionipc.c +++ /dev/null @@ -1,196 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "ipc.h" - -#include "visionipc.h" - -typedef struct VisionPacketWire { - int type; - VisionPacketData d; -} VisionPacketWire; - -int vipc_connect() { - return ipc_connect(VIPC_SOCKET_PATH); -} - - -int vipc_recv(int fd, VisionPacket *out_p) { - VisionPacketWire p = {0}; - VisionPacket p2 = {0}; - int ret = ipc_sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds); - if (ret < 0) { - printf("vipc_recv err: %s\n", strerror(errno)); - } else { - p2.type = p.type; - p2.d = p.d; - *out_p = p2; - } - //printf("%d = vipc_recv(%d, %d): %d %d %d %zu\n", ret, fd, p2.num_fds, out_p->d.stream_bufs.type, out_p->d.stream_bufs.width, out_p->d.stream_bufs.height, out_p->d.stream_bufs.buf_len); - return ret; -} - -int vipc_send(int fd, const VisionPacket *p2) { - assert(p2->num_fds <= VIPC_MAX_FDS); - - VisionPacketWire p = { - .type = p2->type, - .d = p2->d, - }; - int ret = ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL); - //printf("%d = vipc_send(%d, %d): %d %d %d %zu\n", ret, fd, p2->num_fds, p2->d.stream_bufs.type, p2->d.stream_bufs.width, p2->d.stream_bufs.height, p2->d.stream_bufs.buf_len); - return ret; -} - -void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs, - int num_fds, const int* fds) { - for (int i=0; ibuf_len; - bufs[i].addr = mmap(NULL, bufs[i].len, - PROT_READ | PROT_WRITE, - MAP_SHARED, bufs[i].fd, 0); - // printf("b %d %zu -> %p\n", bufs[i].fd, bufs[i].len, bufs[i].addr); - assert(bufs[i].addr != MAP_FAILED); - } -} - - -int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info) { - int err; - - memset(s, 0, sizeof(*s)); - - s->last_idx = -1; - - s->ipc_fd = vipc_connect(); - if (s->ipc_fd < 0) return -1; - - VisionPacket p = { - .type = VIPC_STREAM_SUBSCRIBE, - .d = { .stream_sub = { - .type = type, - .tbuffer = tbuffer, - }, }, - }; - err = vipc_send(s->ipc_fd, &p); - if (err < 0) { - close(s->ipc_fd); - s->ipc_fd = -1; - return -1; - } - - VisionPacket rp; - err = vipc_recv(s->ipc_fd, &rp); - if (err <= 0) { - close(s->ipc_fd); - s->ipc_fd = -1; - return -1; - } - assert(rp.type == VIPC_STREAM_BUFS); - assert(rp.d.stream_bufs.type == type); - - s->bufs_info = rp.d.stream_bufs; - - s->num_bufs = rp.num_fds; - s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf)); - assert(s->bufs); - - vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds); - - if (out_bufs_info) { - *out_bufs_info = s->bufs_info; - } - - return 0; -} - -void visionstream_release(VisionStream *s) { - int err; - if (s->last_idx >= 0) { - VisionPacket rep = { - .type = VIPC_STREAM_RELEASE, - .d = { .stream_rel = { - .type = s->last_type, - .idx = s->last_idx, - }} - }; - err = vipc_send(s->ipc_fd, &rep); - s->last_idx = -1; - } -} - -VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) { - int err; - - VisionPacket rp; - err = vipc_recv(s->ipc_fd, &rp); - if (err <= 0) { - return NULL; - } - assert(rp.type == VIPC_STREAM_ACQUIRE); - - if (s->last_idx >= 0) { - VisionPacket rep = { - .type = VIPC_STREAM_RELEASE, - .d = { .stream_rel = { - .type = s->last_type, - .idx = s->last_idx, - }} - }; - err = vipc_send(s->ipc_fd, &rep); - if (err <= 0) { - return NULL; - } - } - - s->last_type = rp.d.stream_acq.type; - s->last_idx = rp.d.stream_acq.idx; - assert(s->last_idx < s->num_bufs); - - if (out_extra) { - *out_extra = rp.d.stream_acq.extra; - } - - return &s->bufs[s->last_idx]; -} - -void visionstream_destroy(VisionStream *s) { - int err; - - if (s->last_idx >= 0) { - VisionPacket rep = { - .type = VIPC_STREAM_RELEASE, - .d = { .stream_rel = { - .type = s->last_type, - .idx = s->last_idx, - }} - }; - err = vipc_send(s->ipc_fd, &rep); - s->last_idx = -1; - } - - for (int i=0; inum_bufs; i++) { - if (s->bufs[i].addr) { - munmap(s->bufs[i].addr, s->bufs[i].len); - s->bufs[i].addr = NULL; - close(s->bufs[i].fd); - } - } - if (s->bufs) free(s->bufs); - if (s->ipc_fd >= 0) close(s->ipc_fd); -} diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h deleted file mode 100644 index e20500db6..000000000 --- a/selfdrive/common/visionipc.h +++ /dev/null @@ -1,120 +0,0 @@ -#ifndef VISIONIPC_H -#define VISIONIPC_H - -#include -#include -#include - -#define VIPC_SOCKET_PATH "/tmp/vision_socket" -#define VIPC_MAX_FDS 64 - -#ifdef __cplusplus -extern "C" { -#endif - -typedef enum VisionIPCPacketType { - VIPC_INVALID = 0, - VIPC_STREAM_SUBSCRIBE, - VIPC_STREAM_BUFS, - VIPC_STREAM_ACQUIRE, - VIPC_STREAM_RELEASE, -} VisionIPCPacketType; - -typedef enum VisionStreamType { - VISION_STREAM_RGB_BACK, - VISION_STREAM_RGB_FRONT, - VISION_STREAM_RGB_WIDE, - VISION_STREAM_YUV, - VISION_STREAM_YUV_FRONT, - VISION_STREAM_YUV_WIDE, - VISION_STREAM_MAX, -} VisionStreamType; - -typedef struct VisionUIInfo { - int big_box_x, big_box_y; - int big_box_width, big_box_height; - int transformed_width, transformed_height; - - int front_box_x, front_box_y; - int front_box_width, front_box_height; - - int wide_box_x, wide_box_y; - int wide_box_width, wide_box_height; - -} VisionUIInfo; - -typedef struct VisionStreamBufs { - VisionStreamType type; - - int width, height, stride; - size_t buf_len; - - union { - VisionUIInfo ui_info; - } buf_info; -} VisionStreamBufs; - -typedef struct VIPCBufExtra { - // only for yuv - uint32_t frame_id; - uint64_t timestamp_sof; - uint64_t timestamp_eof; -} VIPCBufExtra; - -typedef union VisionPacketData { - struct { - VisionStreamType type; - bool tbuffer; - } stream_sub; - VisionStreamBufs stream_bufs; - struct { - VisionStreamType type; - int idx; - VIPCBufExtra extra; - } stream_acq; - struct { - VisionStreamType type; - int idx; - } stream_rel; -} VisionPacketData; - -typedef struct VisionPacket { - int type; - VisionPacketData d; - int num_fds; - int fds[VIPC_MAX_FDS]; -} VisionPacket; - -int vipc_connect(void); -int vipc_recv(int fd, VisionPacket *out_p); -int vipc_send(int fd, const VisionPacket *p); - -typedef struct VIPCBuf { - int fd; - size_t len; - void* addr; -} VIPCBuf; -void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs, - int num_fds, const int* fds); - - - -typedef struct VisionStream { - int ipc_fd; - int last_idx; - int last_type; - int num_bufs; - VisionStreamBufs bufs_info; - VIPCBuf *bufs; -} VisionStream; - -int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info); -void visionstream_release(VisionStream *s); -VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra); -void visionstream_destroy(VisionStream *s); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/selfdrive/config.py b/selfdrive/config.py index 2a8958088..511f6126c 100644 --- a/selfdrive/config.py +++ b/selfdrive/config.py @@ -11,7 +11,7 @@ class Conversions: MS_TO_KNOTS = 1.9438 KNOTS_TO_MS = 1. / MS_TO_KNOTS #Angle - DEG_TO_RAD = np.pi/180. + DEG_TO_RAD = np.pi / 180. RAD_TO_DEG = 1. / DEG_TO_RAD #Mass LB_TO_KG = 0.453592 @@ -22,8 +22,8 @@ RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame class UIParams: lidar_x, lidar_y, lidar_zoom = 384, 960, 6 - lidar_car_x, lidar_car_y = lidar_x/2., lidar_y/1.1 - car_hwidth = 1.7272/2 * lidar_zoom + lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1 + car_hwidth = 1.7272 / 2 * lidar_zoom car_front = 2.6924 * lidar_zoom car_back = 1.8796 * lidar_zoom car_color = 110 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 184077c20..58a9ac633 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -7,6 +7,7 @@ from common.profiler import Profiler from common.params import Params, put_nonblocking import cereal.messaging as messaging from selfdrive.config import Conversions as CV +from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET @@ -18,9 +19,9 @@ from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.controls.lib.planner import LON_MPC_STEP +from selfdrive.controls.lib.longitudinal_planner import LON_MPC_STEP from selfdrive.locationd.calibrationd import Calibration -from selfdrive.hardware import HARDWARE +from selfdrive.hardware import HARDWARE, TICI LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 @@ -29,14 +30,15 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees SIMULATION = "SIMULATION" in os.environ NOSENSOR = "NOSENSOR" in os.environ +IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned"]) -ThermalStatus = log.ThermalData.ThermalStatus +ThermalStatus = log.DeviceState.ThermalStatus State = log.ControlsState.OpenpilotState -HwType = log.HealthData.HwType -LongitudinalPlanSource = log.Plan.LongitudinalPlanSource -Desire = log.PathPlan.Desire -LaneChangeState = log.PathPlan.LaneChangeState -LaneChangeDirection = log.PathPlan.LaneChangeDirection +PandaType = log.PandaState.PandaType +LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource +Desire = log.LateralPlan.Desire +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection EventName = car.CarEvent.EventName @@ -52,15 +54,17 @@ class Controls: self.sm = sm if self.sm is None: - self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'frontFrame', - 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) + ignore = ['ubloxRaw', 'driverCameraState', 'managerState'] if SIMULATION else None + self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'ubloxRaw', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', + 'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore) self.can_sock = can_sock if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 self.can_sock = messaging.sub_sock('can', timeout=can_timeout) - # wait for one health and one CAN packet + # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") get_one_can(self.can_sock) @@ -70,11 +74,9 @@ class Controls: params = Params() self.is_metric = params.get("IsMetric", encoding='utf8') == "1" self.is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" - internet_needed = (params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None) and (params.get("DisableUpdates") != b"1") community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" - passive = params.get("Passive", encoding='utf8') == "1" or \ - internet_needed or not openpilot_enabled_toggle + passive = params.get("Passive", encoding='utf8') == "1" or not openpilot_enabled_toggle # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() @@ -122,19 +124,18 @@ class Controls: self.last_functional_fan_frame = 0 self.events_prev = [] self.current_alert_types = [ET.PERMANENT] + self.logged_comm_issue = False self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED - self.sm['thermal'].freeSpace = 1. - self.sm['dMonitoringState'].events = [] - self.sm['dMonitoringState'].awarenessStatus = 1. - self.sm['dMonitoringState'].faceDetected = False + self.sm['deviceState'].freeSpacePercent = 100 + self.sm['driverMonitoringState'].events = [] + self.sm['driverMonitoringState'].awarenessStatus = 1. + self.sm['driverMonitoringState'].faceDetected = False self.startup_event = get_startup_event(car_recognized, controller_available) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) - if internet_needed: - self.events.add(EventName.internetConnectivityNeeded, static=True) if community_feature_disallowed: self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: @@ -149,7 +150,7 @@ class Controls: self.events.clear() self.events.add_from_msg(CS.events) - self.events.add_from_msg(self.sm['dMonitoringState'].events) + self.events.add_from_msg(self.sm['driverMonitoringState'].events) # Handle startup event if self.startup_event is not None: @@ -157,20 +158,20 @@ class Controls: self.startup_event = None # Create events for battery, temperature, disk space, and memory - if self.sm['thermal'].batteryPercent < 1 and self.sm['thermal'].chargingError: + if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError: # at zero percent battery, while discharging, OP should not allowed self.events.add(EventName.lowBattery) - if self.sm['thermal'].thermalStatus >= ThermalStatus.red: + if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: self.events.add(EventName.overheat) - if self.sm['thermal'].freeSpace < 0.07: + if self.sm['deviceState'].freeSpacePercent < 7: # under 7% of space free no enable allowed self.events.add(EventName.outOfSpace) - if self.sm['thermal'].memUsedPercent > 90: + if self.sm['deviceState'].memoryUsagePercent > 90: self.events.add(EventName.lowMemory) # Alert if fan isn't spinning for 5 seconds - if self.sm['health'].hwType in [HwType.uno, HwType.dos]: - if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeed > 50: + if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]: + if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: self.events.add(EventName.fanMalfunction) else: @@ -185,8 +186,8 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange: - direction = self.sm['pathPlan'].laneChangeDirection + if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: + direction = self.sm['lateralPlan'].laneChangeDirection if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) @@ -195,51 +196,62 @@ class Controls: self.events.add(EventName.preLaneChangeLeft) else: self.events.add(EventName.preLaneChangeRight) - elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, + elif self.sm['lateralPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL): self.events.add(EventName.canError) - if (self.sm['health'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \ - self.mismatch_counter >= 200: + if (self.sm['pandaState'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \ + self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) - if not self.sm.alive['plan'] and self.sm.alive['pathPlan']: - # only plan not being received: radar not communicating - self.events.add(EventName.radarCommIssue) + + if len(self.sm['radarState'].radarErrors): + self.events.add(EventName.radarFault) + elif not self.sm.valid['liveParameters']: + self.events.add(EventName.vehicleModelInvalid) elif not self.sm.all_alive_and_valid(): self.events.add(EventName.commIssue) - if not self.sm['pathPlan'].mpcSolutionValid: + if not self.logged_comm_issue: + cloudlog.error(f"commIssue - valid: {self.sm.valid} - alive: {self.sm.alive}") + self.logged_comm_issue = True + else: + self.logged_comm_issue = False + + if not self.sm['lateralPlan'].mpcSolutionValid: self.events.add(EventName.plannerError) if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR: if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs self.events.add(EventName.sensorDataInvalid) - if not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000): - # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes - if not (SIMULATION or NOSENSOR): # TODO: send GPS in carla - self.events.add(EventName.noGps) - if not self.sm['pathPlan'].paramsValid: - self.events.add(EventName.vehicleModelInvalid) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: self.events.add(EventName.deviceFalling) - if not self.sm['plan'].radarValid: - self.events.add(EventName.radarFault) - if self.sm['plan'].radarCanError: - self.events.add(EventName.radarCanError) - if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults: + if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults: self.events.add(EventName.relayMalfunction) - if self.sm['plan'].fcw: + if self.sm['longitudinalPlan'].fcw: self.events.add(EventName.fcw) - if not self.sm.alive['frontFrame'] and (self.sm.frame > 5 / DT_CTRL) and not SIMULATION: - self.events.add(EventName.cameraMalfunction) - if self.sm['model'].frameDropPerc > 20 and not SIMULATION: - self.events.add(EventName.modeldLagging) + # TODO: fix simulator + if not SIMULATION: + if not NOSENSOR: + if not self.sm.alive['ubloxRaw'] and (self.sm.frame > 10. / DT_CTRL): + self.events.add(EventName.gpsMalfunction) + elif not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI: + # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes + self.events.add(EventName.noGps) + if not self.sm.all_alive(['roadCameraState', 'driverCameraState']) and (self.sm.frame > 5 / DT_CTRL): + self.events.add(EventName.cameraMalfunction) + if self.sm['modelV2'].frameDropPerc > 20: + self.events.add(EventName.modeldLagging) + + # Check if all manager processes are running + not_running = set(p.name for p in self.sm['managerState'].processes if not p.running) + if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES): + self.events.add(EventName.processNotRunning) # Only allow engagement with brake pressed when stopped behind another stopped car - if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ + if CS.brakePressed and self.sm['longitudinalPlan'].vTargetFuture >= STARTING_TARGET_SPEED \ and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3: self.events.add(EventName.noTarget) @@ -266,7 +278,7 @@ class Controls: if not self.enabled: self.mismatch_counter = 0 - if not self.sm['health'].controlsAllowed and self.enabled: + if not self.sm['pandaState'].controlsAllowed and self.enabled: self.mismatch_counter += 1 self.distance_traveled += CS.vEgo * DT_CTRL @@ -353,8 +365,8 @@ class Controls: def state_control(self, CS): """Given the state, this function returns an actuators packet""" - plan = self.sm['plan'] - path_plan = self.sm['pathPlan'] + lat_plan = self.sm['lateralPlan'] + long_plan = self.sm['longitudinalPlan'] actuators = car.CarControl.Actuators.new_message() @@ -367,21 +379,21 @@ class Controls: self.LaC.reset() self.LoC.reset(v_pid=CS.vEgo) - plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['plan']) + long_plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) # no greater than dt mpc + dt, to prevent too high extraps - dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL + dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL - a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart) - v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 + a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart) + v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0 # Gas/Brake PID loop - actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP) + actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan) + actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, lat_plan) # Check for difference between desired angle and angle for angle based control angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \ - abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD + abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD if angle_control_saturated and not CS.steeringPressed and self.active: self.saturated_count += 1 @@ -392,8 +404,8 @@ class Controls: if (lac_log.saturated and not CS.steeringPressed) or \ (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): # Check if we deviated from the path - left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 - right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 + left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1 + right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1 if left_deviation or right_deviation: self.events.add(EventName.steerSaturated) @@ -415,15 +427,15 @@ class Controls: brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) CC.cruiseControl.speedOverride = float(speed_override if self.CP.enableCruise else 0.0) - CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget) + CC.cruiseControl.accelOverride = self.CI.calc_accel_override(CS.aEgo, self.sm['longitudinalPlan'].aTarget, CS.vEgo, self.sm['longitudinalPlan'].vTarget) CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = self.enabled CC.hudControl.lanesVisible = self.enabled - CC.hudControl.leadVisible = self.sm['plan'].hasLead + CC.hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead - right_lane_visible = self.sm['pathPlan'].rProb > 0.5 - left_lane_visible = self.sm['pathPlan'].lProb > 0.5 + right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 + left_lane_visible = self.sm['lateralPlan'].lProb > 0.5 CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) @@ -431,12 +443,12 @@ class Controls: ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED - meta = self.sm['model'].meta + meta = self.sm['modelV2'].meta if len(meta.desirePrediction) and ldw_allowed: l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] - l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) - r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) + l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET)) CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) @@ -455,10 +467,10 @@ class Controls: can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) - force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \ + force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) - steer_angle_rad = (CS.steeringAngle - self.sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD + steer_angle_rad = (CS.steeringAngleDeg - self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD # controlsState dat = messaging.new_message('controlsState') @@ -471,17 +483,12 @@ class Controls: controlsState.alertBlinkingRate = self.AM.alert_rate controlsState.alertType = self.AM.alert_type controlsState.alertSound = self.AM.audible_alert - controlsState.driverMonitoringOn = self.sm['dMonitoringState'].faceDetected controlsState.canMonoTimes = list(CS.canMonoTimes) - controlsState.planMonoTime = self.sm.logMonoTime['plan'] - controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan'] + controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime['lateralPlan'] controlsState.enabled = self.enabled controlsState.active = self.active - controlsState.vEgo = CS.vEgo - controlsState.vEgoRaw = CS.vEgoRaw - controlsState.angleSteers = CS.steeringAngle controlsState.curvature = self.VM.calc_curvature(steer_angle_rad, CS.vEgo) - controlsState.steerOverride = CS.steeringPressed controlsState.state = self.state controlsState.engageable = not self.events.any(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state @@ -490,16 +497,11 @@ class Controls: controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) - controlsState.angleSteersDes = float(self.LaC.angle_steers_des) + controlsState.steeringAngleDesiredDeg = float(self.LaC.angle_steers_des) controlsState.vTargetLead = float(v_acc) controlsState.aTarget = float(a_acc) - controlsState.jerkFactor = float(self.sm['plan'].jerkFactor) - controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive - controlsState.vCurvature = self.sm['plan'].vCurvature - controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) - controlsState.mapValid = self.sm['plan'].mapValid controlsState.forceDecel = bool(force_decel) controlsState.canErrorCounter = self.can_error_counter diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index a47eb8c0a..79642ccef 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -13,7 +13,7 @@ "_comment": "Append the number of days at the end of the text" }, "Offroad_ConnectivityNeeded": { - "text": "Connect to internet to check for updates. openpilot won't engage until it connects to internet to check for updates.", + "text": "Connect to internet to check for updates. openpilot won't start until it connects to internet to check for updates.", "severity": 1 }, "Offroad_UpdateFailed": { diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ea7002475..ba433066d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,15 +3,15 @@ from selfdrive.config import Conversions as CV from cereal import car # kph -V_CRUISE_MAX = 144 +V_CRUISE_MAX = 135 V_CRUISE_MIN = 8 V_CRUISE_DELTA = 8 V_CRUISE_ENABLE_MIN = 40 - +MPC_N = 16 +CAR_ROTATION_RADIUS = 0.0 class MPC_COST_LAT: PATH = 1.0 - LANE = 3.0 HEADING = 1.0 STEER_RATE = 1.0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index a8f1273e1..49336c45d 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -195,7 +195,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2) def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: - gps_integrated = sm['health'].hwType in [log.HealthData.HwType.uno, log.HealthData.HwType.dos] + gps_integrated = sm['pandaState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] return Alert( "Poor GPS reception", "If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement", @@ -468,6 +468,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo ET.PERMANENT: NormalPermanentAlert("Camera Malfunction", "Contact Support"), }, + EventName.gpsMalfunction: { + ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Contact Support"), + }, + # ********** events that affect controls state transitions ********** EventName.pcmEnable: { @@ -611,17 +615,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo audible_alert=AudibleAlert.chimeDisengage), }, - EventName.radarCommIssue: { - ET.SOFT_DISABLE: SoftDisableAlert("Radar Communication Issue"), - ET.NO_ENTRY: NoEntryAlert("Radar Communication Issue", + EventName.processNotRunning: { + ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device", audible_alert=AudibleAlert.chimeDisengage), }, - EventName.radarCanError: { - ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), - ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), - }, - EventName.radarFault: { ET.SOFT_DISABLE: SoftDisableAlert("Radar Error: Restart the Car"), ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"), @@ -743,13 +741,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), }, - # TODO: this is unclear, update check only happens offroad - EventName.internetConnectivityNeeded: { - ET.PERMANENT: NormalPermanentAlert("Connect to Internet", "An Update Check Is Required to Engage"), - ET.NO_ENTRY: NoEntryAlert("Connect to Internet", - audible_alert=AudibleAlert.chimeDisengage), - }, - EventName.lowSpeedLockout: { ET.PERMANENT: Alert( "Cruise Fault: Restart the car to engage", diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 789ef8571..bb3ba5f52 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,105 +1,91 @@ from common.numpy_fast import interp import numpy as np +from selfdrive.hardware import EON, TICI from cereal import log -CAMERA_OFFSET = 0.06 # m from center car to camera +TRAJECTORY_SIZE = 33 +# camera offset is meters from center car to camera +if EON: + CAMERA_OFFSET = 0.06 +elif TICI: + CAMERA_OFFSET = -0.04 +else: + CAMERA_OFFSET = 0.0 -def compute_path_pinv(length=50): - deg = 3 - x = np.arange(length*1.0) - X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T - pinv = np.linalg.pinv(X) - return pinv - - -def model_polyfit(points, path_pinv): - return np.dot(path_pinv, [float(x) for x in points]) - - -def eval_poly(poly, x): - return poly[3] + poly[2]*x + poly[1]*x**2 + poly[0]*x**3 class LanePlanner: def __init__(self): - self.l_poly = [0., 0., 0., 0.] - self.r_poly = [0., 0., 0., 0.] - self.p_poly = [0., 0., 0., 0.] - self.d_poly = [0., 0., 0., 0.] - + self.ll_t = np.zeros((TRAJECTORY_SIZE,)) + self.ll_x = np.zeros((TRAJECTORY_SIZE,)) + self.lll_y = np.zeros((TRAJECTORY_SIZE,)) + self.rll_y = np.zeros((TRAJECTORY_SIZE,)) self.lane_width_estimate = 3.7 self.lane_width_certainty = 1.0 self.lane_width = 3.7 - self.l_prob = 0. - self.r_prob = 0. + self.lll_prob = 0. + self.rll_prob = 0. + self.d_prob = 0. - self.l_std = 0. - self.r_std = 0. + self.lll_std = 0. + self.rll_std = 0. self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. - self._path_pinv = compute_path_pinv() - self.x_points = np.arange(50) def parse_model(self, md): - if len(md.leftLane.poly): - self.l_poly = np.array(md.leftLane.poly) - self.l_std = float(md.leftLane.std) - self.r_poly = np.array(md.rightLane.poly) - self.r_std = float(md.rightLane.std) - self.p_poly = np.array(md.path.poly) - else: - self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv) # left line - self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv) # right line - self.p_poly = model_polyfit(md.path.points, self._path_pinv) # predicted path - self.l_prob = md.leftLane.prob # left line prob - self.r_prob = md.rightLane.prob # right line prob + if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE: + self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2 + # left and right ll x is the same + self.ll_x = md.laneLines[1].x + # only offset left and right lane lines; offsetting path does not make sense + self.lll_y = np.array(md.laneLines[1].y) - CAMERA_OFFSET + self.rll_y = np.array(md.laneLines[2].y) - CAMERA_OFFSET + self.lll_prob = md.laneLineProbs[1] + self.rll_prob = md.laneLineProbs[2] + self.lll_std = md.laneLineStds[1] + self.rll_std = md.laneLineStds[2] if len(md.meta.desireState): - self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft] - self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight] - - def update_d_poly(self, v_ego): - # only offset left and right lane lines; offsetting p_poly does not make sense - self.l_poly[3] += CAMERA_OFFSET - self.r_poly[3] += CAMERA_OFFSET + self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft] + self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight] + def get_d_path(self, v_ego, path_t, path_xyz): # Reduce reliance on lanelines that are too far apart or # will be in a few seconds - l_prob, r_prob = self.l_prob, self.r_prob - width_poly = self.l_poly - self.r_poly + l_prob, r_prob = self.lll_prob, self.rll_prob + width_pts = self.rll_y - self.lll_y prob_mods = [] for t_check in [0.0, 1.5, 3.0]: - width_at_t = eval_poly(width_poly, t_check * (v_ego + 7)) + width_at_t = interp(t_check * (v_ego + 7), self.ll_x, width_pts) prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0])) mod = min(prob_mods) l_prob *= mod r_prob *= mod # Reduce reliance on uncertain lanelines - l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0]) - r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0]) + l_std_mod = interp(self.lll_std, [.15, .3], [1.0, 0.0]) + r_std_mod = interp(self.rll_std, [.15, .3], [1.0, 0.0]) l_prob *= l_std_mod r_prob *= r_std_mod # Find current lanewidth self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty) - current_lane_width = abs(self.l_poly[3] - self.r_poly[3]) + current_lane_width = abs(self.rll_y[0] - self.lll_y[0]) self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5]) self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \ (1 - self.lane_width_certainty) * speed_lane_width clipped_lane_width = min(4.0, self.lane_width) - path_from_left_lane = self.l_poly.copy() - path_from_left_lane[3] -= clipped_lane_width / 2.0 - path_from_right_lane = self.r_poly.copy() - path_from_right_lane[3] += clipped_lane_width / 2.0 + path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 + path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 - lr_prob = l_prob + r_prob - l_prob * r_prob - - d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) - self.d_poly = lr_prob * d_poly_lane + (1.0 - lr_prob) * self.p_poly.copy() + self.d_prob = l_prob + r_prob - l_prob * r_prob + lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) + lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y) + path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1] + return path_xyz diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 51872f6d0..ca0532e25 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -3,8 +3,8 @@ import numpy as np from cereal import log from common.realtime import DT_CTRL -from common.numpy_fast import clip -from selfdrive.car.toyota.values import SteerLimitParams +from common.numpy_fast import clip, interp +from selfdrive.car.toyota.values import CarControllerParams from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.controls.lib.drive_helpers import get_steer_max @@ -28,27 +28,45 @@ class LatControlINDI(): [7.29394177e+00, 1.39159419e-02], [1.71022442e+01, 3.38495381e-02]]) + self.speed = 0. + self.K = K self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) self.enforce_rate_limit = CP.carName == "toyota" - self.RC = CP.lateralTuning.indi.timeConstant - self.G = CP.lateralTuning.indi.actuatorEffectiveness - self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain - self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain - self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) + self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) + self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) + self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) + self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.reset() + @property + def RC(self): + return interp(self.speed, self._RC[0], self._RC[1]) + + @property + def G(self): + return interp(self.speed, self._G[0], self._G[1]) + + @property + def outer_loop_gain(self): + return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1]) + + @property + def inner_loop_gain(self): + return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) + def reset(self): self.delayed_output = 0. self.output_steer = 0. self.sat_count = 0.0 + self.speed = 0. def _check_saturation(self, control, check_saturation, limit): saturated = abs(control) == limit @@ -62,29 +80,31 @@ class LatControlINDI(): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, path_plan): + def update(self, active, CS, CP, lat_plan): + self.speed = CS.vEgo # Update Kalman filter - y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]]) + y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() - indi_log.steerAngle = math.degrees(self.x[0]) - indi_log.steerRate = math.degrees(self.x[1]) - indi_log.steerAccel = math.degrees(self.x[2]) + indi_log.steeringAngleDeg = math.degrees(self.x[0]) + indi_log.steeringRateDeg = math.degrees(self.x[1]) + indi_log.steeringAccelDeg = math.degrees(self.x[2]) if CS.vEgo < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.delayed_output = 0.0 else: - self.angle_steers_des = path_plan.angleSteers - self.rate_steers_des = path_plan.rateSteers + self.angle_steers_des = lat_plan.steeringAngleDeg + self.rate_steers_des = lat_plan.steeringRateDeg steers_des = math.radians(self.angle_steers_des) rate_des = math.radians(self.rate_steers_des) # Expected actuator value - self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha) + alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) + self.delayed_output = self.delayed_output * alpha + self.output_steer * (1. - alpha) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des @@ -95,12 +115,16 @@ class LatControlINDI(): g_inv = 1. / self.G delta_u = g_inv * accel_error + # If steering pressed, only allow wind down + if CS.steeringPressed and (delta_u * self.output_steer > 0): + delta_u = 0 + # Enforce rate limit if self.enforce_rate_limit: - steer_max = float(SteerLimitParams.STEER_MAX) + steer_max = float(CarControllerParams.STEER_MAX) new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) prev_output_steer_cmd = steer_max * self.output_steer - new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams) + new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) self.output_steer = new_output_steer_cmd / steer_max else: self.output_steer = self.delayed_output + delta_u diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index cb54e52eb..1d9356be4 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -43,17 +43,17 @@ class LatControlLQR(): return self.sat_count > self.sat_limit - def update(self, active, CS, CP, path_plan): + def update(self, active, CS, CP, lat_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed - steering_angle = CS.steeringAngle + steering_angle = CS.steeringAngleDeg # Subtract offset. Zero angle should correspond to zero torque - self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset - steering_angle -= path_plan.angleOffset + self.angle_steers_des = lat_plan.steeringAngleDeg - lat_plan.angleOffsetDeg + steering_angle -= lat_plan.angleOffsetDeg # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) @@ -89,7 +89,7 @@ class LatControlLQR(): check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) - lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset + lqr_log.steeringAngleDeg = angle_steers_k + lat_plan.angleOffsetDeg lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index aa069e81f..be521b0cf 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -15,30 +15,30 @@ class LatControlPID(): def reset(self): self.pid.reset() - def update(self, active, CS, CP, path_plan): + def update(self, active, CS, CP, lat_plan): pid_log = log.ControlsState.LateralPIDState.new_message() - pid_log.steerAngle = float(CS.steeringAngle) - pid_log.steerRate = float(CS.steeringRate) + pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) + pid_log.steeringRateDeg = float(CS.steeringRateDeg) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: - self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: - # TODO: feedforward something based on path_plan.rateSteers - steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque + # TODO: feedforward something based on lat_plan.rateSteers + steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed - output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, + output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript index 6cdc22d17..15dbeb19b 100644 --- a/selfdrive/controls/lib/lateral_mpc/SConscript +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -1,29 +1,47 @@ Import('env', 'arch') cpp_path = [ + "#selfdrive", "#phonelibs/acado/include", "#phonelibs/acado/include/acado", "#phonelibs/qpoases/INCLUDE", "#phonelibs/qpoases/INCLUDE/EXTRAS", "#phonelibs/qpoases/SRC/", "#phonelibs/qpoases", - "lib_mpc_export" - + "lib_mpc_export", ] -mpc_files = [ - "lateral_mpc.c", - Glob("lib_mpc_export/*.c"), - Glob("lib_mpc_export/*.cpp"), +generated_c = [ + 'lib_mpc_export/acado_auxiliary_functions.c', + 'lib_mpc_export/acado_qpoases_interface.cpp', + 'lib_mpc_export/acado_integrator.c', + 'lib_mpc_export/acado_solver.c', +] + +generated_h = [ + 'lib_mpc_export/acado_common.h', + 'lib_mpc_export/acado_auxiliary_functions.h', + 'lib_mpc_export/acado_qpoases_interface.hpp', ] interface_dir = Dir('lib_mpc_export') SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +if GetOption('mpc_generate'): + generator_cpp = File('generator.cpp') + + acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + + generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, + CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + + cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" + env.Command(generated_c + generated_h, generator, cmd) + + + +mpc_files = ["lateral_mpc.c"] + generated_c env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) -# if arch != "aarch64": -# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), -# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), -# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] -# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 5f4a9a28d..96488f4cc 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -1,10 +1,10 @@ #include +#include "common/modeldata.h" #define PI 3.1415926536 #define deg2rad(d) (d/180.0*PI) -const int controlHorizon = 50; - +const int N_steps = 16; using namespace std; int main( ) @@ -17,54 +17,33 @@ int main( ) DifferentialState xx; // x position DifferentialState yy; // y position DifferentialState psi; // vehicle heading - DifferentialState delta; + DifferentialState curvature; - OnlineData curvature_factor; - OnlineData v_ref; // m/s - OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3; - OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3; - OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3; - OnlineData l_prob, r_prob; - OnlineData lane_width; + OnlineData v_ego; + OnlineData rotation_radius; - Control t; + Control curvature_rate; + // Equations of motion - f << dot(xx) == v_ref * cos(psi); - f << dot(yy) == v_ref * sin(psi); - f << dot(psi) == v_ref * delta * curvature_factor; - f << dot(delta) == t; - - auto lr_prob = l_prob + r_prob - l_prob * r_prob; - - auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3; - auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3; - auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3; - - auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2); - - // When the lane is not visible, use an estimate of its position - auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0); - auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0); - - auto c_left_lane = exp(-(weighted_left_lane - yy)); - auto c_right_lane = exp(weighted_right_lane - yy); + f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature); + f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature); + f << dot(psi) == v_ego * curvature; + f << dot(curvature) == curvature_rate; // Running cost Function h; // Distance errors - h << poly_d - yy; - h << lr_prob * c_left_lane; - h << lr_prob * c_right_lane; + h << yy; // Heading error - h << (v_ref + 1.0 ) * (angle_d - psi); + h << (v_ego + 5.0 ) * psi; // Angular rate error - h << (v_ref + 1.0 ) * t; + h << (v_ego + 5.0) * 4 * curvature_rate; - BMatrix Q(5,5); Q.setAll(true); + BMatrix Q(3,3); Q.setAll(true); // Q(0,0) = 1.0; // Q(1,1) = 1.0; // Q(2,2) = 1.0; @@ -75,34 +54,21 @@ int main( ) Function hN; // Distance errors - hN << poly_d - yy; - hN << l_prob * c_left_lane; - hN << r_prob * c_right_lane; + hN << yy; // Heading errors - hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi); + hN << (2.0 * v_ego + 5.0 ) * psi; - BMatrix QN(4,4); QN.setAll(true); + BMatrix QN(2,2); QN.setAll(true); // QN(0,0) = 1.0; // QN(1,1) = 1.0; // QN(2,2) = 1.0; // QN(3,3) = 1.0; - // Non uniform time grid - // First 5 timesteps are 0.05, after that it's 0.15 - DMatrix numSteps(20, 1); - for (int i = 0; i < 5; i++){ - numSteps(i) = 1; - } - for (int i = 5; i < 20; i++){ - numSteps(i) = 3; - } - - // Setup Optimal Control Problem - const double tStart = 0.0; - const double tEnd = 2.5; - - OCP ocp( tStart, tEnd, numSteps); + double T_IDXS_ARR[N_steps + 1]; + memcpy(T_IDXS_ARR, T_IDXS, (N_steps + 1) * sizeof(double)); + Grid times(N_steps + 1, T_IDXS_ARR); + OCP ocp(times); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -111,15 +77,15 @@ int main( ) // car can't go backward to avoid "circles" ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90)); // more than absolute max steer angle - ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50)); - ocp.setNOD(17); + ocp.subjectTo( deg2rad(-50) <= curvature <= deg2rad(50)); + ocp.setNOD(2); OCPexport mpc(ocp); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING ); mpc.set( INTEGRATOR_TYPE, INT_RK4 ); - mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); - mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( NUM_INTEGRATOR_STEPS, 2500); + mpc.set( MAX_NUM_QP_ITERATIONS, 1000); mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index 4972dbc3f..ff4fd4203 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -1,6 +1,6 @@ #include "acado_common.h" #include "acado_auxiliary_functions.h" - +#include "common/modeldata.h" #include #define NX ACADO_NX /* Number of differential state variables. */ @@ -17,48 +17,40 @@ ACADOvariables acadoVariables; ACADOworkspace acadoWorkspace; typedef struct { - double x, y, psi, delta, t; + double x, y, psi, tire_angle, tire_angle_rate; } state_t; - typedef struct { double x[N+1]; double y[N+1]; double psi[N+1]; - double delta[N+1]; - double rate[N]; + double tire_angle[N+1]; + double tire_angle_rate[N]; double cost; } log_t; -void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){ +void init_weights(double pathCost, double headingCost, double steerRateCost){ int i; - const int STEP_MULTIPLIER = 3; + const int STEP_MULTIPLIER = 3.0; for (i = 0; i < N; i++) { - int f = 1; - if (i > 4){ - f = STEP_MULTIPLIER; - } + double f = 20 * (T_IDXS[i+1] - T_IDXS[i]); // Setup diagonal entries acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*1] = laneCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*2] = laneCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*3] = headingCost * f; - acadoVariables.W[NY*NY*i + (NY+1)*4] = steerRateCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*1] = headingCost * f; + acadoVariables.W[NY*NY*i + (NY+1)*2] = steerRateCost * f; } acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*1] = laneCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*2] = laneCost * STEP_MULTIPLIER; - acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER; + acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER; } -void init(double pathCost, double laneCost, double headingCost, double steerRateCost){ +void init(double pathCost, double headingCost, double steerRateCost){ acado_initializeSolver(); int i; /* Initialize the states and controls. */ for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; - for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0; /* Initialize the measurements/reference. */ for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; @@ -67,45 +59,30 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; - init_weights(pathCost, laneCost, headingCost, steerRateCost); + init_weights(pathCost, headingCost, steerRateCost); } -int run_mpc(state_t * x0, log_t * solution, - double l_poly[4], double r_poly[4], double d_poly[4], - double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){ +int run_mpc(state_t * x0, log_t * solution, double v_ego, + double rotation_radius, double target_y[N+1], double target_psi[N+1]){ int i; for (i = 0; i <= NOD * N; i+= NOD){ - acadoVariables.od[i] = curvature_factor; - acadoVariables.od[i+1] = v_ref; - - acadoVariables.od[i+2] = l_poly[0]; - acadoVariables.od[i+3] = l_poly[1]; - acadoVariables.od[i+4] = l_poly[2]; - acadoVariables.od[i+5] = l_poly[3]; - - acadoVariables.od[i+6] = r_poly[0]; - acadoVariables.od[i+7] = r_poly[1]; - acadoVariables.od[i+8] = r_poly[2]; - acadoVariables.od[i+9] = r_poly[3]; - - acadoVariables.od[i+10] = d_poly[0]; - acadoVariables.od[i+11] = d_poly[1]; - acadoVariables.od[i+12] = d_poly[2]; - acadoVariables.od[i+13] = d_poly[3]; - - - acadoVariables.od[i+14] = l_prob; - acadoVariables.od[i+15] = r_prob; - acadoVariables.od[i+16] = lane_width; - + acadoVariables.od[i] = v_ego; + acadoVariables.od[i+1] = rotation_radius; } + for (i = 0; i < N; i+= 1){ + acadoVariables.y[NY*i + 0] = target_y[i]; + acadoVariables.y[NY*i + 1] = (v_ego + 5.0) * target_psi[i]; + acadoVariables.y[NY*i + 2] = 0.0; + } + acadoVariables.yN[0] = target_y[N]; + acadoVariables.yN[1] = (2.0 * v_ego + 5.0) * target_psi[N]; acadoVariables.x0[0] = x0->x; acadoVariables.x0[1] = x0->y; acadoVariables.x0[2] = x0->psi; - acadoVariables.x0[3] = x0->delta; + acadoVariables.x0[3] = x0->tire_angle; acado_preparationStep(); @@ -118,9 +95,9 @@ int run_mpc(state_t * x0, log_t * solution, solution->x[i] = acadoVariables.x[i*NX]; solution->y[i] = acadoVariables.x[i*NX+1]; solution->psi[i] = acadoVariables.x[i*NX+2]; - solution->delta[i] = acadoVariables.x[i*NX+3]; + solution->tire_angle[i] = acadoVariables.x[i*NX+3]; if (i < N){ - solution->rate[i] = acadoVariables.u[i]; + solution->tire_angle_rate[i] = acadoVariables.u[i]; } } solution->cost = acado_getObjective(); diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h index f069b62c0..41638459f 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h @@ -62,9 +62,9 @@ extern "C" /** Indicator for fixed initial state. */ #define ACADO_INITIAL_STATE_FIXED 1 /** Number of control/estimation intervals. */ -#define ACADO_N 20 +#define ACADO_N 16 /** Number of online data values. */ -#define ACADO_NOD 17 +#define ACADO_NOD 2 /** Number of path constraints. */ #define ACADO_NPAC 0 /** Number of control variables. */ @@ -76,11 +76,11 @@ extern "C" /** Number of differential derivative variables. */ #define ACADO_NXD 0 /** Number of references/measurements per node on the first N nodes. */ -#define ACADO_NY 5 +#define ACADO_NY 3 /** Number of references/measurements on the last (N + 1)st node. */ -#define ACADO_NYN 4 +#define ACADO_NYN 2 /** Total number of QP optimization variables. */ -#define ACADO_QP_NV 24 +#define ACADO_QP_NV 20 /** Number of Runge-Kutta stages per integration step. */ #define ACADO_RK_NSTAGES 4 /** Providing interface for arrival cost. */ @@ -102,41 +102,41 @@ extern "C" typedef struct ACADOvariables_ { int dummy; -/** Matrix of size: 21 x 4 (row major format) +/** Matrix of size: 17 x 4 (row major format) * - * Matrix containing 21 differential variable vectors. + * Matrix containing 17 differential variable vectors. */ -real_t x[ 84 ]; +real_t x[ 68 ]; -/** Column vector of size: 20 +/** Column vector of size: 16 * - * Matrix containing 20 control variable vectors. + * Matrix containing 16 control variable vectors. */ -real_t u[ 20 ]; +real_t u[ 16 ]; -/** Matrix of size: 21 x 17 (row major format) +/** Matrix of size: 17 x 2 (row major format) * - * Matrix containing 21 online data vectors. + * Matrix containing 17 online data vectors. */ -real_t od[ 357 ]; +real_t od[ 34 ]; -/** Column vector of size: 100 +/** Column vector of size: 48 * - * Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes. + * Matrix containing 16 reference/measurement vectors of size 3 for first 16 nodes. */ -real_t y[ 100 ]; +real_t y[ 48 ]; -/** Column vector of size: 4 +/** Column vector of size: 2 * - * Reference/measurement vector for the 21. node. + * Reference/measurement vector for the 17. node. */ -real_t yN[ 4 ]; +real_t yN[ 2 ]; -/** Matrix of size: 100 x 5 (row major format) */ -real_t W[ 500 ]; +/** Matrix of size: 48 x 3 (row major format) */ +real_t W[ 144 ]; -/** Matrix of size: 4 x 4 (row major format) */ -real_t WN[ 16 ]; +/** Matrix of size: 2 x 2 (row major format) */ +real_t WN[ 4 ]; /** Column vector of size: 4 * @@ -155,64 +155,61 @@ real_t x0[ 4 ]; */ typedef struct ACADOworkspace_ { -/** Column vector of size: 14 */ -real_t rhs_aux[ 14 ]; +/** Column vector of size: 28 */ +real_t rhs_aux[ 28 ]; real_t rk_ttt; -/** Row vector of size: 42 */ -real_t rk_xxx[ 42 ]; +/** Row vector of size: 27 */ +real_t rk_xxx[ 27 ]; /** Matrix of size: 4 x 24 (row major format) */ real_t rk_kkk[ 96 ]; -/** Row vector of size: 42 */ -real_t state[ 42 ]; +/** Row vector of size: 27 */ +real_t state[ 27 ]; -/** Column vector of size: 80 */ -real_t d[ 80 ]; +/** Column vector of size: 64 */ +real_t d[ 64 ]; -/** Column vector of size: 100 */ -real_t Dy[ 100 ]; +/** Column vector of size: 48 */ +real_t Dy[ 48 ]; -/** Column vector of size: 4 */ -real_t DyN[ 4 ]; +/** Column vector of size: 2 */ +real_t DyN[ 2 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t evGx[ 320 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t evGx[ 256 ]; -/** Column vector of size: 80 */ -real_t evGu[ 80 ]; +/** Column vector of size: 64 */ +real_t evGu[ 64 ]; -/** Column vector of size: 11 */ -real_t objAuxVar[ 11 ]; +/** Row vector of size: 7 */ +real_t objValueIn[ 7 ]; -/** Row vector of size: 22 */ -real_t objValueIn[ 22 ]; +/** Row vector of size: 18 */ +real_t objValueOut[ 18 ]; -/** Row vector of size: 30 */ -real_t objValueOut[ 30 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t Q1[ 256 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t Q1[ 320 ]; +/** Matrix of size: 64 x 3 (row major format) */ +real_t Q2[ 192 ]; -/** Matrix of size: 80 x 5 (row major format) */ -real_t Q2[ 400 ]; +/** Column vector of size: 16 */ +real_t R1[ 16 ]; -/** Column vector of size: 20 */ -real_t R1[ 20 ]; +/** Matrix of size: 16 x 3 (row major format) */ +real_t R2[ 48 ]; -/** Matrix of size: 20 x 5 (row major format) */ -real_t R2[ 100 ]; - -/** Column vector of size: 80 */ -real_t S1[ 80 ]; +/** Column vector of size: 64 */ +real_t S1[ 64 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t QN1[ 16 ]; -/** Matrix of size: 4 x 4 (row major format) */ -real_t QN2[ 16 ]; +/** Matrix of size: 4 x 2 (row major format) */ +real_t QN2[ 8 ]; /** Column vector of size: 4 */ real_t Dx0[ 4 ]; @@ -220,50 +217,50 @@ real_t Dx0[ 4 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t T[ 16 ]; -/** Column vector of size: 840 */ -real_t E[ 840 ]; +/** Column vector of size: 544 */ +real_t E[ 544 ]; -/** Column vector of size: 840 */ -real_t QE[ 840 ]; +/** Column vector of size: 544 */ +real_t QE[ 544 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t QGx[ 320 ]; - -/** Column vector of size: 80 */ -real_t Qd[ 80 ]; - -/** Column vector of size: 84 */ -real_t QDy[ 84 ]; - -/** Matrix of size: 20 x 4 (row major format) */ -real_t H10[ 80 ]; - -/** Matrix of size: 24 x 24 (row major format) */ -real_t H[ 576 ]; - -/** Matrix of size: 40 x 24 (row major format) */ -real_t A[ 960 ]; - -/** Column vector of size: 24 */ -real_t g[ 24 ]; - -/** Column vector of size: 24 */ -real_t lb[ 24 ]; - -/** Column vector of size: 24 */ -real_t ub[ 24 ]; - -/** Column vector of size: 40 */ -real_t lbA[ 40 ]; - -/** Column vector of size: 40 */ -real_t ubA[ 40 ]; - -/** Column vector of size: 24 */ -real_t x[ 24 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t QGx[ 256 ]; /** Column vector of size: 64 */ -real_t y[ 64 ]; +real_t Qd[ 64 ]; + +/** Column vector of size: 68 */ +real_t QDy[ 68 ]; + +/** Matrix of size: 16 x 4 (row major format) */ +real_t H10[ 64 ]; + +/** Matrix of size: 20 x 20 (row major format) */ +real_t H[ 400 ]; + +/** Matrix of size: 32 x 20 (row major format) */ +real_t A[ 640 ]; + +/** Column vector of size: 20 */ +real_t g[ 20 ]; + +/** Column vector of size: 20 */ +real_t lb[ 20 ]; + +/** Column vector of size: 20 */ +real_t ub[ 20 ]; + +/** Column vector of size: 32 */ +real_t lbA[ 32 ]; + +/** Column vector of size: 32 */ +real_t ubA[ 32 ]; + +/** Column vector of size: 20 */ +real_t x[ 20 ]; + +/** Column vector of size: 52 */ +real_t y[ 52 ]; } ACADOworkspace; @@ -314,7 +311,7 @@ void acado_initializeNodesByForwardSimulation( ); /** Shift differential variables vector by one interval. * - * \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation. + * \param strategy Shifting strategy: 1. Initialize node 17 with xEnd. 2. Initialize node 17 by forward simulation. * \param xEnd Value for the x vector on the last node. If =0 the old value is used. * \param uEnd Value for the u vector on the second to last node. If =0 the old value is used. */ diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c index 5df7cb47f..71231b37e 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c @@ -25,59 +25,73 @@ void acado_rhs_forw(const real_t* in, real_t* out) const real_t* xd = in; const real_t* u = in + 24; const real_t* od = in + 25; -/* Vector of auxiliary variables; number of elements: 14. */ +/* Vector of auxiliary variables; number of elements: 28. */ real_t* a = acadoWorkspace.rhs_aux; /* Compute intermediate quantities: */ a[0] = (cos(xd[2])); a[1] = (sin(xd[2])); -a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); -a[3] = (xd[12]*a[2]); -a[4] = (xd[13]*a[2]); -a[5] = (xd[14]*a[2]); -a[6] = (xd[15]*a[2]); -a[7] = (cos(xd[2])); -a[8] = (xd[12]*a[7]); -a[9] = (xd[13]*a[7]); -a[10] = (xd[14]*a[7]); -a[11] = (xd[15]*a[7]); -a[12] = (xd[22]*a[2]); -a[13] = (xd[22]*a[7]); +a[2] = (sin(xd[2])); +a[3] = (cos(xd[2])); +a[4] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); +a[5] = (xd[12]*a[4]); +a[6] = (cos(xd[2])); +a[7] = (xd[12]*a[6]); +a[8] = (xd[13]*a[4]); +a[9] = (xd[13]*a[6]); +a[10] = (xd[14]*a[4]); +a[11] = (xd[14]*a[6]); +a[12] = (xd[15]*a[4]); +a[13] = (xd[15]*a[6]); +a[14] = (cos(xd[2])); +a[15] = (xd[12]*a[14]); +a[16] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2]))); +a[17] = (xd[12]*a[16]); +a[18] = (xd[13]*a[14]); +a[19] = (xd[13]*a[16]); +a[20] = (xd[14]*a[14]); +a[21] = (xd[14]*a[16]); +a[22] = (xd[15]*a[14]); +a[23] = (xd[15]*a[16]); +a[24] = (xd[22]*a[4]); +a[25] = (xd[22]*a[6]); +a[26] = (xd[22]*a[14]); +a[27] = (xd[22]*a[16]); /* Compute outputs: */ -out[0] = (od[1]*a[0]); -out[1] = (od[1]*a[1]); -out[2] = ((od[1]*xd[3])*od[0]); +out[0] = ((od[0]*a[0])-((od[1]*a[1])*(od[0]*xd[3]))); +out[1] = ((od[0]*a[2])+((od[1]*a[3])*(od[0]*xd[3]))); +out[2] = (od[0]*xd[3]); out[3] = u[0]; -out[4] = (od[1]*a[3]); -out[5] = (od[1]*a[4]); -out[6] = (od[1]*a[5]); -out[7] = (od[1]*a[6]); -out[8] = (od[1]*a[8]); -out[9] = (od[1]*a[9]); -out[10] = (od[1]*a[10]); -out[11] = (od[1]*a[11]); -out[12] = ((od[1]*xd[16])*od[0]); -out[13] = ((od[1]*xd[17])*od[0]); -out[14] = ((od[1]*xd[18])*od[0]); -out[15] = ((od[1]*xd[19])*od[0]); +out[4] = ((od[0]*a[5])-(((od[1]*a[7])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[16])))); +out[5] = ((od[0]*a[8])-(((od[1]*a[9])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[17])))); +out[6] = ((od[0]*a[10])-(((od[1]*a[11])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[18])))); +out[7] = ((od[0]*a[12])-(((od[1]*a[13])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[19])))); +out[8] = ((od[0]*a[15])+(((od[1]*a[17])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[16])))); +out[9] = ((od[0]*a[18])+(((od[1]*a[19])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[17])))); +out[10] = ((od[0]*a[20])+(((od[1]*a[21])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[18])))); +out[11] = ((od[0]*a[22])+(((od[1]*a[23])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[19])))); +out[12] = (od[0]*xd[16]); +out[13] = (od[0]*xd[17]); +out[14] = (od[0]*xd[18]); +out[15] = (od[0]*xd[19]); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (real_t)(0.0000000000000000e+00); out[18] = (real_t)(0.0000000000000000e+00); out[19] = (real_t)(0.0000000000000000e+00); -out[20] = (od[1]*a[12]); -out[21] = (od[1]*a[13]); -out[22] = ((od[1]*xd[23])*od[0]); +out[20] = ((od[0]*a[24])-(((od[1]*a[25])*(od[0]*xd[3]))+((od[1]*a[1])*(od[0]*xd[23])))); +out[21] = ((od[0]*a[26])+(((od[1]*a[27])*(od[0]*xd[3]))+((od[1]*a[3])*(od[0]*xd[23])))); +out[22] = (od[0]*xd[23]); out[23] = (real_t)(1.0000000000000000e+00); } -/* Fixed step size:0.05 */ +/* Fixed step size:0.001 */ int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ) { int error; int run1; -int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3}; +int numSteps[16] = {10, 29, 49, 68, 88, 107, 127, 146, 166, 186, 205, 225, 244, 264, 283, 303}; int numInts = numSteps[rk_index]; acadoWorkspace.rk_ttt = 0.0000000000000000e+00; rk_eta[4] = 1.0000000000000000e+00; @@ -103,21 +117,6 @@ rk_eta[23] = 0.0000000000000000e+00; acadoWorkspace.rk_xxx[24] = rk_eta[24]; acadoWorkspace.rk_xxx[25] = rk_eta[25]; acadoWorkspace.rk_xxx[26] = rk_eta[26]; -acadoWorkspace.rk_xxx[27] = rk_eta[27]; -acadoWorkspace.rk_xxx[28] = rk_eta[28]; -acadoWorkspace.rk_xxx[29] = rk_eta[29]; -acadoWorkspace.rk_xxx[30] = rk_eta[30]; -acadoWorkspace.rk_xxx[31] = rk_eta[31]; -acadoWorkspace.rk_xxx[32] = rk_eta[32]; -acadoWorkspace.rk_xxx[33] = rk_eta[33]; -acadoWorkspace.rk_xxx[34] = rk_eta[34]; -acadoWorkspace.rk_xxx[35] = rk_eta[35]; -acadoWorkspace.rk_xxx[36] = rk_eta[36]; -acadoWorkspace.rk_xxx[37] = rk_eta[37]; -acadoWorkspace.rk_xxx[38] = rk_eta[38]; -acadoWorkspace.rk_xxx[39] = rk_eta[39]; -acadoWorkspace.rk_xxx[40] = rk_eta[40]; -acadoWorkspace.rk_xxx[41] = rk_eta[41]; for (run1 = 0; run1 < 1; ++run1) { @@ -147,105 +146,105 @@ acadoWorkspace.rk_xxx[21] = + rk_eta[21]; acadoWorkspace.rk_xxx[22] = + rk_eta[22]; acadoWorkspace.rk_xxx[23] = + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[23] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.4999999999999991e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[24] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[25] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[26] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[27] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[28] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[29] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[30] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[31] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[32] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[33] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[34] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[35] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[36] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[37] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[38] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[39] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[40] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[41] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[42] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[43] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[44] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[45] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[46] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-04*acadoWorkspace.rk_kkk[47] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)4.9999999999999982e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-03*acadoWorkspace.rk_kkk[71] + rk_eta[23]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); -rk_eta[0] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[72]; -rk_eta[1] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[73]; -rk_eta[2] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[74]; -rk_eta[3] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[75]; -rk_eta[4] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[76]; -rk_eta[5] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[77]; -rk_eta[6] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[78]; -rk_eta[7] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[79]; -rk_eta[8] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[80]; -rk_eta[9] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[81]; -rk_eta[10] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[82]; -rk_eta[11] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[83]; -rk_eta[12] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[84]; -rk_eta[13] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[85]; -rk_eta[14] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[86]; -rk_eta[15] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[87]; -rk_eta[16] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[88]; -rk_eta[17] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[89]; -rk_eta[18] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[90]; -rk_eta[19] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[91]; -rk_eta[20] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[92]; -rk_eta[21] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[93]; -rk_eta[22] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[94]; -rk_eta[23] += + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666659e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333297e-03*acadoWorkspace.rk_kkk[95]; +rk_eta[0] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[48] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[72]; +rk_eta[1] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[49] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[73]; +rk_eta[2] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[50] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[74]; +rk_eta[3] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[51] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[75]; +rk_eta[4] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[52] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[76]; +rk_eta[5] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[53] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[77]; +rk_eta[6] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[54] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[78]; +rk_eta[7] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[55] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[79]; +rk_eta[8] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[80]; +rk_eta[9] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[81]; +rk_eta[10] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[82]; +rk_eta[11] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[83]; +rk_eta[12] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[84]; +rk_eta[13] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[85]; +rk_eta[14] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[86]; +rk_eta[15] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[87]; +rk_eta[16] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[88]; +rk_eta[17] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[89]; +rk_eta[18] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[90]; +rk_eta[19] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[91]; +rk_eta[20] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[92]; +rk_eta[21] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[93]; +rk_eta[22] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[94]; +rk_eta[23] += + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333332e-04*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-04*acadoWorkspace.rk_kkk[95]; acadoWorkspace.rk_ttt += 1.0000000000000000e+00; } } diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp index f2665a45c..86b5f1548 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp @@ -40,7 +40,7 @@ int acado_solve( void ) { acado_nWSR = QPOASES_NWSRMAX; - QProblem qp(24, 40); + QProblem qp(20, 32); returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y); diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp index 47d41855e..5b003f08b 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp @@ -37,11 +37,11 @@ */ /** Maximum number of optimization variables. */ -#define QPOASES_NVMAX 24 +#define QPOASES_NVMAX 20 /** Maximum number of constraints. */ -#define QPOASES_NCMAX 40 +#define QPOASES_NCMAX 32 /** Maximum number of working set recalculations. */ -#define QPOASES_NWSRMAX 500 +#define QPOASES_NWSRMAX 1000 /** Print level for qpOASES. */ #define QPOASES_PRINTLEVEL PL_NONE /** The value of EPS */ diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c index 347e77fa7..81a01da09 100644 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c @@ -35,7 +35,7 @@ int ret; int lRun1; ret = 0; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; @@ -43,23 +43,8 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[lRun1]; -acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 17]; -acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 17 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 17 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 17 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 17 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 17 + 5]; -acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 17 + 6]; -acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 17 + 7]; -acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 17 + 8]; -acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 17 + 9]; -acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 17 + 10]; -acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 17 + 11]; -acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 17 + 12]; -acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 17 + 13]; -acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 17 + 14]; -acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 17 + 15]; -acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 17 + 16]; +acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 2 + 1]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -98,253 +83,149 @@ void acado_evaluateLSQ(const real_t* in, real_t* out) const real_t* xd = in; const real_t* u = in + 4; const real_t* od = in + 5; -/* Vector of auxiliary variables; number of elements: 11. */ -real_t* a = acadoWorkspace.objAuxVar; - -/* Compute intermediate quantities: */ -a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); -a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]); -a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]); -a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]); -a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]); -a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); -a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]); /* Compute outputs: */ -out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]); -out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]); -out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]); -out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2])); -out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]); -out[5] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]); -out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[0] = xd[1]; +out[1] = ((od[0]+(real_t)(5.0000000000000000e+00))*xd[2]); +out[2] = (((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00))*u[0]); +out[3] = (real_t)(0.0000000000000000e+00); +out[4] = (real_t)(1.0000000000000000e+00); +out[5] = (real_t)(0.0000000000000000e+00); +out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); out[8] = (real_t)(0.0000000000000000e+00); -out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[4]); -out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[5]); +out[9] = (od[0]+(real_t)(5.0000000000000000e+00)); +out[10] = (real_t)(0.0000000000000000e+00); out[11] = (real_t)(0.0000000000000000e+00); out[12] = (real_t)(0.0000000000000000e+00); -out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[7]); -out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]); +out[13] = (real_t)(0.0000000000000000e+00); +out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); -out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*a[10]); -out[18] = (real_t)(0.0000000000000000e+00); -out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); -out[20] = (real_t)(0.0000000000000000e+00); -out[21] = (real_t)(0.0000000000000000e+00); -out[22] = (real_t)(0.0000000000000000e+00); -out[23] = (real_t)(0.0000000000000000e+00); -out[24] = (real_t)(0.0000000000000000e+00); -out[25] = (real_t)(0.0000000000000000e+00); -out[26] = (real_t)(0.0000000000000000e+00); -out[27] = (real_t)(0.0000000000000000e+00); -out[28] = (real_t)(0.0000000000000000e+00); -out[29] = (od[1]+(real_t)(1.0000000000000000e+00)); +out[17] = ((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00)); } void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) { const real_t* xd = in; const real_t* od = in + 4; -/* Vector of auxiliary variables; number of elements: 11. */ -real_t* a = acadoWorkspace.objAuxVar; - -/* Compute intermediate quantities: */ -a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]))); -a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])))); -a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]); -a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]); -a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))); -a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]); -a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]); -a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2)))); -a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]); /* Compute outputs: */ -out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]); -out[1] = (od[14]*a[0]); -out[2] = (od[15]*a[1]); -out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2])); -out[4] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]); -out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)); +out[0] = xd[1]; +out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00))*xd[2]); +out[2] = (real_t)(0.0000000000000000e+00); +out[3] = (real_t)(1.0000000000000000e+00); +out[4] = (real_t)(0.0000000000000000e+00); +out[5] = (real_t)(0.0000000000000000e+00); out[6] = (real_t)(0.0000000000000000e+00); out[7] = (real_t)(0.0000000000000000e+00); -out[8] = (od[14]*a[4]); -out[9] = (od[14]*a[5]); -out[10] = (real_t)(0.0000000000000000e+00); -out[11] = (real_t)(0.0000000000000000e+00); -out[12] = (od[15]*a[7]); -out[13] = (od[15]*a[8]); -out[14] = (real_t)(0.0000000000000000e+00); -out[15] = (real_t)(0.0000000000000000e+00); -out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*a[10]); -out[17] = (real_t)(0.0000000000000000e+00); -out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))); -out[19] = (real_t)(0.0000000000000000e+00); +out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00)); +out[9] = (real_t)(0.0000000000000000e+00); } void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 ) { -tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[10] + tmpFx[12]*tmpObjS[15] + tmpFx[16]*tmpObjS[20]; -tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[6] + tmpFx[8]*tmpObjS[11] + tmpFx[12]*tmpObjS[16] + tmpFx[16]*tmpObjS[21]; -tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[7] + tmpFx[8]*tmpObjS[12] + tmpFx[12]*tmpObjS[17] + tmpFx[16]*tmpObjS[22]; -tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[4]*tmpObjS[8] + tmpFx[8]*tmpObjS[13] + tmpFx[12]*tmpObjS[18] + tmpFx[16]*tmpObjS[23]; -tmpQ2[4] = + tmpFx[0]*tmpObjS[4] + tmpFx[4]*tmpObjS[9] + tmpFx[8]*tmpObjS[14] + tmpFx[12]*tmpObjS[19] + tmpFx[16]*tmpObjS[24]; -tmpQ2[5] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[10] + tmpFx[13]*tmpObjS[15] + tmpFx[17]*tmpObjS[20]; -tmpQ2[6] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[6] + tmpFx[9]*tmpObjS[11] + tmpFx[13]*tmpObjS[16] + tmpFx[17]*tmpObjS[21]; -tmpQ2[7] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[7] + tmpFx[9]*tmpObjS[12] + tmpFx[13]*tmpObjS[17] + tmpFx[17]*tmpObjS[22]; -tmpQ2[8] = + tmpFx[1]*tmpObjS[3] + tmpFx[5]*tmpObjS[8] + tmpFx[9]*tmpObjS[13] + tmpFx[13]*tmpObjS[18] + tmpFx[17]*tmpObjS[23]; -tmpQ2[9] = + tmpFx[1]*tmpObjS[4] + tmpFx[5]*tmpObjS[9] + tmpFx[9]*tmpObjS[14] + tmpFx[13]*tmpObjS[19] + tmpFx[17]*tmpObjS[24]; -tmpQ2[10] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[10] + tmpFx[14]*tmpObjS[15] + tmpFx[18]*tmpObjS[20]; -tmpQ2[11] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[6] + tmpFx[10]*tmpObjS[11] + tmpFx[14]*tmpObjS[16] + tmpFx[18]*tmpObjS[21]; -tmpQ2[12] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[7] + tmpFx[10]*tmpObjS[12] + tmpFx[14]*tmpObjS[17] + tmpFx[18]*tmpObjS[22]; -tmpQ2[13] = + tmpFx[2]*tmpObjS[3] + tmpFx[6]*tmpObjS[8] + tmpFx[10]*tmpObjS[13] + tmpFx[14]*tmpObjS[18] + tmpFx[18]*tmpObjS[23]; -tmpQ2[14] = + tmpFx[2]*tmpObjS[4] + tmpFx[6]*tmpObjS[9] + tmpFx[10]*tmpObjS[14] + tmpFx[14]*tmpObjS[19] + tmpFx[18]*tmpObjS[24]; -tmpQ2[15] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[10] + tmpFx[15]*tmpObjS[15] + tmpFx[19]*tmpObjS[20]; -tmpQ2[16] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[6] + tmpFx[11]*tmpObjS[11] + tmpFx[15]*tmpObjS[16] + tmpFx[19]*tmpObjS[21]; -tmpQ2[17] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[7] + tmpFx[11]*tmpObjS[12] + tmpFx[15]*tmpObjS[17] + tmpFx[19]*tmpObjS[22]; -tmpQ2[18] = + tmpFx[3]*tmpObjS[3] + tmpFx[7]*tmpObjS[8] + tmpFx[11]*tmpObjS[13] + tmpFx[15]*tmpObjS[18] + tmpFx[19]*tmpObjS[23]; -tmpQ2[19] = + tmpFx[3]*tmpObjS[4] + tmpFx[7]*tmpObjS[9] + tmpFx[11]*tmpObjS[14] + tmpFx[15]*tmpObjS[19] + tmpFx[19]*tmpObjS[24]; -tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[12] + tmpQ2[4]*tmpFx[16]; -tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9] + tmpQ2[3]*tmpFx[13] + tmpQ2[4]*tmpFx[17]; -tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10] + tmpQ2[3]*tmpFx[14] + tmpQ2[4]*tmpFx[18]; -tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11] + tmpQ2[3]*tmpFx[15] + tmpQ2[4]*tmpFx[19]; -tmpQ1[4] = + tmpQ2[5]*tmpFx[0] + tmpQ2[6]*tmpFx[4] + tmpQ2[7]*tmpFx[8] + tmpQ2[8]*tmpFx[12] + tmpQ2[9]*tmpFx[16]; -tmpQ1[5] = + tmpQ2[5]*tmpFx[1] + tmpQ2[6]*tmpFx[5] + tmpQ2[7]*tmpFx[9] + tmpQ2[8]*tmpFx[13] + tmpQ2[9]*tmpFx[17]; -tmpQ1[6] = + tmpQ2[5]*tmpFx[2] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[10] + tmpQ2[8]*tmpFx[14] + tmpQ2[9]*tmpFx[18]; -tmpQ1[7] = + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[11] + tmpQ2[8]*tmpFx[15] + tmpQ2[9]*tmpFx[19]; -tmpQ1[8] = + tmpQ2[10]*tmpFx[0] + tmpQ2[11]*tmpFx[4] + tmpQ2[12]*tmpFx[8] + tmpQ2[13]*tmpFx[12] + tmpQ2[14]*tmpFx[16]; -tmpQ1[9] = + tmpQ2[10]*tmpFx[1] + tmpQ2[11]*tmpFx[5] + tmpQ2[12]*tmpFx[9] + tmpQ2[13]*tmpFx[13] + tmpQ2[14]*tmpFx[17]; -tmpQ1[10] = + tmpQ2[10]*tmpFx[2] + tmpQ2[11]*tmpFx[6] + tmpQ2[12]*tmpFx[10] + tmpQ2[13]*tmpFx[14] + tmpQ2[14]*tmpFx[18]; -tmpQ1[11] = + tmpQ2[10]*tmpFx[3] + tmpQ2[11]*tmpFx[7] + tmpQ2[12]*tmpFx[11] + tmpQ2[13]*tmpFx[15] + tmpQ2[14]*tmpFx[19]; -tmpQ1[12] = + tmpQ2[15]*tmpFx[0] + tmpQ2[16]*tmpFx[4] + tmpQ2[17]*tmpFx[8] + tmpQ2[18]*tmpFx[12] + tmpQ2[19]*tmpFx[16]; -tmpQ1[13] = + tmpQ2[15]*tmpFx[1] + tmpQ2[16]*tmpFx[5] + tmpQ2[17]*tmpFx[9] + tmpQ2[18]*tmpFx[13] + tmpQ2[19]*tmpFx[17]; -tmpQ1[14] = + tmpQ2[15]*tmpFx[2] + tmpQ2[16]*tmpFx[6] + tmpQ2[17]*tmpFx[10] + tmpQ2[18]*tmpFx[14] + tmpQ2[19]*tmpFx[18]; -tmpQ1[15] = + tmpQ2[15]*tmpFx[3] + tmpQ2[16]*tmpFx[7] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[19]; +tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[3] + tmpFx[8]*tmpObjS[6]; +tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[4] + tmpFx[8]*tmpObjS[7]; +tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[8]; +tmpQ2[3] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[3] + tmpFx[9]*tmpObjS[6]; +tmpQ2[4] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[4] + tmpFx[9]*tmpObjS[7]; +tmpQ2[5] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[8]; +tmpQ2[6] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[3] + tmpFx[10]*tmpObjS[6]; +tmpQ2[7] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[4] + tmpFx[10]*tmpObjS[7]; +tmpQ2[8] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[8]; +tmpQ2[9] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[3] + tmpFx[11]*tmpObjS[6]; +tmpQ2[10] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[4] + tmpFx[11]*tmpObjS[7]; +tmpQ2[11] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[8]; +tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8]; +tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9]; +tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10]; +tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11]; +tmpQ1[4] = + tmpQ2[3]*tmpFx[0] + tmpQ2[4]*tmpFx[4] + tmpQ2[5]*tmpFx[8]; +tmpQ1[5] = + tmpQ2[3]*tmpFx[1] + tmpQ2[4]*tmpFx[5] + tmpQ2[5]*tmpFx[9]; +tmpQ1[6] = + tmpQ2[3]*tmpFx[2] + tmpQ2[4]*tmpFx[6] + tmpQ2[5]*tmpFx[10]; +tmpQ1[7] = + tmpQ2[3]*tmpFx[3] + tmpQ2[4]*tmpFx[7] + tmpQ2[5]*tmpFx[11]; +tmpQ1[8] = + tmpQ2[6]*tmpFx[0] + tmpQ2[7]*tmpFx[4] + tmpQ2[8]*tmpFx[8]; +tmpQ1[9] = + tmpQ2[6]*tmpFx[1] + tmpQ2[7]*tmpFx[5] + tmpQ2[8]*tmpFx[9]; +tmpQ1[10] = + tmpQ2[6]*tmpFx[2] + tmpQ2[7]*tmpFx[6] + tmpQ2[8]*tmpFx[10]; +tmpQ1[11] = + tmpQ2[6]*tmpFx[3] + tmpQ2[7]*tmpFx[7] + tmpQ2[8]*tmpFx[11]; +tmpQ1[12] = + tmpQ2[9]*tmpFx[0] + tmpQ2[10]*tmpFx[4] + tmpQ2[11]*tmpFx[8]; +tmpQ1[13] = + tmpQ2[9]*tmpFx[1] + tmpQ2[10]*tmpFx[5] + tmpQ2[11]*tmpFx[9]; +tmpQ1[14] = + tmpQ2[9]*tmpFx[2] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[10]; +tmpQ1[15] = + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[11]; } void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 ) { -tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[15] + tmpFu[4]*tmpObjS[20]; -tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[16] + tmpFu[4]*tmpObjS[21]; -tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[12] + tmpFu[3]*tmpObjS[17] + tmpFu[4]*tmpObjS[22]; -tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[8] + tmpFu[2]*tmpObjS[13] + tmpFu[3]*tmpObjS[18] + tmpFu[4]*tmpObjS[23]; -tmpR2[4] = + tmpFu[0]*tmpObjS[4] + tmpFu[1]*tmpObjS[9] + tmpFu[2]*tmpObjS[14] + tmpFu[3]*tmpObjS[19] + tmpFu[4]*tmpObjS[24]; -tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3] + tmpR2[4]*tmpFu[4]; +tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[3] + tmpFu[2]*tmpObjS[6]; +tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[4] + tmpFu[2]*tmpObjS[7]; +tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[8]; +tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2]; } void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 ) { -tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[8] + tmpFx[12]*tmpObjSEndTerm[12]; -tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[9] + tmpFx[12]*tmpObjSEndTerm[13]; -tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[6] + tmpFx[8]*tmpObjSEndTerm[10] + tmpFx[12]*tmpObjSEndTerm[14]; -tmpQN2[3] = + tmpFx[0]*tmpObjSEndTerm[3] + tmpFx[4]*tmpObjSEndTerm[7] + tmpFx[8]*tmpObjSEndTerm[11] + tmpFx[12]*tmpObjSEndTerm[15]; -tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[9]*tmpObjSEndTerm[8] + tmpFx[13]*tmpObjSEndTerm[12]; -tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[9]*tmpObjSEndTerm[9] + tmpFx[13]*tmpObjSEndTerm[13]; -tmpQN2[6] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[6] + tmpFx[9]*tmpObjSEndTerm[10] + tmpFx[13]*tmpObjSEndTerm[14]; -tmpQN2[7] = + tmpFx[1]*tmpObjSEndTerm[3] + tmpFx[5]*tmpObjSEndTerm[7] + tmpFx[9]*tmpObjSEndTerm[11] + tmpFx[13]*tmpObjSEndTerm[15]; -tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[4] + tmpFx[10]*tmpObjSEndTerm[8] + tmpFx[14]*tmpObjSEndTerm[12]; -tmpQN2[9] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[5] + tmpFx[10]*tmpObjSEndTerm[9] + tmpFx[14]*tmpObjSEndTerm[13]; -tmpQN2[10] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[6]*tmpObjSEndTerm[6] + tmpFx[10]*tmpObjSEndTerm[10] + tmpFx[14]*tmpObjSEndTerm[14]; -tmpQN2[11] = + tmpFx[2]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[7] + tmpFx[10]*tmpObjSEndTerm[11] + tmpFx[14]*tmpObjSEndTerm[15]; -tmpQN2[12] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[4] + tmpFx[11]*tmpObjSEndTerm[8] + tmpFx[15]*tmpObjSEndTerm[12]; -tmpQN2[13] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[5] + tmpFx[11]*tmpObjSEndTerm[9] + tmpFx[15]*tmpObjSEndTerm[13]; -tmpQN2[14] = + tmpFx[3]*tmpObjSEndTerm[2] + tmpFx[7]*tmpObjSEndTerm[6] + tmpFx[11]*tmpObjSEndTerm[10] + tmpFx[15]*tmpObjSEndTerm[14]; -tmpQN2[15] = + tmpFx[3]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[7] + tmpFx[11]*tmpObjSEndTerm[11] + tmpFx[15]*tmpObjSEndTerm[15]; -tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[8] + tmpQN2[3]*tmpFx[12]; -tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[9] + tmpQN2[3]*tmpFx[13]; -tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[10] + tmpQN2[3]*tmpFx[14]; -tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[11] + tmpQN2[3]*tmpFx[15]; -tmpQN1[4] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4] + tmpQN2[6]*tmpFx[8] + tmpQN2[7]*tmpFx[12]; -tmpQN1[5] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5] + tmpQN2[6]*tmpFx[9] + tmpQN2[7]*tmpFx[13]; -tmpQN1[6] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6] + tmpQN2[6]*tmpFx[10] + tmpQN2[7]*tmpFx[14]; -tmpQN1[7] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7] + tmpQN2[6]*tmpFx[11] + tmpQN2[7]*tmpFx[15]; -tmpQN1[8] = + tmpQN2[8]*tmpFx[0] + tmpQN2[9]*tmpFx[4] + tmpQN2[10]*tmpFx[8] + tmpQN2[11]*tmpFx[12]; -tmpQN1[9] = + tmpQN2[8]*tmpFx[1] + tmpQN2[9]*tmpFx[5] + tmpQN2[10]*tmpFx[9] + tmpQN2[11]*tmpFx[13]; -tmpQN1[10] = + tmpQN2[8]*tmpFx[2] + tmpQN2[9]*tmpFx[6] + tmpQN2[10]*tmpFx[10] + tmpQN2[11]*tmpFx[14]; -tmpQN1[11] = + tmpQN2[8]*tmpFx[3] + tmpQN2[9]*tmpFx[7] + tmpQN2[10]*tmpFx[11] + tmpQN2[11]*tmpFx[15]; -tmpQN1[12] = + tmpQN2[12]*tmpFx[0] + tmpQN2[13]*tmpFx[4] + tmpQN2[14]*tmpFx[8] + tmpQN2[15]*tmpFx[12]; -tmpQN1[13] = + tmpQN2[12]*tmpFx[1] + tmpQN2[13]*tmpFx[5] + tmpQN2[14]*tmpFx[9] + tmpQN2[15]*tmpFx[13]; -tmpQN1[14] = + tmpQN2[12]*tmpFx[2] + tmpQN2[13]*tmpFx[6] + tmpQN2[14]*tmpFx[10] + tmpQN2[15]*tmpFx[14]; -tmpQN1[15] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[11] + tmpQN2[15]*tmpFx[15]; +tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[2]; +tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[3]; +tmpQN2[2] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[2]; +tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[3]; +tmpQN2[4] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[2]; +tmpQN2[5] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[3]; +tmpQN2[6] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[2]; +tmpQN2[7] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[3]; +tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4]; +tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5]; +tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6]; +tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7]; +tmpQN1[4] = + tmpQN2[2]*tmpFx[0] + tmpQN2[3]*tmpFx[4]; +tmpQN1[5] = + tmpQN2[2]*tmpFx[1] + tmpQN2[3]*tmpFx[5]; +tmpQN1[6] = + tmpQN2[2]*tmpFx[2] + tmpQN2[3]*tmpFx[6]; +tmpQN1[7] = + tmpQN2[2]*tmpFx[3] + tmpQN2[3]*tmpFx[7]; +tmpQN1[8] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4]; +tmpQN1[9] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5]; +tmpQN1[10] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6]; +tmpQN1[11] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7]; +tmpQN1[12] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[4]; +tmpQN1[13] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[5]; +tmpQN1[14] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[6]; +tmpQN1[15] = + tmpQN2[6]*tmpFx[3] + tmpQN2[7]*tmpFx[7]; } void acado_evaluateObjective( ) { int runObj; -for (runObj = 0; runObj < 20; ++runObj) +for (runObj = 0; runObj < 16; ++runObj) { acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 17]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 17 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 17 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 17 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 17 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 17 + 5]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 17 + 6]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 17 + 7]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 17 + 8]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 17 + 9]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 17 + 10]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 17 + 11]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 17 + 12]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 17 + 13]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 17 + 14]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 17 + 15]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 17 + 16]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 2 + 1]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); -acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0]; -acadoWorkspace.Dy[runObj * 5 + 1] = acadoWorkspace.objValueOut[1]; -acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2]; -acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3]; -acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4]; +acadoWorkspace.Dy[runObj * 3] = acadoWorkspace.objValueOut[0]; +acadoWorkspace.Dy[runObj * 3 + 1] = acadoWorkspace.objValueOut[1]; +acadoWorkspace.Dy[runObj * 3 + 2] = acadoWorkspace.objValueOut[2]; -acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 3 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) ); -acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 15 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 3 ]) ); } -acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[340]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[341]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[342]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[343]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[344]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[345]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[346]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[347]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[348]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[349]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[350]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[351]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[352]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[353]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[354]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[355]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[356]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[32]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[33]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; -acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; -acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3]; -acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 2 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); } @@ -414,22 +295,22 @@ Gu2[3] = Gu1[3]; void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; } void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = R11[0]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = R11[0]; } void acado_zeroBlockH11( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = 0.0000000000000000e+00; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = 0.0000000000000000e+00; } void acado_copyHTH( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = acadoWorkspace.H[(iCol * 24 + 96) + (iRow + 4)]; +acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = acadoWorkspace.H[(iCol * 20 + 80) + (iRow + 4)]; } void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) @@ -450,15 +331,15 @@ dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + ac void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 ) { -RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3] + R2[4]*Dy1[4]; +RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2]; } void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 ) { -QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3] + Q2[4]*Dy1[4]; -QDy1[1] = + Q2[5]*Dy1[0] + Q2[6]*Dy1[1] + Q2[7]*Dy1[2] + Q2[8]*Dy1[3] + Q2[9]*Dy1[4]; -QDy1[2] = + Q2[10]*Dy1[0] + Q2[11]*Dy1[1] + Q2[12]*Dy1[2] + Q2[13]*Dy1[3] + Q2[14]*Dy1[4]; -QDy1[3] = + Q2[15]*Dy1[0] + Q2[16]*Dy1[1] + Q2[17]*Dy1[2] + Q2[18]*Dy1[3] + Q2[19]*Dy1[4]; +QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2]; +QDy1[1] = + Q2[3]*Dy1[0] + Q2[4]*Dy1[1] + Q2[5]*Dy1[2]; +QDy1[2] = + Q2[6]*Dy1[0] + Q2[7]*Dy1[1] + Q2[8]*Dy1[2]; +QDy1[3] = + Q2[9]*Dy1[0] + Q2[10]*Dy1[1] + Q2[11]*Dy1[2]; } void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 ) @@ -493,18 +374,18 @@ acadoWorkspace.H[0] = 0.0000000000000000e+00; acadoWorkspace.H[1] = 0.0000000000000000e+00; acadoWorkspace.H[2] = 0.0000000000000000e+00; acadoWorkspace.H[3] = 0.0000000000000000e+00; -acadoWorkspace.H[24] = 0.0000000000000000e+00; -acadoWorkspace.H[25] = 0.0000000000000000e+00; -acadoWorkspace.H[26] = 0.0000000000000000e+00; -acadoWorkspace.H[27] = 0.0000000000000000e+00; -acadoWorkspace.H[48] = 0.0000000000000000e+00; -acadoWorkspace.H[49] = 0.0000000000000000e+00; -acadoWorkspace.H[50] = 0.0000000000000000e+00; -acadoWorkspace.H[51] = 0.0000000000000000e+00; -acadoWorkspace.H[72] = 0.0000000000000000e+00; -acadoWorkspace.H[73] = 0.0000000000000000e+00; -acadoWorkspace.H[74] = 0.0000000000000000e+00; -acadoWorkspace.H[75] = 0.0000000000000000e+00; +acadoWorkspace.H[20] = 0.0000000000000000e+00; +acadoWorkspace.H[21] = 0.0000000000000000e+00; +acadoWorkspace.H[22] = 0.0000000000000000e+00; +acadoWorkspace.H[23] = 0.0000000000000000e+00; +acadoWorkspace.H[40] = 0.0000000000000000e+00; +acadoWorkspace.H[41] = 0.0000000000000000e+00; +acadoWorkspace.H[42] = 0.0000000000000000e+00; +acadoWorkspace.H[43] = 0.0000000000000000e+00; +acadoWorkspace.H[60] = 0.0000000000000000e+00; +acadoWorkspace.H[61] = 0.0000000000000000e+00; +acadoWorkspace.H[62] = 0.0000000000000000e+00; +acadoWorkspace.H[63] = 0.0000000000000000e+00; } void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) @@ -513,18 +394,18 @@ acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12] acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13]; acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14]; acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15]; -acadoWorkspace.H[24] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; -acadoWorkspace.H[25] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; -acadoWorkspace.H[26] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; -acadoWorkspace.H[27] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; -acadoWorkspace.H[48] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; -acadoWorkspace.H[49] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; -acadoWorkspace.H[50] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; -acadoWorkspace.H[51] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; -acadoWorkspace.H[72] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; -acadoWorkspace.H[73] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; -acadoWorkspace.H[74] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; -acadoWorkspace.H[75] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; +acadoWorkspace.H[20] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; +acadoWorkspace.H[21] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; +acadoWorkspace.H[22] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; +acadoWorkspace.H[23] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; +acadoWorkspace.H[40] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; +acadoWorkspace.H[41] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; +acadoWorkspace.H[42] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; +acadoWorkspace.H[43] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; +acadoWorkspace.H[60] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; +acadoWorkspace.H[61] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; +acadoWorkspace.H[62] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; +acadoWorkspace.H[63] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15]; } void acado_macCTSlx( real_t* const C0, real_t* const g0 ) @@ -552,9 +433,9 @@ int lRun2; int lRun3; int lRun4; int lRun5; -/** Row vector of size: 40 */ -static const int xBoundIndices[ 40 ] = -{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67, 70, 71, 74, 75, 78, 79, 82, 83 }; +/** Row vector of size: 32 */ +static const int xBoundIndices[ 32 ] = +{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T ); acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) ); @@ -781,104 +662,6 @@ acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 540 ]) ); -acado_moveGxT( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.d[ 64 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.evGx[ 256 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 544 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.E[ 548 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.E[ 552 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 556 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.E[ 560 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.E[ 564 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 568 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.E[ 572 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.E[ 576 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 580 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.E[ 584 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.E[ 588 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 592 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.E[ 596 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.E[ 600 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 604 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 64 ]), &(acadoWorkspace.E[ 608 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.d[ 68 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.evGx[ 272 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.E[ 612 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.E[ 616 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 620 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.E[ 624 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.E[ 628 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 632 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.E[ 636 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.E[ 640 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.E[ 644 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.E[ 648 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.E[ 652 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.E[ 656 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.E[ 660 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.E[ 664 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.E[ 668 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.E[ 672 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.E[ 676 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 68 ]), &(acadoWorkspace.E[ 680 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.d[ 72 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.evGx[ 288 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.E[ 684 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.E[ 688 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.E[ 692 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.E[ 696 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.E[ 700 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.E[ 704 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.E[ 708 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.E[ 712 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.E[ 716 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.E[ 720 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.E[ 724 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.E[ 728 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.E[ 732 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.E[ 736 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.E[ 740 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.E[ 744 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.E[ 748 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.E[ 752 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 72 ]), &(acadoWorkspace.E[ 756 ]) ); - -acado_moveGxT( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.d[ 76 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.evGx[ 304 ]) ); - -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.E[ 760 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.E[ 764 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.E[ 768 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.E[ 772 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.E[ 776 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.E[ 780 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.E[ 784 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.E[ 788 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.E[ 792 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.E[ 796 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.E[ 800 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.E[ 804 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.E[ 808 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.E[ 812 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.E[ 816 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.E[ 820 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.E[ 824 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.E[ 828 ]) ); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.E[ 832 ]) ); - -acado_moveGuE( &(acadoWorkspace.evGu[ 76 ]), &(acadoWorkspace.E[ 836 ]) ); - acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) ); @@ -894,11 +677,7 @@ acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(ac acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); -acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE ); acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); @@ -1020,96 +799,22 @@ acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acado acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_multGxGu( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); acado_zeroBlockH00( ); acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); @@ -1128,10 +833,6 @@ acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); acado_zeroBlockH10( acadoWorkspace.H10 ); acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); @@ -1150,10 +851,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), aca acado_multQETGx( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.H10 ); acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.H10 ); acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.H10 ); -acado_multQETGx( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.H10 ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 4 ]) ); @@ -1170,10 +867,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 4 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 4 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 8 ]) ); @@ -1189,10 +882,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 8 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 8 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 12 ]) ); @@ -1207,10 +896,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 12 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 12 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 16 ]) ); @@ -1224,10 +909,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 16 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 16 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 20 ]) ); @@ -1240,10 +921,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 20 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 20 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 24 ]) ); @@ -1255,10 +932,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 24 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 24 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 28 ]) ); @@ -1269,10 +942,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 28 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 28 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 32 ]) ); @@ -1282,10 +951,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 32 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 32 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 36 ]) ); @@ -1294,10 +959,6 @@ acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 36 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 36 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 40 ]) ); @@ -1305,64 +966,26 @@ acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(a acado_multQETGx( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 40 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 40 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 44 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 44 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 48 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 48 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 52 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 52 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 56 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 56 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 56 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 56 ]) ); acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) ); acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 60 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 64 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 68 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 72 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 72 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 72 ]) ); -acado_zeroBlockH10( &(acadoWorkspace.H10[ 76 ]) ); -acado_multQETGx( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.H10[ 76 ]) ); acadoWorkspace.H[4] = acadoWorkspace.H10[0]; acadoWorkspace.H[5] = acadoWorkspace.H10[4]; @@ -1380,70 +1003,54 @@ acadoWorkspace.H[16] = acadoWorkspace.H10[48]; acadoWorkspace.H[17] = acadoWorkspace.H10[52]; acadoWorkspace.H[18] = acadoWorkspace.H10[56]; acadoWorkspace.H[19] = acadoWorkspace.H10[60]; -acadoWorkspace.H[20] = acadoWorkspace.H10[64]; -acadoWorkspace.H[21] = acadoWorkspace.H10[68]; -acadoWorkspace.H[22] = acadoWorkspace.H10[72]; -acadoWorkspace.H[23] = acadoWorkspace.H10[76]; -acadoWorkspace.H[28] = acadoWorkspace.H10[1]; -acadoWorkspace.H[29] = acadoWorkspace.H10[5]; -acadoWorkspace.H[30] = acadoWorkspace.H10[9]; -acadoWorkspace.H[31] = acadoWorkspace.H10[13]; -acadoWorkspace.H[32] = acadoWorkspace.H10[17]; -acadoWorkspace.H[33] = acadoWorkspace.H10[21]; -acadoWorkspace.H[34] = acadoWorkspace.H10[25]; -acadoWorkspace.H[35] = acadoWorkspace.H10[29]; -acadoWorkspace.H[36] = acadoWorkspace.H10[33]; -acadoWorkspace.H[37] = acadoWorkspace.H10[37]; -acadoWorkspace.H[38] = acadoWorkspace.H10[41]; -acadoWorkspace.H[39] = acadoWorkspace.H10[45]; -acadoWorkspace.H[40] = acadoWorkspace.H10[49]; -acadoWorkspace.H[41] = acadoWorkspace.H10[53]; -acadoWorkspace.H[42] = acadoWorkspace.H10[57]; -acadoWorkspace.H[43] = acadoWorkspace.H10[61]; -acadoWorkspace.H[44] = acadoWorkspace.H10[65]; -acadoWorkspace.H[45] = acadoWorkspace.H10[69]; -acadoWorkspace.H[46] = acadoWorkspace.H10[73]; -acadoWorkspace.H[47] = acadoWorkspace.H10[77]; -acadoWorkspace.H[52] = acadoWorkspace.H10[2]; -acadoWorkspace.H[53] = acadoWorkspace.H10[6]; -acadoWorkspace.H[54] = acadoWorkspace.H10[10]; -acadoWorkspace.H[55] = acadoWorkspace.H10[14]; -acadoWorkspace.H[56] = acadoWorkspace.H10[18]; -acadoWorkspace.H[57] = acadoWorkspace.H10[22]; -acadoWorkspace.H[58] = acadoWorkspace.H10[26]; -acadoWorkspace.H[59] = acadoWorkspace.H10[30]; -acadoWorkspace.H[60] = acadoWorkspace.H10[34]; -acadoWorkspace.H[61] = acadoWorkspace.H10[38]; -acadoWorkspace.H[62] = acadoWorkspace.H10[42]; -acadoWorkspace.H[63] = acadoWorkspace.H10[46]; -acadoWorkspace.H[64] = acadoWorkspace.H10[50]; -acadoWorkspace.H[65] = acadoWorkspace.H10[54]; -acadoWorkspace.H[66] = acadoWorkspace.H10[58]; -acadoWorkspace.H[67] = acadoWorkspace.H10[62]; -acadoWorkspace.H[68] = acadoWorkspace.H10[66]; -acadoWorkspace.H[69] = acadoWorkspace.H10[70]; -acadoWorkspace.H[70] = acadoWorkspace.H10[74]; -acadoWorkspace.H[71] = acadoWorkspace.H10[78]; -acadoWorkspace.H[76] = acadoWorkspace.H10[3]; -acadoWorkspace.H[77] = acadoWorkspace.H10[7]; -acadoWorkspace.H[78] = acadoWorkspace.H10[11]; -acadoWorkspace.H[79] = acadoWorkspace.H10[15]; -acadoWorkspace.H[80] = acadoWorkspace.H10[19]; -acadoWorkspace.H[81] = acadoWorkspace.H10[23]; -acadoWorkspace.H[82] = acadoWorkspace.H10[27]; -acadoWorkspace.H[83] = acadoWorkspace.H10[31]; -acadoWorkspace.H[84] = acadoWorkspace.H10[35]; -acadoWorkspace.H[85] = acadoWorkspace.H10[39]; -acadoWorkspace.H[86] = acadoWorkspace.H10[43]; -acadoWorkspace.H[87] = acadoWorkspace.H10[47]; -acadoWorkspace.H[88] = acadoWorkspace.H10[51]; -acadoWorkspace.H[89] = acadoWorkspace.H10[55]; -acadoWorkspace.H[90] = acadoWorkspace.H10[59]; -acadoWorkspace.H[91] = acadoWorkspace.H10[63]; -acadoWorkspace.H[92] = acadoWorkspace.H10[67]; -acadoWorkspace.H[93] = acadoWorkspace.H10[71]; -acadoWorkspace.H[94] = acadoWorkspace.H10[75]; -acadoWorkspace.H[95] = acadoWorkspace.H10[79]; +acadoWorkspace.H[24] = acadoWorkspace.H10[1]; +acadoWorkspace.H[25] = acadoWorkspace.H10[5]; +acadoWorkspace.H[26] = acadoWorkspace.H10[9]; +acadoWorkspace.H[27] = acadoWorkspace.H10[13]; +acadoWorkspace.H[28] = acadoWorkspace.H10[17]; +acadoWorkspace.H[29] = acadoWorkspace.H10[21]; +acadoWorkspace.H[30] = acadoWorkspace.H10[25]; +acadoWorkspace.H[31] = acadoWorkspace.H10[29]; +acadoWorkspace.H[32] = acadoWorkspace.H10[33]; +acadoWorkspace.H[33] = acadoWorkspace.H10[37]; +acadoWorkspace.H[34] = acadoWorkspace.H10[41]; +acadoWorkspace.H[35] = acadoWorkspace.H10[45]; +acadoWorkspace.H[36] = acadoWorkspace.H10[49]; +acadoWorkspace.H[37] = acadoWorkspace.H10[53]; +acadoWorkspace.H[38] = acadoWorkspace.H10[57]; +acadoWorkspace.H[39] = acadoWorkspace.H10[61]; +acadoWorkspace.H[44] = acadoWorkspace.H10[2]; +acadoWorkspace.H[45] = acadoWorkspace.H10[6]; +acadoWorkspace.H[46] = acadoWorkspace.H10[10]; +acadoWorkspace.H[47] = acadoWorkspace.H10[14]; +acadoWorkspace.H[48] = acadoWorkspace.H10[18]; +acadoWorkspace.H[49] = acadoWorkspace.H10[22]; +acadoWorkspace.H[50] = acadoWorkspace.H10[26]; +acadoWorkspace.H[51] = acadoWorkspace.H10[30]; +acadoWorkspace.H[52] = acadoWorkspace.H10[34]; +acadoWorkspace.H[53] = acadoWorkspace.H10[38]; +acadoWorkspace.H[54] = acadoWorkspace.H10[42]; +acadoWorkspace.H[55] = acadoWorkspace.H10[46]; +acadoWorkspace.H[56] = acadoWorkspace.H10[50]; +acadoWorkspace.H[57] = acadoWorkspace.H10[54]; +acadoWorkspace.H[58] = acadoWorkspace.H10[58]; +acadoWorkspace.H[59] = acadoWorkspace.H10[62]; +acadoWorkspace.H[64] = acadoWorkspace.H10[3]; +acadoWorkspace.H[65] = acadoWorkspace.H10[7]; +acadoWorkspace.H[66] = acadoWorkspace.H10[11]; +acadoWorkspace.H[67] = acadoWorkspace.H10[15]; +acadoWorkspace.H[68] = acadoWorkspace.H10[19]; +acadoWorkspace.H[69] = acadoWorkspace.H10[23]; +acadoWorkspace.H[70] = acadoWorkspace.H10[27]; +acadoWorkspace.H[71] = acadoWorkspace.H10[31]; +acadoWorkspace.H[72] = acadoWorkspace.H10[35]; +acadoWorkspace.H[73] = acadoWorkspace.H10[39]; +acadoWorkspace.H[74] = acadoWorkspace.H10[43]; +acadoWorkspace.H[75] = acadoWorkspace.H10[47]; +acadoWorkspace.H[76] = acadoWorkspace.H10[51]; +acadoWorkspace.H[77] = acadoWorkspace.H10[55]; +acadoWorkspace.H[78] = acadoWorkspace.H10[59]; +acadoWorkspace.H[79] = acadoWorkspace.H10[63]; acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); @@ -1462,10 +1069,6 @@ acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 544 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); -acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 760 ]) ); acado_zeroBlockH11( 0, 1 ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 8 ]) ); @@ -1483,10 +1086,6 @@ acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 316 ]) acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 368 ]) ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 424 ]) ); acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 764 ]) ); acado_zeroBlockH11( 0, 2 ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1503,10 +1102,6 @@ acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 0, 3 ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1522,10 +1117,6 @@ acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 0, 4 ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1540,10 +1131,6 @@ acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 0, 5 ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1557,10 +1144,6 @@ acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 0, 6 ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -1573,10 +1156,6 @@ acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 0, 7 ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -1588,10 +1167,6 @@ acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 0, 8 ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -1602,10 +1177,6 @@ acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 0, 9 ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -1615,10 +1186,6 @@ acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 0, 10 ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -1627,10 +1194,6 @@ acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 0, 11 ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -1638,62 +1201,24 @@ acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 0, 12 ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 0, 13 ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 0, 14 ); acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 0, 15 ); acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 0, 16 ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 0, 17 ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 0, 18 ); -acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 0, 19 ); -acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); @@ -1711,10 +1236,6 @@ acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 548 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 616 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 688 ]) ); -acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 764 ]) ); acado_zeroBlockH11( 1, 2 ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1731,10 +1252,6 @@ acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 1, 3 ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1750,10 +1267,6 @@ acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 1, 4 ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1768,10 +1281,6 @@ acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 1, 5 ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1785,10 +1294,6 @@ acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 1, 6 ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -1801,10 +1306,6 @@ acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 1, 7 ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -1816,10 +1317,6 @@ acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 1, 8 ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -1830,10 +1327,6 @@ acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 1, 9 ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -1843,10 +1336,6 @@ acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 1, 10 ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -1855,10 +1344,6 @@ acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 1, 11 ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -1866,62 +1351,24 @@ acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 1, 12 ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 1, 13 ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 1, 14 ); acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 1, 15 ); acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 1, 16 ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 1, 17 ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 1, 18 ); -acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 1, 19 ); -acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); @@ -1938,10 +1385,6 @@ acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 620 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 692 ]) ); -acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); acado_zeroBlockH11( 2, 3 ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -1957,10 +1400,6 @@ acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 2, 4 ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -1975,10 +1414,6 @@ acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 2, 5 ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -1992,10 +1427,6 @@ acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 2, 6 ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2008,10 +1439,6 @@ acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 2, 7 ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2023,10 +1450,6 @@ acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 2, 8 ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2037,10 +1460,6 @@ acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 2, 9 ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2050,10 +1469,6 @@ acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 2, 10 ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2062,10 +1477,6 @@ acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 2, 11 ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2073,62 +1484,24 @@ acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 2, 12 ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 2, 13 ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 2, 14 ); acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 2, 15 ); acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 2, 16 ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 2, 17 ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 2, 18 ); -acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 2, 19 ); -acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); @@ -2144,10 +1517,6 @@ acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 556 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); -acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 772 ]) ); acado_zeroBlockH11( 3, 4 ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -2162,10 +1531,6 @@ acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 3, 5 ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2179,10 +1544,6 @@ acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 3, 6 ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2195,10 +1556,6 @@ acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 3, 7 ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2210,10 +1567,6 @@ acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 3, 8 ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2224,10 +1577,6 @@ acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 3, 9 ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2237,10 +1586,6 @@ acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 3, 10 ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2249,10 +1594,6 @@ acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 3, 11 ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2260,62 +1601,24 @@ acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 3, 12 ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 3, 13 ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 3, 14 ); acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 3, 15 ); acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 3, 16 ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 3, 17 ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 3, 18 ); -acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 3, 19 ); -acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); @@ -2330,10 +1633,6 @@ acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 560 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 628 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 700 ]) ); -acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 776 ]) ); acado_zeroBlockH11( 4, 5 ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2347,10 +1646,6 @@ acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 4, 6 ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2363,10 +1658,6 @@ acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 4, 7 ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2378,10 +1669,6 @@ acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 4, 8 ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2392,10 +1679,6 @@ acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 4, 9 ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2405,10 +1688,6 @@ acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 4, 10 ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2417,10 +1696,6 @@ acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 4, 11 ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2428,62 +1703,24 @@ acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 4, 12 ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 4, 13 ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 4, 14 ); acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 4, 15 ); acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 4, 16 ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 4, 17 ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 4, 18 ); -acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 4, 19 ); -acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); @@ -2497,10 +1734,6 @@ acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 632 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 704 ]) ); -acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); acado_zeroBlockH11( 5, 6 ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2513,10 +1746,6 @@ acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 5, 7 ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2528,10 +1757,6 @@ acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 5, 8 ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2542,10 +1767,6 @@ acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 5, 9 ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2555,10 +1776,6 @@ acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 5, 10 ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2567,10 +1784,6 @@ acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 5, 11 ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2578,62 +1791,24 @@ acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 5, 12 ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 5, 13 ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 5, 14 ); acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 5, 15 ); acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 5, 16 ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 5, 17 ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 5, 18 ); -acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 5, 19 ); -acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); @@ -2646,10 +1821,6 @@ acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 568 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); -acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 784 ]) ); acado_zeroBlockH11( 6, 7 ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2661,10 +1832,6 @@ acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 6, 8 ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2675,10 +1842,6 @@ acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 6, 9 ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2688,10 +1851,6 @@ acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 6, 10 ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2700,10 +1859,6 @@ acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 6, 11 ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2711,62 +1866,24 @@ acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 6, 12 ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 6, 13 ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 6, 14 ); acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 6, 15 ); acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 6, 16 ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 6, 17 ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 6, 18 ); -acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 6, 19 ); -acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); @@ -2778,10 +1895,6 @@ acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 572 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 640 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 712 ]) ); -acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 788 ]) ); acado_zeroBlockH11( 7, 8 ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2792,10 +1905,6 @@ acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 7, 9 ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2805,10 +1914,6 @@ acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 7, 10 ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2817,10 +1922,6 @@ acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 7, 11 ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2828,62 +1929,24 @@ acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 7, 12 ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 7, 13 ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 7, 14 ); acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 7, 15 ); acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 7, 16 ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 7, 17 ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 7, 18 ); -acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 7, 19 ); -acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); @@ -2894,10 +1957,6 @@ acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 644 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 716 ]) ); -acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); acado_zeroBlockH11( 8, 9 ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2907,10 +1966,6 @@ acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 8, 10 ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -2919,10 +1974,6 @@ acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 8, 11 ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -2930,62 +1981,24 @@ acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 8, 12 ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 8, 13 ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 8, 14 ); acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 8, 15 ); acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 8, 16 ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 8, 17 ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 8, 18 ); -acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 8, 19 ); -acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); @@ -2995,10 +2008,6 @@ acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 580 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); -acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 796 ]) ); acado_zeroBlockH11( 9, 10 ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -3007,10 +2016,6 @@ acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 352 ] acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 9, 11 ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3018,62 +2023,24 @@ acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 356 ] acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 9, 12 ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 9, 13 ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 9, 14 ); acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 9, 15 ); acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 9, 16 ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 9, 17 ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 9, 18 ); -acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 9, 19 ); -acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); @@ -3082,10 +2049,6 @@ acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 584 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 652 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 724 ]) ); -acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 800 ]) ); acado_zeroBlockH11( 10, 11 ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3093,62 +2056,24 @@ acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 356 acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 10, 12 ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 10, 13 ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 10, 14 ); acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 10, 15 ); acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 10, 16 ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 10, 17 ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 10, 18 ); -acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 10, 19 ); -acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); @@ -3156,254 +2081,64 @@ acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 656 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 728 ]) ); -acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); acado_zeroBlockH11( 11, 12 ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 11, 13 ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 11, 14 ); acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 11, 15 ); acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 11, 16 ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 11, 17 ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 11, 18 ); -acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 11, 19 ); -acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 592 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); -acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 808 ]) ); acado_zeroBlockH11( 12, 13 ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 12, 14 ); acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 12, 15 ); acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 12, 16 ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 12, 17 ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 12, 18 ); -acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 12, 19 ); -acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) ); acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 596 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 664 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 736 ]) ); -acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 812 ]) ); acado_zeroBlockH11( 13, 14 ); acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 13, 15 ); acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 13, 16 ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 13, 17 ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 13, 18 ); -acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 13, 19 ); -acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) ); acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) ); acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 668 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 740 ]) ); -acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); acado_zeroBlockH11( 14, 15 ); acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 14, 16 ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 14, 17 ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 14, 18 ); -acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 14, 19 ); -acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) ); acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 604 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); -acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 820 ]) ); - -acado_zeroBlockH11( 15, 16 ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 15, 17 ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 15, 18 ); -acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 15, 19 ); -acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QE[ 608 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 676 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 748 ]) ); -acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 824 ]) ); - -acado_zeroBlockH11( 16, 17 ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 16, 18 ); -acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 16, 19 ); -acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QE[ 680 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 752 ]) ); -acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); - -acado_zeroBlockH11( 17, 18 ); -acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 17, 19 ); -acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) ); -acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); -acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 832 ]) ); - -acado_zeroBlockH11( 18, 19 ); -acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QE[ 836 ]) ); - -acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) ); -acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QE[ 836 ]) ); acado_copyHTH( 1, 0 ); @@ -3526,157 +2261,71 @@ acado_copyHTH( 15, 11 ); acado_copyHTH( 15, 12 ); acado_copyHTH( 15, 13 ); acado_copyHTH( 15, 14 ); -acado_copyHTH( 16, 0 ); -acado_copyHTH( 16, 1 ); -acado_copyHTH( 16, 2 ); -acado_copyHTH( 16, 3 ); -acado_copyHTH( 16, 4 ); -acado_copyHTH( 16, 5 ); -acado_copyHTH( 16, 6 ); -acado_copyHTH( 16, 7 ); -acado_copyHTH( 16, 8 ); -acado_copyHTH( 16, 9 ); -acado_copyHTH( 16, 10 ); -acado_copyHTH( 16, 11 ); -acado_copyHTH( 16, 12 ); -acado_copyHTH( 16, 13 ); -acado_copyHTH( 16, 14 ); -acado_copyHTH( 16, 15 ); -acado_copyHTH( 17, 0 ); -acado_copyHTH( 17, 1 ); -acado_copyHTH( 17, 2 ); -acado_copyHTH( 17, 3 ); -acado_copyHTH( 17, 4 ); -acado_copyHTH( 17, 5 ); -acado_copyHTH( 17, 6 ); -acado_copyHTH( 17, 7 ); -acado_copyHTH( 17, 8 ); -acado_copyHTH( 17, 9 ); -acado_copyHTH( 17, 10 ); -acado_copyHTH( 17, 11 ); -acado_copyHTH( 17, 12 ); -acado_copyHTH( 17, 13 ); -acado_copyHTH( 17, 14 ); -acado_copyHTH( 17, 15 ); -acado_copyHTH( 17, 16 ); -acado_copyHTH( 18, 0 ); -acado_copyHTH( 18, 1 ); -acado_copyHTH( 18, 2 ); -acado_copyHTH( 18, 3 ); -acado_copyHTH( 18, 4 ); -acado_copyHTH( 18, 5 ); -acado_copyHTH( 18, 6 ); -acado_copyHTH( 18, 7 ); -acado_copyHTH( 18, 8 ); -acado_copyHTH( 18, 9 ); -acado_copyHTH( 18, 10 ); -acado_copyHTH( 18, 11 ); -acado_copyHTH( 18, 12 ); -acado_copyHTH( 18, 13 ); -acado_copyHTH( 18, 14 ); -acado_copyHTH( 18, 15 ); -acado_copyHTH( 18, 16 ); -acado_copyHTH( 18, 17 ); -acado_copyHTH( 19, 0 ); -acado_copyHTH( 19, 1 ); -acado_copyHTH( 19, 2 ); -acado_copyHTH( 19, 3 ); -acado_copyHTH( 19, 4 ); -acado_copyHTH( 19, 5 ); -acado_copyHTH( 19, 6 ); -acado_copyHTH( 19, 7 ); -acado_copyHTH( 19, 8 ); -acado_copyHTH( 19, 9 ); -acado_copyHTH( 19, 10 ); -acado_copyHTH( 19, 11 ); -acado_copyHTH( 19, 12 ); -acado_copyHTH( 19, 13 ); -acado_copyHTH( 19, 14 ); -acado_copyHTH( 19, 15 ); -acado_copyHTH( 19, 16 ); -acado_copyHTH( 19, 17 ); -acado_copyHTH( 19, 18 ); -acadoWorkspace.H[96] = acadoWorkspace.H10[0]; -acadoWorkspace.H[97] = acadoWorkspace.H10[1]; -acadoWorkspace.H[98] = acadoWorkspace.H10[2]; -acadoWorkspace.H[99] = acadoWorkspace.H10[3]; -acadoWorkspace.H[120] = acadoWorkspace.H10[4]; -acadoWorkspace.H[121] = acadoWorkspace.H10[5]; -acadoWorkspace.H[122] = acadoWorkspace.H10[6]; -acadoWorkspace.H[123] = acadoWorkspace.H10[7]; -acadoWorkspace.H[144] = acadoWorkspace.H10[8]; -acadoWorkspace.H[145] = acadoWorkspace.H10[9]; -acadoWorkspace.H[146] = acadoWorkspace.H10[10]; -acadoWorkspace.H[147] = acadoWorkspace.H10[11]; -acadoWorkspace.H[168] = acadoWorkspace.H10[12]; -acadoWorkspace.H[169] = acadoWorkspace.H10[13]; -acadoWorkspace.H[170] = acadoWorkspace.H10[14]; -acadoWorkspace.H[171] = acadoWorkspace.H10[15]; -acadoWorkspace.H[192] = acadoWorkspace.H10[16]; -acadoWorkspace.H[193] = acadoWorkspace.H10[17]; -acadoWorkspace.H[194] = acadoWorkspace.H10[18]; -acadoWorkspace.H[195] = acadoWorkspace.H10[19]; -acadoWorkspace.H[216] = acadoWorkspace.H10[20]; -acadoWorkspace.H[217] = acadoWorkspace.H10[21]; -acadoWorkspace.H[218] = acadoWorkspace.H10[22]; -acadoWorkspace.H[219] = acadoWorkspace.H10[23]; -acadoWorkspace.H[240] = acadoWorkspace.H10[24]; -acadoWorkspace.H[241] = acadoWorkspace.H10[25]; -acadoWorkspace.H[242] = acadoWorkspace.H10[26]; -acadoWorkspace.H[243] = acadoWorkspace.H10[27]; -acadoWorkspace.H[264] = acadoWorkspace.H10[28]; -acadoWorkspace.H[265] = acadoWorkspace.H10[29]; -acadoWorkspace.H[266] = acadoWorkspace.H10[30]; -acadoWorkspace.H[267] = acadoWorkspace.H10[31]; -acadoWorkspace.H[288] = acadoWorkspace.H10[32]; -acadoWorkspace.H[289] = acadoWorkspace.H10[33]; -acadoWorkspace.H[290] = acadoWorkspace.H10[34]; -acadoWorkspace.H[291] = acadoWorkspace.H10[35]; -acadoWorkspace.H[312] = acadoWorkspace.H10[36]; -acadoWorkspace.H[313] = acadoWorkspace.H10[37]; -acadoWorkspace.H[314] = acadoWorkspace.H10[38]; -acadoWorkspace.H[315] = acadoWorkspace.H10[39]; -acadoWorkspace.H[336] = acadoWorkspace.H10[40]; -acadoWorkspace.H[337] = acadoWorkspace.H10[41]; -acadoWorkspace.H[338] = acadoWorkspace.H10[42]; -acadoWorkspace.H[339] = acadoWorkspace.H10[43]; -acadoWorkspace.H[360] = acadoWorkspace.H10[44]; -acadoWorkspace.H[361] = acadoWorkspace.H10[45]; -acadoWorkspace.H[362] = acadoWorkspace.H10[46]; -acadoWorkspace.H[363] = acadoWorkspace.H10[47]; -acadoWorkspace.H[384] = acadoWorkspace.H10[48]; -acadoWorkspace.H[385] = acadoWorkspace.H10[49]; -acadoWorkspace.H[386] = acadoWorkspace.H10[50]; -acadoWorkspace.H[387] = acadoWorkspace.H10[51]; -acadoWorkspace.H[408] = acadoWorkspace.H10[52]; -acadoWorkspace.H[409] = acadoWorkspace.H10[53]; -acadoWorkspace.H[410] = acadoWorkspace.H10[54]; -acadoWorkspace.H[411] = acadoWorkspace.H10[55]; -acadoWorkspace.H[432] = acadoWorkspace.H10[56]; -acadoWorkspace.H[433] = acadoWorkspace.H10[57]; -acadoWorkspace.H[434] = acadoWorkspace.H10[58]; -acadoWorkspace.H[435] = acadoWorkspace.H10[59]; -acadoWorkspace.H[456] = acadoWorkspace.H10[60]; -acadoWorkspace.H[457] = acadoWorkspace.H10[61]; -acadoWorkspace.H[458] = acadoWorkspace.H10[62]; -acadoWorkspace.H[459] = acadoWorkspace.H10[63]; -acadoWorkspace.H[480] = acadoWorkspace.H10[64]; -acadoWorkspace.H[481] = acadoWorkspace.H10[65]; -acadoWorkspace.H[482] = acadoWorkspace.H10[66]; -acadoWorkspace.H[483] = acadoWorkspace.H10[67]; -acadoWorkspace.H[504] = acadoWorkspace.H10[68]; -acadoWorkspace.H[505] = acadoWorkspace.H10[69]; -acadoWorkspace.H[506] = acadoWorkspace.H10[70]; -acadoWorkspace.H[507] = acadoWorkspace.H10[71]; -acadoWorkspace.H[528] = acadoWorkspace.H10[72]; -acadoWorkspace.H[529] = acadoWorkspace.H10[73]; -acadoWorkspace.H[530] = acadoWorkspace.H10[74]; -acadoWorkspace.H[531] = acadoWorkspace.H10[75]; -acadoWorkspace.H[552] = acadoWorkspace.H10[76]; -acadoWorkspace.H[553] = acadoWorkspace.H10[77]; -acadoWorkspace.H[554] = acadoWorkspace.H10[78]; -acadoWorkspace.H[555] = acadoWorkspace.H10[79]; +acadoWorkspace.H[80] = acadoWorkspace.H10[0]; +acadoWorkspace.H[81] = acadoWorkspace.H10[1]; +acadoWorkspace.H[82] = acadoWorkspace.H10[2]; +acadoWorkspace.H[83] = acadoWorkspace.H10[3]; +acadoWorkspace.H[100] = acadoWorkspace.H10[4]; +acadoWorkspace.H[101] = acadoWorkspace.H10[5]; +acadoWorkspace.H[102] = acadoWorkspace.H10[6]; +acadoWorkspace.H[103] = acadoWorkspace.H10[7]; +acadoWorkspace.H[120] = acadoWorkspace.H10[8]; +acadoWorkspace.H[121] = acadoWorkspace.H10[9]; +acadoWorkspace.H[122] = acadoWorkspace.H10[10]; +acadoWorkspace.H[123] = acadoWorkspace.H10[11]; +acadoWorkspace.H[140] = acadoWorkspace.H10[12]; +acadoWorkspace.H[141] = acadoWorkspace.H10[13]; +acadoWorkspace.H[142] = acadoWorkspace.H10[14]; +acadoWorkspace.H[143] = acadoWorkspace.H10[15]; +acadoWorkspace.H[160] = acadoWorkspace.H10[16]; +acadoWorkspace.H[161] = acadoWorkspace.H10[17]; +acadoWorkspace.H[162] = acadoWorkspace.H10[18]; +acadoWorkspace.H[163] = acadoWorkspace.H10[19]; +acadoWorkspace.H[180] = acadoWorkspace.H10[20]; +acadoWorkspace.H[181] = acadoWorkspace.H10[21]; +acadoWorkspace.H[182] = acadoWorkspace.H10[22]; +acadoWorkspace.H[183] = acadoWorkspace.H10[23]; +acadoWorkspace.H[200] = acadoWorkspace.H10[24]; +acadoWorkspace.H[201] = acadoWorkspace.H10[25]; +acadoWorkspace.H[202] = acadoWorkspace.H10[26]; +acadoWorkspace.H[203] = acadoWorkspace.H10[27]; +acadoWorkspace.H[220] = acadoWorkspace.H10[28]; +acadoWorkspace.H[221] = acadoWorkspace.H10[29]; +acadoWorkspace.H[222] = acadoWorkspace.H10[30]; +acadoWorkspace.H[223] = acadoWorkspace.H10[31]; +acadoWorkspace.H[240] = acadoWorkspace.H10[32]; +acadoWorkspace.H[241] = acadoWorkspace.H10[33]; +acadoWorkspace.H[242] = acadoWorkspace.H10[34]; +acadoWorkspace.H[243] = acadoWorkspace.H10[35]; +acadoWorkspace.H[260] = acadoWorkspace.H10[36]; +acadoWorkspace.H[261] = acadoWorkspace.H10[37]; +acadoWorkspace.H[262] = acadoWorkspace.H10[38]; +acadoWorkspace.H[263] = acadoWorkspace.H10[39]; +acadoWorkspace.H[280] = acadoWorkspace.H10[40]; +acadoWorkspace.H[281] = acadoWorkspace.H10[41]; +acadoWorkspace.H[282] = acadoWorkspace.H10[42]; +acadoWorkspace.H[283] = acadoWorkspace.H10[43]; +acadoWorkspace.H[300] = acadoWorkspace.H10[44]; +acadoWorkspace.H[301] = acadoWorkspace.H10[45]; +acadoWorkspace.H[302] = acadoWorkspace.H10[46]; +acadoWorkspace.H[303] = acadoWorkspace.H10[47]; +acadoWorkspace.H[320] = acadoWorkspace.H10[48]; +acadoWorkspace.H[321] = acadoWorkspace.H10[49]; +acadoWorkspace.H[322] = acadoWorkspace.H10[50]; +acadoWorkspace.H[323] = acadoWorkspace.H10[51]; +acadoWorkspace.H[340] = acadoWorkspace.H10[52]; +acadoWorkspace.H[341] = acadoWorkspace.H10[53]; +acadoWorkspace.H[342] = acadoWorkspace.H10[54]; +acadoWorkspace.H[343] = acadoWorkspace.H10[55]; +acadoWorkspace.H[360] = acadoWorkspace.H10[56]; +acadoWorkspace.H[361] = acadoWorkspace.H10[57]; +acadoWorkspace.H[362] = acadoWorkspace.H10[58]; +acadoWorkspace.H[363] = acadoWorkspace.H10[59]; +acadoWorkspace.H[380] = acadoWorkspace.H10[60]; +acadoWorkspace.H[381] = acadoWorkspace.H10[61]; +acadoWorkspace.H[382] = acadoWorkspace.H10[62]; +acadoWorkspace.H[383] = acadoWorkspace.H10[63]; acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); @@ -3693,11 +2342,7 @@ acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWo acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.Qd[ 64 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.Qd[ 68 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) ); -acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) ); acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); @@ -3715,10 +2360,6 @@ acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.g ); acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); @@ -3735,10 +2376,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 544 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 760 ]), &(acadoWorkspace.g[ 4 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); @@ -3754,10 +2391,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 548 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 616 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 688 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 764 ]), &(acadoWorkspace.g[ 5 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); @@ -3772,10 +2405,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 620 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 692 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.g[ 6 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); @@ -3789,10 +2418,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 556 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 772 ]), &(acadoWorkspace.g[ 7 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.g[ 8 ]) ); @@ -3805,10 +2430,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 560 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 628 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 700 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 776 ]), &(acadoWorkspace.g[ 8 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 9 ]) ); @@ -3820,10 +2441,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 632 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 704 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.g[ 9 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 10 ]) ); @@ -3834,10 +2451,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 568 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 784 ]), &(acadoWorkspace.g[ 10 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.g[ 11 ]) ); @@ -3847,10 +2460,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 572 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 640 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 712 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 788 ]), &(acadoWorkspace.g[ 11 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) ); @@ -3859,10 +2468,6 @@ acado_macETSlu( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 644 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 716 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.g[ 12 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 13 ]) ); @@ -3870,65 +2475,27 @@ acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 580 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 796 ]), &(acadoWorkspace.g[ 13 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 584 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 652 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 724 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 800 ]), &(acadoWorkspace.g[ 14 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 656 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 728 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.g[ 15 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 592 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 808 ]), &(acadoWorkspace.g[ 16 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 596 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 664 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 736 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 812 ]), &(acadoWorkspace.g[ 17 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.g[ 18 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 668 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 740 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.g[ 18 ]) ); acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 604 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 820 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 608 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 676 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 748 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 824 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 680 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 752 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 832 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_macETSlu( &(acadoWorkspace.QE[ 836 ]), &(acadoWorkspace.g[ 23 ]) ); acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2]; @@ -3945,10 +2512,6 @@ acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12]; acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13]; acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14]; acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15]; -acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16]; -acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17]; -acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18]; -acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19]; acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2]; @@ -3965,23 +2528,19 @@ acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12]; acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13]; acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14]; acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15]; -acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16]; -acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17]; -acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18]; -acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19]; -for (lRun1 = 0; lRun1 < 40; ++lRun1) +for (lRun1 = 0; lRun1 < 32; ++lRun1) { lRun3 = xBoundIndices[ lRun1 ] - 4; lRun4 = ((lRun3) / (4)) + (1); -acadoWorkspace.A[lRun1 * 24] = acadoWorkspace.evGx[lRun3 * 4]; -acadoWorkspace.A[lRun1 * 24 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; -acadoWorkspace.A[lRun1 * 24 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; -acadoWorkspace.A[lRun1 * 24 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; +acadoWorkspace.A[lRun1 * 20] = acadoWorkspace.evGx[lRun3 * 4]; +acadoWorkspace.A[lRun1 * 20 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; +acadoWorkspace.A[lRun1 * 20 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; +acadoWorkspace.A[lRun1 * 20 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; for (lRun2 = 0; lRun2 < lRun4; ++lRun2) { lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4)); -acadoWorkspace.A[(lRun1 * 24) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; +acadoWorkspace.A[(lRun1 * 20) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; } } @@ -4044,109 +2603,47 @@ acadoWorkspace.Dy[44] -= acadoVariables.y[44]; acadoWorkspace.Dy[45] -= acadoVariables.y[45]; acadoWorkspace.Dy[46] -= acadoVariables.y[46]; acadoWorkspace.Dy[47] -= acadoVariables.y[47]; -acadoWorkspace.Dy[48] -= acadoVariables.y[48]; -acadoWorkspace.Dy[49] -= acadoVariables.y[49]; -acadoWorkspace.Dy[50] -= acadoVariables.y[50]; -acadoWorkspace.Dy[51] -= acadoVariables.y[51]; -acadoWorkspace.Dy[52] -= acadoVariables.y[52]; -acadoWorkspace.Dy[53] -= acadoVariables.y[53]; -acadoWorkspace.Dy[54] -= acadoVariables.y[54]; -acadoWorkspace.Dy[55] -= acadoVariables.y[55]; -acadoWorkspace.Dy[56] -= acadoVariables.y[56]; -acadoWorkspace.Dy[57] -= acadoVariables.y[57]; -acadoWorkspace.Dy[58] -= acadoVariables.y[58]; -acadoWorkspace.Dy[59] -= acadoVariables.y[59]; -acadoWorkspace.Dy[60] -= acadoVariables.y[60]; -acadoWorkspace.Dy[61] -= acadoVariables.y[61]; -acadoWorkspace.Dy[62] -= acadoVariables.y[62]; -acadoWorkspace.Dy[63] -= acadoVariables.y[63]; -acadoWorkspace.Dy[64] -= acadoVariables.y[64]; -acadoWorkspace.Dy[65] -= acadoVariables.y[65]; -acadoWorkspace.Dy[66] -= acadoVariables.y[66]; -acadoWorkspace.Dy[67] -= acadoVariables.y[67]; -acadoWorkspace.Dy[68] -= acadoVariables.y[68]; -acadoWorkspace.Dy[69] -= acadoVariables.y[69]; -acadoWorkspace.Dy[70] -= acadoVariables.y[70]; -acadoWorkspace.Dy[71] -= acadoVariables.y[71]; -acadoWorkspace.Dy[72] -= acadoVariables.y[72]; -acadoWorkspace.Dy[73] -= acadoVariables.y[73]; -acadoWorkspace.Dy[74] -= acadoVariables.y[74]; -acadoWorkspace.Dy[75] -= acadoVariables.y[75]; -acadoWorkspace.Dy[76] -= acadoVariables.y[76]; -acadoWorkspace.Dy[77] -= acadoVariables.y[77]; -acadoWorkspace.Dy[78] -= acadoVariables.y[78]; -acadoWorkspace.Dy[79] -= acadoVariables.y[79]; -acadoWorkspace.Dy[80] -= acadoVariables.y[80]; -acadoWorkspace.Dy[81] -= acadoVariables.y[81]; -acadoWorkspace.Dy[82] -= acadoVariables.y[82]; -acadoWorkspace.Dy[83] -= acadoVariables.y[83]; -acadoWorkspace.Dy[84] -= acadoVariables.y[84]; -acadoWorkspace.Dy[85] -= acadoVariables.y[85]; -acadoWorkspace.Dy[86] -= acadoVariables.y[86]; -acadoWorkspace.Dy[87] -= acadoVariables.y[87]; -acadoWorkspace.Dy[88] -= acadoVariables.y[88]; -acadoWorkspace.Dy[89] -= acadoVariables.y[89]; -acadoWorkspace.Dy[90] -= acadoVariables.y[90]; -acadoWorkspace.Dy[91] -= acadoVariables.y[91]; -acadoWorkspace.Dy[92] -= acadoVariables.y[92]; -acadoWorkspace.Dy[93] -= acadoVariables.y[93]; -acadoWorkspace.Dy[94] -= acadoVariables.y[94]; -acadoWorkspace.Dy[95] -= acadoVariables.y[95]; -acadoWorkspace.Dy[96] -= acadoVariables.y[96]; -acadoWorkspace.Dy[97] -= acadoVariables.y[97]; -acadoWorkspace.Dy[98] -= acadoVariables.y[98]; -acadoWorkspace.Dy[99] -= acadoVariables.y[99]; acadoWorkspace.DyN[0] -= acadoVariables.yN[0]; acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; -acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; -acadoWorkspace.DyN[3] -= acadoVariables.yN[3]; acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 5 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 10 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 25 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 35 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 50 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 55 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 65 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 70 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 75 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 85 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 90 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 95 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 3 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 6 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 9 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 18 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 21 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 27 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 33 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 39 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 42 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 19 ]) ); acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); -acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 40 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.QDy[ 8 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 12 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 80 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 16 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 100 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.QDy[ 20 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 24 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 140 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.QDy[ 28 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 160 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 32 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 36 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 200 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.QDy[ 40 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 220 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.QDy[ 44 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 48 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 260 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.QDy[ 52 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 280 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.QDy[ 56 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 300 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.QDy[ 60 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 64 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.QDy[ 4 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.QDy[ 8 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.QDy[ 12 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 16 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 20 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.QDy[ 24 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.QDy[ 28 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 32 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.QDy[ 36 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 40 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.QDy[ 44 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 48 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.QDy[ 52 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.QDy[ 56 ]) ); +acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 60 ]) ); -acadoWorkspace.QDy[80] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[81] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[82] = + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[3]; -acadoWorkspace.QDy[83] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[64] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[65] = + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[66] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1]; +acadoWorkspace.QDy[67] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1]; acadoWorkspace.QDy[4] += acadoWorkspace.Qd[0]; acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1]; @@ -4212,27 +2709,11 @@ acadoWorkspace.QDy[64] += acadoWorkspace.Qd[60]; acadoWorkspace.QDy[65] += acadoWorkspace.Qd[61]; acadoWorkspace.QDy[66] += acadoWorkspace.Qd[62]; acadoWorkspace.QDy[67] += acadoWorkspace.Qd[63]; -acadoWorkspace.QDy[68] += acadoWorkspace.Qd[64]; -acadoWorkspace.QDy[69] += acadoWorkspace.Qd[65]; -acadoWorkspace.QDy[70] += acadoWorkspace.Qd[66]; -acadoWorkspace.QDy[71] += acadoWorkspace.Qd[67]; -acadoWorkspace.QDy[72] += acadoWorkspace.Qd[68]; -acadoWorkspace.QDy[73] += acadoWorkspace.Qd[69]; -acadoWorkspace.QDy[74] += acadoWorkspace.Qd[70]; -acadoWorkspace.QDy[75] += acadoWorkspace.Qd[71]; -acadoWorkspace.QDy[76] += acadoWorkspace.Qd[72]; -acadoWorkspace.QDy[77] += acadoWorkspace.Qd[73]; -acadoWorkspace.QDy[78] += acadoWorkspace.Qd[74]; -acadoWorkspace.QDy[79] += acadoWorkspace.Qd[75]; -acadoWorkspace.QDy[80] += acadoWorkspace.Qd[76]; -acadoWorkspace.QDy[81] += acadoWorkspace.Qd[77]; -acadoWorkspace.QDy[82] += acadoWorkspace.Qd[78]; -acadoWorkspace.QDy[83] += acadoWorkspace.Qd[79]; -acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[83]; -acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[83]; +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67]; +acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67]; acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); @@ -4251,10 +2732,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 4 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 4 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 5 ]) ); @@ -4270,10 +2747,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 5 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 5 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 6 ]) ); @@ -4288,10 +2761,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 6 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 6 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); @@ -4305,10 +2774,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 7 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 7 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 8 ]) ); @@ -4321,10 +2786,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 8 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 8 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 9 ]) ); @@ -4336,10 +2797,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 9 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 9 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) ); @@ -4350,10 +2807,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 10 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 10 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 11 ]) ); @@ -4363,10 +2816,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 11 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 11 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 12 ]) ); @@ -4375,10 +2824,6 @@ acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 12 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 12 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) ); @@ -4386,65 +2831,27 @@ acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acado acado_multEQDy( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 13 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 13 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 14 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 14 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 15 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 15 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 16 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 16 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 17 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 17 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 18 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 18 ]) ); acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 19 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.QDy[ 68 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 20 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 21 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QDy[ 76 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 22 ]) ); -acado_multEQDy( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.QDy[ 80 ]), &(acadoWorkspace.g[ 23 ]) ); acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; @@ -4550,30 +2957,6 @@ acadoWorkspace.ubA[30] = (real_t)1.5707963268000000e+00 - tmp; tmp = acadoVariables.x[67] + acadoWorkspace.d[63]; acadoWorkspace.lbA[31] = (real_t)-8.7266462600000005e-01 - tmp; acadoWorkspace.ubA[31] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[70] + acadoWorkspace.d[66]; -acadoWorkspace.lbA[32] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[32] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[71] + acadoWorkspace.d[67]; -acadoWorkspace.lbA[33] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[33] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[74] + acadoWorkspace.d[70]; -acadoWorkspace.lbA[34] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[34] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[75] + acadoWorkspace.d[71]; -acadoWorkspace.lbA[35] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[35] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[78] + acadoWorkspace.d[74]; -acadoWorkspace.lbA[36] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[36] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[79] + acadoWorkspace.d[75]; -acadoWorkspace.lbA[37] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[37] = (real_t)8.7266462600000005e-01 - tmp; -tmp = acadoVariables.x[82] + acadoWorkspace.d[78]; -acadoWorkspace.lbA[38] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[83] + acadoWorkspace.d[79]; -acadoWorkspace.lbA[39] = (real_t)-8.7266462600000005e-01 - tmp; -acadoWorkspace.ubA[39] = (real_t)8.7266462600000005e-01 - tmp; } @@ -4600,10 +2983,6 @@ acadoVariables.u[12] += acadoWorkspace.x[16]; acadoVariables.u[13] += acadoWorkspace.x[17]; acadoVariables.u[14] += acadoWorkspace.x[18]; acadoVariables.u[15] += acadoWorkspace.x[19]; -acadoVariables.u[16] += acadoWorkspace.x[20]; -acadoVariables.u[17] += acadoWorkspace.x[21]; -acadoVariables.u[18] += acadoWorkspace.x[22]; -acadoVariables.u[19] += acadoWorkspace.x[23]; acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0]; acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1]; @@ -4669,22 +3048,6 @@ acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWo acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61]; acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62]; acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63]; -acadoVariables.x[68] += + acadoWorkspace.evGx[256]*acadoWorkspace.x[0] + acadoWorkspace.evGx[257]*acadoWorkspace.x[1] + acadoWorkspace.evGx[258]*acadoWorkspace.x[2] + acadoWorkspace.evGx[259]*acadoWorkspace.x[3] + acadoWorkspace.d[64]; -acadoVariables.x[69] += + acadoWorkspace.evGx[260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[263]*acadoWorkspace.x[3] + acadoWorkspace.d[65]; -acadoVariables.x[70] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.d[66]; -acadoVariables.x[71] += + acadoWorkspace.evGx[268]*acadoWorkspace.x[0] + acadoWorkspace.evGx[269]*acadoWorkspace.x[1] + acadoWorkspace.evGx[270]*acadoWorkspace.x[2] + acadoWorkspace.evGx[271]*acadoWorkspace.x[3] + acadoWorkspace.d[67]; -acadoVariables.x[72] += + acadoWorkspace.evGx[272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[275]*acadoWorkspace.x[3] + acadoWorkspace.d[68]; -acadoVariables.x[73] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.d[69]; -acadoVariables.x[74] += + acadoWorkspace.evGx[280]*acadoWorkspace.x[0] + acadoWorkspace.evGx[281]*acadoWorkspace.x[1] + acadoWorkspace.evGx[282]*acadoWorkspace.x[2] + acadoWorkspace.evGx[283]*acadoWorkspace.x[3] + acadoWorkspace.d[70]; -acadoVariables.x[75] += + acadoWorkspace.evGx[284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[287]*acadoWorkspace.x[3] + acadoWorkspace.d[71]; -acadoVariables.x[76] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.d[72]; -acadoVariables.x[77] += + acadoWorkspace.evGx[292]*acadoWorkspace.x[0] + acadoWorkspace.evGx[293]*acadoWorkspace.x[1] + acadoWorkspace.evGx[294]*acadoWorkspace.x[2] + acadoWorkspace.evGx[295]*acadoWorkspace.x[3] + acadoWorkspace.d[73]; -acadoVariables.x[78] += + acadoWorkspace.evGx[296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[299]*acadoWorkspace.x[3] + acadoWorkspace.d[74]; -acadoVariables.x[79] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.d[75]; -acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWorkspace.evGx[305]*acadoWorkspace.x[1] + acadoWorkspace.evGx[306]*acadoWorkspace.x[2] + acadoWorkspace.evGx[307]*acadoWorkspace.x[3] + acadoWorkspace.d[76]; -acadoVariables.x[81] += + acadoWorkspace.evGx[308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[311]*acadoWorkspace.x[3] + acadoWorkspace.d[77]; -acadoVariables.x[82] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.d[78]; -acadoVariables.x[83] += + acadoWorkspace.evGx[316]*acadoWorkspace.x[0] + acadoWorkspace.evGx[317]*acadoWorkspace.x[1] + acadoWorkspace.evGx[318]*acadoWorkspace.x[2] + acadoWorkspace.evGx[319]*acadoWorkspace.x[3] + acadoWorkspace.d[79]; acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 4 ]) ); acado_multEDu( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 8 ]) ); @@ -4822,80 +3185,6 @@ acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 64 ]) ); acado_multEDu( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 64 ]) ); acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 64 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 544 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 548 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 556 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 560 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 568 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 572 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 580 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 584 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 592 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 596 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 604 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 608 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 68 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 616 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 620 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 628 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 632 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 640 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 644 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 652 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 656 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 664 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 668 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 676 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 680 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 72 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 688 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 692 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 700 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 704 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 712 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 716 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 724 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 728 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 736 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 740 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 748 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 752 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 76 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 760 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 764 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 772 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 776 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 784 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 788 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 796 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 800 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 808 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 812 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 820 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 824 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 832 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 80 ]) ); -acado_multEDu( &(acadoWorkspace.E[ 836 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 80 ]) ); } int acado_preparationStep( ) @@ -4936,30 +3225,15 @@ return ret; void acado_initializeNodesByForwardSimulation( ) { int index; -for (index = 0; index < 20; ++index) +for (index = 0; index < 16; ++index) { acadoWorkspace.state[0] = acadoVariables.x[index * 4]; acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2]; acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3]; acadoWorkspace.state[24] = acadoVariables.u[index]; -acadoWorkspace.state[25] = acadoVariables.od[index * 17]; -acadoWorkspace.state[26] = acadoVariables.od[index * 17 + 1]; -acadoWorkspace.state[27] = acadoVariables.od[index * 17 + 2]; -acadoWorkspace.state[28] = acadoVariables.od[index * 17 + 3]; -acadoWorkspace.state[29] = acadoVariables.od[index * 17 + 4]; -acadoWorkspace.state[30] = acadoVariables.od[index * 17 + 5]; -acadoWorkspace.state[31] = acadoVariables.od[index * 17 + 6]; -acadoWorkspace.state[32] = acadoVariables.od[index * 17 + 7]; -acadoWorkspace.state[33] = acadoVariables.od[index * 17 + 8]; -acadoWorkspace.state[34] = acadoVariables.od[index * 17 + 9]; -acadoWorkspace.state[35] = acadoVariables.od[index * 17 + 10]; -acadoWorkspace.state[36] = acadoVariables.od[index * 17 + 11]; -acadoWorkspace.state[37] = acadoVariables.od[index * 17 + 12]; -acadoWorkspace.state[38] = acadoVariables.od[index * 17 + 13]; -acadoWorkspace.state[39] = acadoVariables.od[index * 17 + 14]; -acadoWorkspace.state[40] = acadoVariables.od[index * 17 + 15]; -acadoWorkspace.state[41] = acadoVariables.od[index * 17 + 16]; +acadoWorkspace.state[25] = acadoVariables.od[index * 2]; +acadoWorkspace.state[26] = acadoVariables.od[index * 2 + 1]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -4973,7 +3247,7 @@ acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3]; void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) { int index; -for (index = 0; index < 20; ++index) +for (index = 0; index < 16; ++index) { acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; @@ -4983,63 +3257,48 @@ acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7]; if (strategy == 1 && xEnd != 0) { -acadoVariables.x[80] = xEnd[0]; -acadoVariables.x[81] = xEnd[1]; -acadoVariables.x[82] = xEnd[2]; -acadoVariables.x[83] = xEnd[3]; +acadoVariables.x[64] = xEnd[0]; +acadoVariables.x[65] = xEnd[1]; +acadoVariables.x[66] = xEnd[2]; +acadoVariables.x[67] = xEnd[3]; } else if (strategy == 2) { -acadoWorkspace.state[0] = acadoVariables.x[80]; -acadoWorkspace.state[1] = acadoVariables.x[81]; -acadoWorkspace.state[2] = acadoVariables.x[82]; -acadoWorkspace.state[3] = acadoVariables.x[83]; +acadoWorkspace.state[0] = acadoVariables.x[64]; +acadoWorkspace.state[1] = acadoVariables.x[65]; +acadoWorkspace.state[2] = acadoVariables.x[66]; +acadoWorkspace.state[3] = acadoVariables.x[67]; if (uEnd != 0) { acadoWorkspace.state[24] = uEnd[0]; } else { -acadoWorkspace.state[24] = acadoVariables.u[19]; +acadoWorkspace.state[24] = acadoVariables.u[15]; } -acadoWorkspace.state[25] = acadoVariables.od[340]; -acadoWorkspace.state[26] = acadoVariables.od[341]; -acadoWorkspace.state[27] = acadoVariables.od[342]; -acadoWorkspace.state[28] = acadoVariables.od[343]; -acadoWorkspace.state[29] = acadoVariables.od[344]; -acadoWorkspace.state[30] = acadoVariables.od[345]; -acadoWorkspace.state[31] = acadoVariables.od[346]; -acadoWorkspace.state[32] = acadoVariables.od[347]; -acadoWorkspace.state[33] = acadoVariables.od[348]; -acadoWorkspace.state[34] = acadoVariables.od[349]; -acadoWorkspace.state[35] = acadoVariables.od[350]; -acadoWorkspace.state[36] = acadoVariables.od[351]; -acadoWorkspace.state[37] = acadoVariables.od[352]; -acadoWorkspace.state[38] = acadoVariables.od[353]; -acadoWorkspace.state[39] = acadoVariables.od[354]; -acadoWorkspace.state[40] = acadoVariables.od[355]; -acadoWorkspace.state[41] = acadoVariables.od[356]; +acadoWorkspace.state[25] = acadoVariables.od[32]; +acadoWorkspace.state[26] = acadoVariables.od[33]; -acado_integrate(acadoWorkspace.state, 1, 19); +acado_integrate(acadoWorkspace.state, 1, 15); -acadoVariables.x[80] = acadoWorkspace.state[0]; -acadoVariables.x[81] = acadoWorkspace.state[1]; -acadoVariables.x[82] = acadoWorkspace.state[2]; -acadoVariables.x[83] = acadoWorkspace.state[3]; +acadoVariables.x[64] = acadoWorkspace.state[0]; +acadoVariables.x[65] = acadoWorkspace.state[1]; +acadoVariables.x[66] = acadoWorkspace.state[2]; +acadoVariables.x[67] = acadoWorkspace.state[3]; } } void acado_shiftControls( real_t* const uEnd ) { int index; -for (index = 0; index < 19; ++index) +for (index = 0; index < 15; ++index) { acadoVariables.u[index] = acadoVariables.u[index + 1]; } if (uEnd != 0) { -acadoVariables.u[19] = uEnd[0]; +acadoVariables.u[15] = uEnd[0]; } } @@ -5050,9 +3309,9 @@ real_t kkt; int index; real_t prd; -kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23]; +kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19]; kkt = fabs( kkt ); -for (index = 0; index < 24; ++index) +for (index = 0; index < 20; ++index) { prd = acadoWorkspace.y[index]; if (prd > 1e-12) @@ -5060,9 +3319,9 @@ kkt += fabs(acadoWorkspace.lb[index] * prd); else if (prd < -1e-12) kkt += fabs(acadoWorkspace.ub[index] * prd); } -for (index = 0; index < 40; ++index) +for (index = 0; index < 32; ++index) { -prd = acadoWorkspace.y[index + 24]; +prd = acadoWorkspace.y[index + 20]; if (prd > 1e-12) kkt += fabs(acadoWorkspace.lbA[index] * prd); else if (prd < -1e-12) @@ -5076,86 +3335,48 @@ real_t acado_getObjective( ) real_t objVal; int lRun1; -/** Row vector of size: 5 */ -real_t tmpDy[ 5 ]; +/** Row vector of size: 3 */ +real_t tmpDy[ 3 ]; -/** Row vector of size: 4 */ -real_t tmpDyN[ 4 ]; +/** Row vector of size: 2 */ +real_t tmpDyN[ 2 ]; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2]; acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 17]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 17 + 1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 17 + 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 17 + 3]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 17 + 4]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 17 + 5]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 17 + 6]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 17 + 7]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 17 + 8]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 17 + 9]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 17 + 10]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 17 + 11]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 17 + 12]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 17 + 13]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 17 + 14]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 17 + 15]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 17 + 16]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 2 + 1]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); -acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5]; -acadoWorkspace.Dy[lRun1 * 5 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 5 + 1]; -acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 5 + 2]; -acadoWorkspace.Dy[lRun1 * 5 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 5 + 3]; -acadoWorkspace.Dy[lRun1 * 5 + 4] = acadoWorkspace.objValueOut[4] - acadoVariables.y[lRun1 * 5 + 4]; +acadoWorkspace.Dy[lRun1 * 3] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 3]; +acadoWorkspace.Dy[lRun1 * 3 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 3 + 1]; +acadoWorkspace.Dy[lRun1 * 3 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 3 + 2]; } -acadoWorkspace.objValueIn[0] = acadoVariables.x[80]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[81]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[82]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[83]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[340]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[341]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[342]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[343]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[344]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[345]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[346]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[347]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[348]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[349]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[350]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[351]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[352]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[353]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[354]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[355]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[356]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[64]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[65]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[66]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[67]; +acadoWorkspace.objValueIn[4] = acadoVariables.od[32]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[33]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; -acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; -acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3] - acadoVariables.yN[3]; objVal = 0.0000000000000000e+00; -for (lRun1 = 0; lRun1 < 20; ++lRun1) +for (lRun1 = 0; lRun1 < 16; ++lRun1) { -tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 5] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 10] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 15] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 20]; -tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 1] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 6] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 11] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 16] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 21]; -tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 2] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 7] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 12] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 17] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 22]; -tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 3] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 8] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 13] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 18] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 23]; -tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 4] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 9] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 14] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 19] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 24]; -objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4]; +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 3] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 6]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 1] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 4] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 7]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 2] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 5] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 8]; +objVal += + acadoWorkspace.Dy[lRun1 * 3]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 3 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 3 + 2]*tmpDy[2]; } -tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[8] + acadoWorkspace.DyN[3]*acadoVariables.WN[12]; -tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[9] + acadoWorkspace.DyN[3]*acadoVariables.WN[13]; -tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[6] + acadoWorkspace.DyN[2]*acadoVariables.WN[10] + acadoWorkspace.DyN[3]*acadoVariables.WN[14]; -tmpDyN[3] = + acadoWorkspace.DyN[0]*acadoVariables.WN[3] + acadoWorkspace.DyN[1]*acadoVariables.WN[7] + acadoWorkspace.DyN[2]*acadoVariables.WN[11] + acadoWorkspace.DyN[3]*acadoVariables.WN[15]; -objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2] + acadoWorkspace.DyN[3]*tmpDyN[3]; +tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[2]; +tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[3]; +objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1]; objVal *= 0.5; return objVal; diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 9fc3a6c40..7ef94fc59 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -9,23 +9,24 @@ libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix()) ffi = FFI() ffi.cdef(""" typedef struct { - double x, y, psi, delta, t; + double x, y, psi, curvature, curvature_rate; } state_t; +int N = 16; typedef struct { - double x[21]; - double y[21]; - double psi[21]; - double delta[21]; - double rate[20]; + double x[N+1]; + double y[N+1]; + double psi[N+1]; + double curvature[N+1]; + double curvature_rate[N]; double cost; } log_t; -void init(double pathCost, double laneCost, double headingCost, double steerRateCost); -void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost); +void init(double pathCost, double headingCost, double steerRateCost); +void init_weights(double pathCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, - double l_poly[4], double r_poly[4], double d_poly[4], - double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width); + double v_ego, double rotation_radius, + double target_y[N+1], double target_psi[N+1]); """) libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py new file mode 100644 index 000000000..724f93ff1 --- /dev/null +++ b/selfdrive/controls/lib/lateral_planner.py @@ -0,0 +1,254 @@ +import os +import math +import numpy as np +from common.realtime import sec_since_boot, DT_MDL +from common.numpy_fast import interp +from selfdrive.swaglog import cloudlog +from selfdrive.controls.lib.lateral_mpc import libmpc_py +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS +from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE +from selfdrive.config import Conversions as CV +from common.params import Params +import cereal.messaging as messaging +from cereal import log + +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection + +LOG_MPC = os.environ.get('LOG_MPC', False) + +LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10. + +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, + }, +} + + +class LateralPlanner(): + def __init__(self, CP): + self.LP = LanePlanner() + + self.last_cloudlog_t = 0 + self.steer_rate_cost = CP.steerRateCost + + self.setup_mpc() + self.solution_invalid_cnt = 0 + self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 + self.prev_one_blinker = False + self.desire = log.LateralPlan.Desire.none + + self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) + self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) + self.t_idxs = np.arange(TRAJECTORY_SIZE) + self.y_pts = np.zeros(TRAJECTORY_SIZE) + + def setup_mpc(self): + self.libmpc = libmpc_py.libmpc + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) + + self.mpc_solution = libmpc_py.ffi.new("log_t *") + self.cur_state = libmpc_py.ffi.new("state_t *") + self.cur_state[0].x = 0.0 + self.cur_state[0].y = 0.0 + self.cur_state[0].psi = 0.0 + self.cur_state[0].curvature = 0.0 + + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_time = 0.0 + + def update(self, sm, CP, VM): + v_ego = sm['carState'].vEgo + active = sm['controlsState'].active + steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffsetDeg + steering_wheel_angle_deg = sm['carState'].steeringAngleDeg + + # Update vehicle model + x = max(sm['liveParameters'].stiffnessFactor, 0.1) + sr = max(sm['liveParameters'].steerRatio, 0.1) + VM.update_params(x, sr) + curvature_factor = VM.curvature_factor(v_ego) + measured_curvature = -curvature_factor * math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR + + + md = sm['modelV2'] + self.LP.parse_model(sm['modelV2']) + if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: + self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) + self.t_idxs = np.array(md.position.t) + self.plan_yaw = list(md.orientation.z) + + # Lane change logic + one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + if sm['carState'].leftBlinker: + self.lane_change_direction = LaneChangeDirection.left + elif sm['carState'].rightBlinker: + self.lane_change_direction = LaneChangeDirection.right + + if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + else: + torque_applied = sm['carState'].steeringPressed and \ + ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or + (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + + blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + + # State transitions + # off + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + + # pre + elif self.lane_change_state == LaneChangeState.preLaneChange: + if not one_blinker or below_lane_change_speed: + self.lane_change_state = LaneChangeState.off + elif torque_applied and not blindspot_detected: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # starting + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + # fade out over .5s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) + # 98% certainty + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # finishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing: + # fade in laneline over 1s + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + if one_blinker and self.lane_change_ll_prob > 0.99: + self.lane_change_state = LaneChangeState.preLaneChange + elif self.lane_change_ll_prob > 0.99: + self.lane_change_state = LaneChangeState.off + + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + + # Turn off lanes during lane change + if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: + self.LP.lll_prob *= self.lane_change_ll_prob + self.LP.rll_prob *= self.lane_change_ll_prob + d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) + y_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) + heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) + self.y_pts = y_pts + + assert len(y_pts) == MPC_N + 1 + assert len(heading_pts) == MPC_N + 1 + self.libmpc.run_mpc(self.cur_state, self.mpc_solution, + float(v_ego), + CAR_ROTATION_RADIUS, + list(y_pts), + list(heading_pts)) + # init state for next + self.cur_state.x = 0.0 + self.cur_state.y = 0.0 + self.cur_state.psi = 0.0 + self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature) + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + current_curvature = self.mpc_solution.curvature[0] + psi = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.psi) + next_curvature_rate = self.mpc_solution.curvature_rate[0] + + # MPC can plan to turn the wheel and turn back before t_delay. This means + # in high delay cases some corrections never even get commanded. So just use + # psi to calculate a simple linearization of desired curvature + curvature_diff_from_psi = psi/(max(v_ego, 1e-1) * delay) - current_curvature + next_curvature = current_curvature + 2*curvature_diff_from_psi + + # reset to current steer angle if not active or overriding + if active: + curvature_desired = next_curvature + desired_curvature_rate = next_curvature_rate + else: + curvature_desired = measured_curvature + desired_curvature_rate = 0.0 + + # negative sign, controls uses different convention + self.desired_steering_wheel_angle_deg = -float(math.degrees(curvature_desired * VM.sR)/curvature_factor) + steering_wheel_angle_offset_deg + self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_curvature_rate * VM.sR)/curvature_factor) + + # Check for infeasable MPC solution + mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) + t = sec_since_boot() + if mpc_nans: + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.cur_state.curvature = measured_curvature + + if t > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = t + cloudlog.warning("Lateral mpc - nan: True") + + if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge + self.solution_invalid_cnt += 1 + else: + self.solution_invalid_cnt = 0 + + def publish(self, sm, pm): + plan_solution_valid = self.solution_invalid_cnt < 2 + plan_send = messaging.new_message('lateralPlan') + plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) + plan_send.lateralPlan.laneWidth = float(self.LP.lane_width) + plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts] + plan_send.lateralPlan.lProb = float(self.LP.lll_prob) + plan_send.lateralPlan.rProb = float(self.LP.rll_prob) + plan_send.lateralPlan.dProb = float(self.LP.d_prob) + + plan_send.lateralPlan.steeringAngleDeg = float(self.desired_steering_wheel_angle_deg) + plan_send.lateralPlan.steeringRateDeg = float(self.desired_steering_wheel_angle_rate_deg) + plan_send.lateralPlan.angleOffsetDeg = float(sm['liveParameters'].angleOffsetAverageDeg) + plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid) + + plan_send.lateralPlan.desire = self.desire + plan_send.lateralPlan.laneChangeState = self.lane_change_state + plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction + + pm.send('lateralPlan', plan_send) + + if LOG_MPC: + dat = messaging.new_message('liveMpc') + dat.liveMpc.x = list(self.mpc_solution[0].x) + dat.liveMpc.y = list(self.mpc_solution[0].y) + dat.liveMpc.psi = list(self.mpc_solution[0].psi) + dat.liveMpc.tire_angle = list(self.mpc_solution[0].tire_angle) + dat.liveMpc.cost = self.mpc_solution[0].cost + pm.send('liveMpc', dat) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f9bf15cc0..0e51d521d 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -25,21 +25,24 @@ class LongitudinalMpc(): self.new_lead = False self.last_cloudlog_t = 0.0 + self.n_its = 0 + self.duration = 0 - def send_mpc_solution(self, pm, qp_iterations, calculation_time): - qp_iterations = max(0, qp_iterations) - dat = messaging.new_message('liveLongitudinalMpc') - dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) - dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) - dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) - dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) - dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) - dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost - dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau - dat.liveLongitudinalMpc.qpIterations = qp_iterations - dat.liveLongitudinalMpc.mpcId = self.mpc_id - dat.liveLongitudinalMpc.calculationTime = calculation_time - pm.send('liveLongitudinalMpc', dat) + def publish(self, pm): + if LOG_MPC: + qp_iterations = max(0, self.n_its) + dat = messaging.new_message('liveLongitudinalMpc') + dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) + dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) + dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) + dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l) + dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l) + dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost + dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau + dat.liveLongitudinalMpc.qpIterations = qp_iterations + dat.liveLongitudinalMpc.mpcId = self.mpc_id + dat.liveLongitudinalMpc.calculationTime = self.duration + pm.send('liveLongitudinalMpc', dat) def setup_mpc(self): ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) @@ -56,7 +59,7 @@ class LongitudinalMpc(): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a - def update(self, pm, CS, lead): + def update(self, CS, lead): v_ego = CS.vEgo # Setup current mpc state @@ -91,11 +94,8 @@ class LongitudinalMpc(): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) - duration = int((sec_since_boot() - t) * 1e9) - - if LOG_MPC: - self.send_mpc_solution(pm, n_its, duration) + self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + self.duration = int((sec_since_boot() - t) * 1e9) # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed self.v_mpc = self.mpc_solution[0].v_ego[1] diff --git a/selfdrive/controls/lib/longitudinal_mpc/SConscript b/selfdrive/controls/lib/longitudinal_mpc/SConscript index 072ff646a..584d5103e 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc/SConscript @@ -8,25 +8,41 @@ cpp_path = [ "#phonelibs/qpoases/INCLUDE/EXTRAS", "#phonelibs/qpoases/SRC/", "#phonelibs/qpoases", - "lib_mpc_export" - + "lib_mpc_export", ] -mpc_files = [ - "longitudinal_mpc.c", - Glob("lib_mpc_export/*.c"), - Glob("lib_mpc_export/*.cpp"), +generated_c = [ + 'lib_mpc_export/acado_auxiliary_functions.c', + 'lib_mpc_export/acado_qpoases_interface.cpp', + 'lib_mpc_export/acado_integrator.c', + 'lib_mpc_export/acado_solver.c', ] +generated_h = [ + 'lib_mpc_export/acado_common.h', + 'lib_mpc_export/acado_auxiliary_functions.h', + 'lib_mpc_export/acado_qpoases_interface.hpp', +] + + interface_dir = Dir('lib_mpc_export') SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) +if GetOption('mpc_generate'): + generator_cpp = File('generator.cpp') + + acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"), + File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")] + + generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path, + CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"]) + + cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}" + env.Command(generated_c + generated_h, generator, cmd) + + +mpc_files = ["longitudinal_mpc.c"] + generated_c env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) - -# if arch != "aarch64": -# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), -# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), -# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] -# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/longitudinal_planner.py similarity index 76% rename from selfdrive/controls/lib/planner.py rename to selfdrive/controls/lib/longitudinal_planner.py index 2567b4606..d7c7aff3a 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -5,7 +5,6 @@ from common.params import Params from common.numpy_fast import interp import cereal.messaging as messaging -from cereal import car from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV @@ -66,6 +65,8 @@ class Planner(): self.v_acc_start = 0.0 self.a_acc_start = 0.0 + self.v_acc_next = 0.0 + self.a_acc_next = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 @@ -77,6 +78,8 @@ class Planner(): self.fcw_checker = FCWChecker() self.path_x = np.arange(192) + self.fcw = False + self.params = Params() self.first_loop = True @@ -104,7 +107,7 @@ class Planner(): self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) - def update(self, sm, pm, CP, VM, PP): + def update(self, sm, CP, VM, PP): """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo @@ -122,11 +125,14 @@ class Planner(): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 + self.v_acc_start = self.v_acc_next + self.a_acc_start = self.a_acc_next + # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: 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_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration @@ -156,8 +162,8 @@ class Planner(): self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) - self.mpc1.update(pm, sm['carState'], lead_1) - self.mpc2.update(pm, sm['carState'], lead_2) + self.mpc1.update(sm['carState'], lead_1) + self.mpc2.update(sm['carState'], lead_2) self.choose_solution(v_cruise_setpoint, enabled) @@ -166,55 +172,46 @@ class Planner(): self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker - fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, - sm['controlsState'].active, - v_ego, sm['carState'].aEgo, - lead_1.dRel, lead_1.vLead, lead_1.aLeadK, - lead_1.yRel, lead_1.vLat, - lead_1.fcw, blinkers) and not sm['carState'].brakePressed - if fcw: + self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, + sm['controlsState'].active, + v_ego, sm['carState'].aEgo, + lead_1.dRel, lead_1.vLead, lead_1.aLeadK, + lead_1.yRel, lead_1.vLat, + lead_1.fcw, blinkers) and not sm['carState'].brakePressed + if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) - radar_dead = not sm.alive['radarState'] - - radar_errors = list(sm['radarState'].radarErrors) - radar_fault = car.RadarData.Error.fault in radar_errors - radar_can_error = car.RadarData.Error.canError in radar_errors - - # **** send the plan **** - plan_send = messaging.new_message('plan') - - plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) - - plan_send.plan.mdMonoTime = sm.logMonoTime['model'] - plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] - - # longitudal plan - plan_send.plan.vCruise = float(self.v_cruise) - plan_send.plan.aCruise = float(self.a_cruise) - plan_send.plan.vStart = float(self.v_acc_start) - plan_send.plan.aStart = float(self.a_acc_start) - plan_send.plan.vTarget = float(self.v_acc) - plan_send.plan.aTarget = float(self.a_acc) - plan_send.plan.vTargetFuture = float(self.v_acc_future) - plan_send.plan.hasLead = self.mpc1.prev_lead_status - plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource - - radar_valid = not (radar_dead or radar_fault) - plan_send.plan.radarValid = bool(radar_valid) - plan_send.plan.radarCanError = bool(radar_can_error) - - plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] - - # Send out fcw - plan_send.plan.fcw = fcw - - pm.send('plan', plan_send) - # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0 - self.v_acc_start = v_acc_sol - self.a_acc_start = a_acc_sol + self.v_acc_next = v_acc_sol + self.a_acc_next = a_acc_sol self.first_loop = False + + def publish(self, sm, pm): + self.mpc1.publish(pm) + self.mpc2.publish(pm) + + plan_send = messaging.new_message('longitudinalPlan') + + plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) + + longitudinalPlan = plan_send.longitudinalPlan + longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2'] + longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState'] + + longitudinalPlan.vCruise = float(self.v_cruise) + longitudinalPlan.aCruise = float(self.a_cruise) + longitudinalPlan.vStart = float(self.v_acc_start) + longitudinalPlan.aStart = float(self.a_acc_start) + longitudinalPlan.vTarget = float(self.v_acc) + longitudinalPlan.aTarget = float(self.a_acc) + longitudinalPlan.vTargetFuture = float(self.v_acc_future) + longitudinalPlan.hasLead = self.mpc1.prev_lead_status + longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource + longitudinalPlan.fcw = self.fcw + + longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState'] + + pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py deleted file mode 100644 index a58eae005..000000000 --- a/selfdrive/controls/lib/pathplanner.py +++ /dev/null @@ -1,233 +0,0 @@ -import os -import math -from common.realtime import sec_since_boot, DT_MDL -from selfdrive.swaglog import cloudlog -from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT -from selfdrive.controls.lib.lane_planner import LanePlanner -from selfdrive.config import Conversions as CV -from common.params import Params -import cereal.messaging as messaging -from cereal import log - -LaneChangeState = log.PathPlan.LaneChangeState -LaneChangeDirection = log.PathPlan.LaneChangeDirection - -LOG_MPC = os.environ.get('LOG_MPC', False) - -LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS -LANE_CHANGE_TIME_MAX = 10. - -DESIRES = { - LaneChangeDirection.none: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, - }, - LaneChangeDirection.left: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, - }, - LaneChangeDirection.right: { - LaneChangeState.off: log.PathPlan.Desire.none, - LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, - }, -} - - -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay - states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay - states[0].y = states[0].x * math.sin(states[0].psi / 2) - return states - - -class PathPlanner(): - def __init__(self, CP): - self.LP = LanePlanner() - - self.last_cloudlog_t = 0 - self.steer_rate_cost = CP.steerRateCost - - self.setup_mpc() - self.solution_invalid_cnt = 0 - self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - self.lane_change_timer = 0.0 - self.lane_change_ll_prob = 1.0 - self.prev_one_blinker = False - - def setup_mpc(self): - self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) - - self.mpc_solution = libmpc_py.ffi.new("log_t *") - self.cur_state = libmpc_py.ffi.new("state_t *") - self.cur_state[0].x = 0.0 - self.cur_state[0].y = 0.0 - self.cur_state[0].psi = 0.0 - self.cur_state[0].delta = 0.0 - - self.angle_steers_des = 0.0 - self.angle_steers_des_mpc = 0.0 - self.angle_steers_des_prev = 0.0 - self.angle_steers_des_time = 0.0 - - def update(self, sm, pm, CP, VM): - v_ego = sm['carState'].vEgo - angle_steers = sm['carState'].steeringAngle - active = sm['controlsState'].active - - angle_offset = sm['liveParameters'].angleOffset - - # Run MPC - self.angle_steers_des_prev = self.angle_steers_des_mpc - - # Update vehicle model - x = max(sm['liveParameters'].stiffnessFactor, 0.1) - sr = max(sm['liveParameters'].steerRatio, 0.1) - VM.update_params(x, sr) - - curvature_factor = VM.curvature_factor(v_ego) - - self.LP.parse_model(sm['model']) - - # Lane change logic - one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker - below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - - if sm['carState'].leftBlinker: - self.lane_change_direction = LaneChangeDirection.left - elif sm['carState'].rightBlinker: - self.lane_change_direction = LaneChangeDirection.right - - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not self.lane_change_enabled): - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - else: - torque_applied = sm['carState'].steeringPressed and \ - ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) - - blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob - - # State transitions - # off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: - self.lane_change_state = LaneChangeState.preLaneChange - self.lane_change_ll_prob = 1.0 - - # pre - elif self.lane_change_state == LaneChangeState.preLaneChange: - if not one_blinker or below_lane_change_speed: - self.lane_change_state = LaneChangeState.off - elif torque_applied and not blindspot_detected: - self.lane_change_state = LaneChangeState.laneChangeStarting - - # starting - elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out over .5s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0) - # 98% certainty - if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: - self.lane_change_state = LaneChangeState.laneChangeFinishing - - # finishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing: - # fade in laneline over 1s - self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) - if one_blinker and self.lane_change_ll_prob > 0.99: - self.lane_change_state = LaneChangeState.preLaneChange - elif self.lane_change_ll_prob > 0.99: - self.lane_change_state = LaneChangeState.off - - if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: - self.lane_change_timer = 0.0 - else: - self.lane_change_timer += DT_MDL - - self.prev_one_blinker = one_blinker - - desire = DESIRES[self.lane_change_direction][self.lane_change_state] - - # Turn off lanes during lane change - if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: - self.LP.l_prob *= self.lane_change_ll_prob - self.LP.r_prob *= self.lane_change_ll_prob - self.LP.update_d_poly(v_ego) - - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, 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, - list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), - self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) - - # reset to current steer angle if not active or overriding - if active: - delta_desired = self.mpc_solution[0].delta[1] - rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) - else: - delta_desired = math.radians(angle_steers - angle_offset) / VM.sR - rate_desired = 0.0 - - self.cur_state[0].delta = delta_desired - - self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) - - # Check for infeasable MPC solution - mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) - t = sec_since_boot() - if mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) - self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR - - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t - cloudlog.warning("Lateral mpc - nan: True") - - if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge - self.solution_invalid_cnt += 1 - else: - self.solution_invalid_cnt = 0 - plan_solution_valid = self.solution_invalid_cnt < 2 - - plan_send = messaging.new_message('pathPlan') - plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) - plan_send.pathPlan.laneWidth = float(self.LP.lane_width) - plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] - plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] - plan_send.pathPlan.lProb = float(self.LP.l_prob) - plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] - plan_send.pathPlan.rProb = float(self.LP.r_prob) - - plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) - plan_send.pathPlan.rateSteers = float(rate_desired) - plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) - plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) - plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) - - plan_send.pathPlan.desire = desire - plan_send.pathPlan.laneChangeState = self.lane_change_state - plan_send.pathPlan.laneChangeDirection = self.lane_change_direction - - pm.send('pathPlan', plan_send) - - if LOG_MPC: - dat = messaging.new_message('liveMpc') - dat.liveMpc.x = list(self.mpc_solution[0].x) - dat.liveMpc.y = list(self.mpc_solution[0].y) - dat.liveMpc.psi = list(self.mpc_solution[0].psi) - dat.liveMpc.delta = list(self.mpc_solution[0].delta) - dat.liveMpc.cost = self.mpc_solution[0].cost - pm.send('liveMpc', dat) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 3861bf574..132bba8bd 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -132,11 +132,11 @@ class Cluster(): def get_RadarState_from_vision(self, lead_msg, v_ego): return { - "dRel": float(lead_msg.dist - RADAR_TO_CAMERA), - "yRel": float(lead_msg.relY), - "vRel": float(lead_msg.relVel), - "vLead": float(v_ego + lead_msg.relVel), - "vLeadK": float(v_ego + lead_msg.relVel), + "dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.xyva[1]), + "vRel": float(lead_msg.xyva[2]), + "vLead": float(v_ego + lead_msg.xyva[2]), + "vLeadK": float(v_ego + lead_msg.xyva[2]), "aLeadK": float(0), "aLeadTau": _LEAD_ACCEL_TAU, "fcw": False, diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 1f19266f1..2d61a1b53 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -3,9 +3,9 @@ from cereal import car from common.params import Params from common.realtime import Priority, config_realtime_process from selfdrive.swaglog import cloudlog -from selfdrive.controls.lib.planner import Planner +from selfdrive.controls.lib.longitudinal_planner import Planner from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.controls.lib.pathplanner import PathPlanner +from selfdrive.controls.lib.lateral_planner import LateralPlanner import cereal.messaging as messaging @@ -18,16 +18,16 @@ def plannerd_thread(sm=None, pm=None): cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) - PP = PathPlanner(CP) + PP = LateralPlanner(CP) VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters'], - poll=['radarState', 'model']) + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2', 'liveParameters'], + poll=['radarState', 'modelV2']) if pm is None: - pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) + pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True @@ -37,10 +37,12 @@ def plannerd_thread(sm=None, pm=None): while True: sm.update() - if sm.updated['model']: - PP.update(sm, pm, CP, VM) + if sm.updated['modelV2']: + PP.update(sm, CP, VM) + PP.publish(sm, pm) if sm.updated['radarState']: - PL.update(sm, pm, CP, VM, PP) + PL.update(sm, CP, VM, PP) + PL.publish(sm, pm) def main(sm=None, pm=None): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6cb5f36a0..8f23d9d73 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -37,12 +37,12 @@ def laplacian_cdf(x, mu, b): def match_vision_to_cluster(v_ego, lead, clusters): # match vision point to best statistical cluster match - offset_vision_dist = lead.dist - RADAR_TO_CAMERA + offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) - prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) - prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0]) + prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1]) + prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -52,7 +52,7 @@ def match_vision_to_cluster(v_ego, lead, clusters): # if no 'sane' match is found return -1 # stationary radar points can be false positives dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 3) + vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3) if dist_sane and vel_sane: return cluster else: @@ -100,10 +100,10 @@ class RadarD(): def update(self, sm, rr, enable_lead): self.current_time = 1e-9*max(sm.logMonoTime.values()) - if sm.updated['controlsState']: - self.v_ego = sm['controlsState'].vEgo + if sm.updated['carState']: + self.v_ego = sm['carState'].vEgo self.v_ego_hist.append(self.v_ego) - if sm.updated['model']: + if sm.updated['modelV2']: self.ready = True ar_pts = {} @@ -157,16 +157,17 @@ class RadarD(): # *** publish radarState *** dat = messaging.new_message('radarState') - dat.valid = sm.all_alive_and_valid() + dat.valid = sm.all_alive_and_valid() and len(rr.errors) == 0 radarState = dat.radarState - radarState.mdMonoTime = sm.logMonoTime['model'] + radarState.mdMonoTime = sm.logMonoTime['modelV2'] radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) - radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] + radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) + if len(sm['modelV2'].leads) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False) return dat @@ -187,7 +188,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['model', 'controlsState']) + sm = messaging.SubMaster(['modelV2', 'carState']) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 76f14afa0..0bc3efdee 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -24,23 +24,19 @@ import argparse import re from collections import defaultdict +import selfdrive.manager as manager # Do statistics every 5 seconds PRINT_INTERVAL = 5 SLEEP_INTERVAL = 0.2 monitored_proc_names = [ - # openpilot procs - 'controlsd', 'locationd', 'loggerd','plannerd', - 'ubloxd', 'thermald', 'uploader', 'deleter', 'radard', 'logmessaged', 'tombstoned', - 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringd', - 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena', 'locationd', 'paramsd', - + # offroad APK 'ai.comma.plus.offroad', # android procs 'SurfaceFlinger', 'sensors.qcom' -] +] + manager.car_started_processes + manager.persistent_processes cpu_time_names = ['user', 'system', 'children_user', 'children_system'] @@ -48,9 +44,7 @@ timer = getattr(time, 'monotonic', time.time) def get_arg_parser(): - parser = argparse.ArgumentParser( - description="Unlogger and UI", - formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument("proc_names", nargs="?", default='', help="Process names to be monitored, comma seperated") diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 45bc8984c..9999d8657 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -16,11 +16,11 @@ def cycle_alerts(duration=200, is_metric=False): print(alerts) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") - sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration', - 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman']) + sm = messaging.SubMaster(['deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman']) controls_state = messaging.pub_sock('controlsState') - thermal = messaging.pub_sock('thermal') + deviceState = messaging.pub_sock('deviceState') idx, last_alert_millis = 0, 0 @@ -56,9 +56,9 @@ def cycle_alerts(duration=200, is_metric=False): controls_state.send(dat.to_bytes()) dat = messaging.new_message() - dat.init('thermal') - dat.thermal.started = True - thermal.send(dat.to_bytes()) + dat.init('deviceState') + dat.deviceState.started = True + deviceState.send(dat.to_bytes()) frame += 1 time.sleep(0.01) diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py index 162a6381b..4ca69f5e9 100755 --- a/selfdrive/debug/filter_log_message.py +++ b/selfdrive/debug/filter_log_message.py @@ -3,7 +3,8 @@ import argparse import json import cereal.messaging as messaging - +from tools.lib.logreader import LogReader +from tools.lib.route import Route LEVELS = { "DEBUG": 10, @@ -23,32 +24,55 @@ ANDROID_LOG_SOURCE = { } +def print_logmessage(t, msg, min_level): + try: + log = json.loads(msg) + if log['levelnum'] >= min_level: + print(f"[{t / 1e9:.6f}] {log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") + except json.decoder.JSONDecodeError: + print(f"[{t / 1e9:.6f}] decode error: {msg}") + + +def print_androidlog(t, msg): + source = ANDROID_LOG_SOURCE[msg.id] + print(f"[{t / 1e9:.6f}] {source} {msg.pid} {msg.tag} - {msg.message}") + + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument('--level', default='DEBUG') parser.add_argument('--addr', default='127.0.0.1') - parser.add_argument("socket", type=str, nargs='*', help="socket name") + parser.add_argument("route", type=str, nargs='*', help="route name + segment number for offline usage") args = parser.parse_args() - sm = messaging.SubMaster(['logMessage', 'androidLog'], addr=args.addr) + logs = None + if len(args.route): + r = Route(args.route[0]) + logs = r.log_paths() # TODO: switch to qlogs when logs are in there + + if len(args.route) == 2 and logs: + n = int(args.route[1]) + logs = [logs[n]] min_level = LEVELS[args.level] - while True: - sm.update() + if logs: + for log in logs: + if log: + lr = LogReader(log) + for m in lr: + if m.which() == 'logMessage': + print_logmessage(m.logMonoTime, m.logMessage, min_level) + elif m.which() == 'androidLog': + print_androidlog(m.logMonoTime, m.androidLog) + else: + sm = messaging.SubMaster(['logMessage', 'androidLog'], addr=args.addr) + while True: + sm.update() - if sm.updated['logMessage']: - t = sm.logMonoTime['logMessage'] - try: - log = json.loads(sm['logMessage']) - if log['levelnum'] >= min_level: - print(f"[{t / 1e9:.6f}] {log['filename']}:{log.get('lineno', '')} - {log.get('funcname', '')}: {log['msg']}") - except json.decoder.JSONDecodeError: - print(f"[{t / 1e9:.6f}] decode error: {sm['logMessage']}") + if sm.updated['logMessage']: + print_logmessage(sm.logMonoTime['logMessage'], sm['logMessage'], min_level) - if sm.updated['androidLog']: - t = sm.logMonoTime['androidLog'] - m = sm['androidLog'] - source = ANDROID_LOG_SOURCE[m.id] - print(f"[{t / 1e9:.6f}] {source} {m.pid} {m.tag} - {m.message}") + if sm.updated['androidLog']: + print_androidlog(sm.logMonoTime['androidLog'], sm['androidLog']) diff --git a/selfdrive/debug/fingerprint_from_route.py b/selfdrive/debug/fingerprint_from_route.py index a06dfe180..098e39fbe 100755 --- a/selfdrive/debug/fingerprint_from_route.py +++ b/selfdrive/debug/fingerprint_from_route.py @@ -37,7 +37,7 @@ def get_fingerprint(lr): if __name__ == "__main__": if len(sys.argv) < 2: - print("Usage: ./get_fingerprint_internal.py ") + print("Usage: ./fingerprint_from_route.py ") sys.exit(1) route = Route(sys.argv[1]) diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 8322e9108..de854f070 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -1,9 +1,8 @@ #!/usr/bin/env python3 import argparse -import numpy as np - from cereal.messaging import SubMaster +from common.numpy_fast import mean def cputime_total(ct): @@ -34,7 +33,7 @@ if __name__ == "__main__": parser.add_argument('--cpu', action='store_true') args = parser.parse_args() - sm = SubMaster(['thermal', 'procLog']) + sm = SubMaster(['deviceState', 'procLog']) last_temp = 0.0 last_mem = 0.0 @@ -47,10 +46,10 @@ if __name__ == "__main__": while True: sm.update() - if sm.updated['thermal']: - t = sm['thermal'] - last_temp = np.mean(t.cpu) - last_mem = t.memUsedPercent + if sm.updated['deviceState']: + t = sm['deviceState'] + last_temp = mean(t.cpuTempC) + last_mem = t.memoryUsagePercent if sm.updated['procLog']: m = sm['procLog'] @@ -72,7 +71,7 @@ if __name__ == "__main__": total_times = total_times_new[:] busy_times = busy_times_new[:] - print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * np.mean(cores), last_mem, last_temp)) + print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * mean(cores), last_mem, last_temp)) if args.cpu and prev_proclog is not None: procs = {} diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 90346d524..1c7f0077e 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -46,8 +46,8 @@ if __name__ == "__main__": lr = LogReader(qlog_path) for msg in lr: - if msg.which() == "health": - if msg.health.hwType not in ['uno', 'blackPanda']: + if msg.which() == "pandaState": + if msg.pandaState.pandaType not in ['uno', 'blackPanda']: dongles.append(dongle_id) break diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index e31d6cf3c..7b619805c 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -3,20 +3,20 @@ import time import cereal.messaging as messaging from selfdrive.manager import start_managed_process, kill_managed_process -services = ['controlsState', 'thermal', 'radarState'] # the services needed to be spoofed to start ui offroad +services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad procs = ['camerad', 'ui', 'modeld', 'calibrationd'] [start_managed_process(p) for p in procs] # start needed processes pm = messaging.PubMaster(services) -dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services] +dat_cs, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services] dat_cs.controlsState.rearViewCam = False # ui checks for these two messages -dat_thermal.thermal.started = True +dat_deviceState.deviceState.started = True try: while True: pm.send('controlsState', dat_cs) - pm.send('thermal', dat_thermal) + pm.send('deviceState', dat_deviceState) pm.send('radarState', dat_radar) - time.sleep(1 / 100) # continually send, rate doesn't matter for thermal + time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState except KeyboardInterrupt: [kill_managed_process(p) for p in procs] diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py index 24163a128..f5274deb6 100644 --- a/selfdrive/hardware/base.py +++ b/selfdrive/hardware/base.py @@ -1,5 +1,7 @@ from abc import abstractmethod +from collections import namedtuple +ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient']) class HardwareBase: @staticmethod @@ -24,6 +26,14 @@ class HardwareBase: def uninstall(self): pass + @abstractmethod + def get_os_version(self): + pass + + @abstractmethod + def get_device_type(self): + pass + @abstractmethod def get_sound_card_online(self): pass @@ -83,3 +93,11 @@ class HardwareBase: @abstractmethod def get_current_power_draw(self): pass + + @abstractmethod + def shutdown(self): + pass + + @abstractmethod + def get_thermal_config(self): + pass diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py index 231b5ba6b..4666c2a63 100644 --- a/selfdrive/hardware/eon/hardware.py +++ b/selfdrive/hardware/eon/hardware.py @@ -6,10 +6,10 @@ import struct import subprocess from cereal import log -from selfdrive.hardware.base import HardwareBase +from selfdrive.hardware.base import HardwareBase, ThermalConfig -NetworkType = log.ThermalData.NetworkType -NetworkStrength = log.ThermalData.NetworkStrength +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength def service_call(call): @@ -61,6 +61,13 @@ def getprop(key): class Android(HardwareBase): + def get_os_version(self): + with open("/VERSION") as f: + return f.read().strip() + + def get_device_type(self): + return "eon" + def get_sound_card_online(self): return (os.path.isfile('/proc/asound/card0/state') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') @@ -335,3 +342,9 @@ class Android(HardwareBase): def get_current_power_draw(self): # We don't have a good direct way to measure this on android return None + + def shutdown(self): + os.system('LD_LIBRARY_PATH="" svc power shutdown') + + def get_thermal_config(self): + return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1)) diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py index 181de1103..1744b6950 100644 --- a/selfdrive/hardware/pc/hardware.py +++ b/selfdrive/hardware/pc/hardware.py @@ -1,13 +1,19 @@ import random from cereal import log -from selfdrive.hardware.base import HardwareBase +from selfdrive.hardware.base import HardwareBase, ThermalConfig -NetworkType = log.ThermalData.NetworkType -NetworkStrength = log.ThermalData.NetworkStrength +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength class Pc(HardwareBase): + def get_os_version(self): + return None + + def get_device_type(self): + return "pc" + def get_sound_card_online(self): return True @@ -64,3 +70,9 @@ class Pc(HardwareBase): def get_current_power_draw(self): return 0 + + def shutdown(self): + print("SHUTDOWN!") + + def get_thermal_config(self): + return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1)) diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json new file mode 100644 index 000000000..467bad92f --- /dev/null +++ b/selfdrive/hardware/tici/agnos.json @@ -0,0 +1,18 @@ +[ + { + "name": "system", + "url": "https://commadist.azureedge.net/agnosupdate-staging/system-09c324e1fc98905efffc356592235a5ea2955922b04d16f4406d0a96122b69b2.img.xz", + "hash": "8f7ed837a155abbdb3bc9490b65c8a3c67553a0aa32f429aa57d54134c1fe886", + "hash_raw": "09c324e1fc98905efffc356592235a5ea2955922b04d16f4406d0a96122b69b2", + "size": 10737418240, + "sparse": true + }, + { + "name": "boot", + "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038.img.xz", + "hash": "d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038", + "hash_raw": "d292aacf2f760e85c60c1a25adcecae9df2c80ee3f3240ea8592bbac369db038", + "size": 14768128, + "sparse": false + } +] diff --git a/selfdrive/hardware/tici/agnos.py b/selfdrive/hardware/tici/agnos.py new file mode 100755 index 000000000..b85fc4ad2 --- /dev/null +++ b/selfdrive/hardware/tici/agnos.py @@ -0,0 +1,176 @@ +#!/usr/bin/env python3 +import json +import lzma +import hashlib +import requests +import struct +import subprocess +import os + +from common.spinner import Spinner + + +class StreamingDecompressor: + def __init__(self, url): + self.buf = b"" + + self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}) + self.it = self.req.iter_content(chunk_size=1024 * 1024) + self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO) + self.eof = False + self.sha256 = hashlib.sha256() + + def read(self, length): + while len(self.buf) < length: + self.req.raise_for_status() + + try: + compressed = next(self.it) + except StopIteration: + self.eof = True + break + out = self.decompressor.decompress(compressed) + self.buf += out + + result = self.buf[:length] + self.buf = self.buf[length:] + + self.sha256.update(result) + return result + + +def unsparsify(f): + magic = struct.unpack("I", f.read(4))[0] + assert(magic == 0xed26ff3a) + + # Version + major = struct.unpack("H", f.read(2))[0] + minor = struct.unpack("H", f.read(2))[0] + assert(major == 1 and minor == 0) + + # Header sizes + _ = struct.unpack("H", f.read(2))[0] + _ = struct.unpack("H", f.read(2))[0] + + block_sz = struct.unpack("I", f.read(4))[0] + _ = struct.unpack("I", f.read(4))[0] + num_chunks = struct.unpack("I", f.read(4))[0] + _ = struct.unpack("I", f.read(4))[0] + + for _ in range(num_chunks): + chunk_type = struct.unpack("H", f.read(2))[0] + _ = struct.unpack("H", f.read(2))[0] + out_blocks = struct.unpack("I", f.read(4))[0] + _ = struct.unpack("I", f.read(4))[0] + + if chunk_type == 0xcac1: # Raw + # TODO: yield in smaller chunks. Yielding only block_sz is too slow. Largest observed data chunk is 252 MB. + yield f.read(out_blocks * block_sz) + elif chunk_type == 0xcac2: # Fill + filler = f.read(4) * (block_sz // 4) + for _ in range(out_blocks): + yield filler + elif chunk_type == 0xcac3: # Don't care + yield b"" + else: + raise Exception("Unhandled sparse chunk type") + + +def flash_partition(cloudlog, spinner, target_slot, partition): + cloudlog.info(f"Downloading and writing {partition['name']}") + + downloader = StreamingDecompressor(partition['url']) + with open(f"/dev/disk/by-partlabel/{partition['name']}{target_slot}", 'wb+') as out: + partition_size = partition['size'] + + # Check if partition is already flashed + out.seek(partition_size) + if out.read(64) == partition['hash_raw'].lower().encode(): + cloudlog.info(f"Already flashed {partition['name']}") + return + + # Clear hash before flashing + out.seek(partition_size) + out.write(b"\x00" * 64) + out.seek(0) + os.sync() + + # Flash partition + if partition['sparse']: + raw_hash = hashlib.sha256() + for chunk in unsparsify(downloader): + raw_hash.update(chunk) + out.write(chunk) + + if spinner is not None: + spinner.update_progress(out.tell(), partition_size) + + if raw_hash.hexdigest().lower() != partition['hash_raw'].lower(): + raise Exception(f"Unsparse hash mismatch '{raw_hash.hexdigest().lower()}'") + else: + while not downloader.eof: + out.write(downloader.read(1024 * 1024)) + + if spinner is not None: + spinner.update_progress(out.tell(), partition_size) + + if downloader.sha256.hexdigest().lower() != partition['hash'].lower(): + raise Exception("Uncompressed hash mismatch") + + if out.tell() != partition['size']: + raise Exception("Uncompressed size mismatch") + + # Write hash after successfull flash + os.sync() + out.write(partition['hash_raw'].lower().encode()) + + +def flash_agnos_update(manifest_path, cloudlog, spinner=None): + update = json.load(open(manifest_path)) + + current_slot = subprocess.check_output(["abctl", "--boot_slot"], encoding='utf-8').strip() + target_slot = "_b" if current_slot == "_a" else "_a" + target_slot_number = "0" if target_slot == "_a" else "1" + + cloudlog.info(f"Current slot {current_slot}, target slot {target_slot}") + + # set target slot as unbootable + os.system(f"abctl --set_unbootable {target_slot_number}") + + for partition in update: + success = False + + for retries in range(10): + try: + flash_partition(cloudlog, spinner, target_slot, partition) + success = True + break + + except requests.exceptions.RequestException: + cloudlog.exception("Failed") + spinner.update("Waiting for internet...") + cloudlog.info(f"Failed to download {partition['name']}, retrying ({retries})") + time.sleep(10) + + if not success: + cloudlog.info(f"Failed to flash {partition['name']}, aborting") + raise Exception("Maximum retries exceeded") + + cloudlog.info(f"AGNOS ready on slot {target_slot}") + + +if __name__ == "__main__": + import logging + import time + import sys + + if len(sys.argv) != 2: + print("Usage: ./agnos.py ") + exit(1) + + spinner = Spinner() + spinner.update("Updating AGNOS") + time.sleep(5) + + logging.basicConfig(level=logging.INFO) + flash_agnos_update(sys.argv[1], logging, spinner) diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index 4c2e7178f..7fca07a89 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -1,7 +1,9 @@ +import os import subprocess +from pathlib import Path from cereal import log -from selfdrive.hardware.base import HardwareBase +from selfdrive.hardware.base import HardwareBase, ThermalConfig NM = 'org.freedesktop.NetworkManager' NM_CON_ACT = NM + '.Connection.Active' @@ -16,8 +18,10 @@ MM_SIM = MM + ".Sim" MM_MODEM_STATE_CONNECTED = 11 -NetworkType = log.ThermalData.NetworkType -NetworkStrength = log.ThermalData.NetworkStrength +TIMEOUT = 0.1 + +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength # https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 @@ -32,50 +36,61 @@ class Tici(HardwareBase): self.nm = self.bus.get_object(NM, '/org/freedesktop/NetworkManager') self.mm = self.bus.get_object(MM, '/org/freedesktop/ModemManager1') + def get_os_version(self): + with open("/VERSION") as f: + return f.read().strip() + + def get_device_type(self): + return "tici" + def get_sound_card_online(self): - return True + return os.system("pulseaudio --check") == 0 def reboot(self, reason=None): subprocess.check_output(["sudo", "reboot"]) def uninstall(self): - # TODO: implement uninstall. reboot to factory reset? - pass + Path("/data/__system_reset__").touch() + os.sync() + self.reboot() def get_serial(self): return self.get_cmdline()['androidboot.serialno'] def get_network_type(self): - primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS) - primary_connection = self.bus.get_object(NM, primary_connection) - tp = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS) + try: + primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + primary_connection = self.bus.get_object(NM, primary_connection) + tp = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) - if tp in ['802-3-ethernet', '802-11-wireless']: - return NetworkType.wifi - elif tp in ['gsm']: - modem = self.get_modem() - access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS) - if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE: - return NetworkType.cell4G - elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS: - return NetworkType.cell3G - else: - return NetworkType.cell2G + if tp in ['802-3-ethernet', '802-11-wireless']: + return NetworkType.wifi + elif tp in ['gsm']: + modem = self.get_modem() + access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE: + return NetworkType.cell4G + elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS: + return NetworkType.cell3G + else: + return NetworkType.cell2G + except Exception: + pass return NetworkType.none def get_modem(self): - objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager") + objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=TIMEOUT) modem_path = list(objects.keys())[0] return self.bus.get_object(MM, modem_path) def get_wlan(self): - wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM) + wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM, timeout=TIMEOUT) return self.bus.get_object(NM, wlan_path) def get_sim_info(self): modem = self.get_modem() - sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS) + sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) if sim_path == "/": return { @@ -88,11 +103,11 @@ class Tici(HardwareBase): else: sim = self.bus.get_object(MM, sim_path) return { - 'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS)), - 'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS)), + 'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)), + 'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)), 'network_type': ["Unknown"], 'sim_state': ["READY"], - 'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS) == MM_MODEM_STATE_CONNECTED, + 'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE_CONNECTED, } def get_subscriber_info(self): @@ -102,7 +117,7 @@ class Tici(HardwareBase): if slot != 0: return "" - return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS)) + return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) def parse_strength(self, percentage): if percentage < 25: @@ -117,19 +132,22 @@ class Tici(HardwareBase): def get_network_strength(self, network_type): network_strength = NetworkStrength.unknown - if network_type == NetworkType.none: - pass - elif network_type == NetworkType.wifi: - wlan = self.get_wlan() - active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS) - if active_ap_path != "/": - active_ap = self.bus.get_object(NM, active_ap_path) - strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS)) + try: + if network_type == NetworkType.none: + pass + elif network_type == NetworkType.wifi: + wlan = self.get_wlan() + active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if active_ap_path != "/": + active_ap = self.bus.get_object(NM, active_ap_path) + strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) + network_strength = self.parse_strength(strength) + else: # Cellular + modem = self.get_modem() + strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)[0]) network_strength = self.parse_strength(strength) - else: # Cellular - modem = self.get_modem() - strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS)[0]) - network_strength = self.parse_strength(strength) + except Exception: + pass return network_strength @@ -158,3 +176,10 @@ class Tici(HardwareBase): def get_current_power_draw(self): return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6) + + def shutdown(self): + # Note that for this to work and have the device stay powered off, the panda needs to be in UsbPowerMode::CLIENT! + os.system("sudo poweroff") + + def get_thermal_config(self): + return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(70, 1000)) diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 59516574d..43e5d1fec 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -2,14 +2,7 @@ Import('env', 'common', 'cereal', 'messaging') loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] -env.Program("ubloxd", [ - "ubloxd.cc", - "ublox_msg.cc", - "ubloxd_main.cc"], - LIBS=loc_libs) +env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs) -env.Program("ubloxd_test", [ - "ubloxd_test.cc", - "ublox_msg.cc", - "ubloxd_main.cc"], - LIBS=loc_libs) +if GetOption("test"): + env.Program("ubloxd_test", ["ubloxd_test.cc", "ublox_msg.cc", "ubloxd_main.cc"], LIBS=loc_libs) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 9026fc339..d061452f5 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -1,8 +1,10 @@ #!/usr/bin/env python3 +import json import numpy as np import sympy as sp import cereal.messaging as messaging from cereal import log +from common.params import Params import common.transformations.coordinates as coord from common.transformations.orientation import ecef_euler_from_ned, \ euler_from_quat, \ @@ -194,7 +196,7 @@ class Localizer(): orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION]) orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef) - orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)]) + orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)]) orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps)) if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1: @@ -238,6 +240,7 @@ class Localizer(): def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log for sensor_reading in log: + sensor_time = 1e-9 * sensor_reading.timestamp # TODO: handle messages from two IMUs at the same time if sensor_reading.source == SensorSource.lsm6ds3: continue @@ -247,7 +250,7 @@ class Localizer(): self.gyro_counter += 1 if self.gyro_counter % SENSOR_DECIMATION == 0: v = sensor_reading.gyroUncalibrated.v - self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) + self.update_kalman(sensor_time, ObservationKind.PHONE_GYRO, [-v[2], -v[1], -v[0]]) # Accelerometer if sensor_reading.sensor == 1 and sensor_reading.type == 1: @@ -258,7 +261,7 @@ class Localizer(): self.acc_counter += 1 if self.acc_counter % SENSOR_DECIMATION == 0: v = sensor_reading.acceleration.v - self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) + self.update_kalman(sensor_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) def handle_live_calib(self, current_time, log): if len(log.rpyCalib): @@ -295,6 +298,7 @@ def locationd_thread(sm, pm, disabled_logs=None): if pm is None: pm = messaging.PubMaster(['liveLocationKalman']) + params = Params() localizer = Localizer(disabled_logs=disabled_logs) while True: @@ -327,6 +331,14 @@ def locationd_thread(sm, pm, disabled_logs=None): msg.liveLocationKalman.gpsOK = gps_age < 1.0 pm.send('liveLocationKalman', msg) + if sm.frame % 1200 == 0 and msg.liveLocationKalman.gpsOK: # once a minute + location = { + 'latitude': msg.liveLocationKalman.positionGeodetic.value[0], + 'longitude': msg.liveLocationKalman.positionGeodetic.value[1], + 'altitude': msg.liveLocationKalman.positionGeodetic.value[2], + } + params.put("LastGPSPosition", json.dumps(location)) + def main(sm=None, pm=None): locationd_thread(sm, pm) diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index d2153487b..8c9cb7a37 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -57,7 +57,7 @@ class LiveKalman(): # process noise Q = np.diag([0.03**2, 0.03**2, 0.03**2, - 0.001**2, 0.001*2, 0.001**2, + 0.001**2, 0.001**2, 0.001**2, 0.01**2, 0.01**2, 0.01**2, 0.1**2, 0.1**2, 0.1**2, (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index c0afe1697..263f5cc14 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -48,7 +48,7 @@ class ParamsLearner: self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]])) elif which == 'carState': - self.steering_angle = msg.steeringAngle + self.steering_angle = msg.steeringAngleDeg self.steering_pressed = msg.steeringPressed self.speed = msg.vEgo @@ -56,7 +56,7 @@ class ParamsLearner: self.active = self.speed > 5 and in_linear_region if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]])) self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]])) if not self.active: @@ -88,18 +88,23 @@ def main(sm=None, pm=None): cloudlog.info("Parameter learner found parameters for wrong car.") params = None - if (params is not None) and not all(( - abs(params['angleOffsetAverage']) < 10.0, - min_sr <= params['steerRatio'] <= max_sr)): - cloudlog.info(f"Invalid starting values found {params}") + try: + if params is not None and not all(( + abs(params.get('angleOffsetAverageDeg')) < 10.0, + min_sr <= params['steerRatio'] <= max_sr)): + cloudlog.info(f"Invalid starting values found {params}") + params = None + except Exception as e: + cloudlog.info(f"Error reading params {params}: {str(e)}") params = None + # TODO: cache the params with the capnp struct if params is None: params = { 'carFingerprint': CP.carFingerprint, 'steerRatio': CP.steerRatio, 'stiffnessFactor': 1.0, - 'angleOffsetAverage': 0.0, + 'angleOffsetAverageDeg': 0.0, } cloudlog.info("Parameter learner resetting to default values") @@ -107,7 +112,7 @@ def main(sm=None, pm=None): # Without a way to detect this we have to reset the stiffness every drive params['stiffnessFactor'] = 1.0 - learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage'])) + learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg'])) while True: sm.update() @@ -127,11 +132,11 @@ def main(sm=None, pm=None): x = learner.kf.x msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) - msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) - msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST]) + msg.liveParameters.angleOffsetAverageDeg = math.degrees(x[States.ANGLE_OFFSET]) + msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees(x[States.ANGLE_OFFSET_FAST]) msg.liveParameters.valid = all(( - abs(msg.liveParameters.angleOffsetAverage) < 10.0, - abs(msg.liveParameters.angleOffset) < 10.0, + abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0, + abs(msg.liveParameters.angleOffsetDeg) < 10.0, 0.2 <= msg.liveParameters.stiffnessFactor <= 5.0, min_sr <= msg.liveParameters.steerRatio <= max_sr, )) @@ -141,7 +146,7 @@ def main(sm=None, pm=None): 'carFingerprint': CP.carFingerprint, 'steerRatio': msg.liveParameters.steerRatio, 'stiffnessFactor': msg.liveParameters.stiffnessFactor, - 'angleOffsetAverage': msg.liveParameters.angleOffsetAverage, + 'angleOffsetAverageDeg': msg.liveParameters.angleOffsetAverageDeg, } put_nonblocking("LiveParameters", json.dumps(params)) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 74d56ed34..154200c08 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -1,20 +1,11 @@ #include #include -#include #include -#include -#include -#include -#include -#include #include #include #include -#include -#include "common/params.h" #include "common/swaglog.h" -#include "common/timing.h" #include "ublox_msg.h" @@ -173,8 +164,7 @@ inline bool UbloxMsgParser::valid_cheksum() { inline bool UbloxMsgParser::valid() { return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE && - needed_bytes() == 0 && - valid_cheksum(); + needed_bytes() == 0 && valid_cheksum(); } inline bool UbloxMsgParser::valid_so_far() { @@ -186,8 +176,9 @@ inline bool UbloxMsgParser::valid_so_far() { //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]); return false; } - if(needed_bytes() == 0 && !valid()) + if(needed_bytes() == 0 && !valid()) { return false; + } return true; } @@ -201,7 +192,7 @@ kj::Array UbloxMsgParser::gen_solution() { 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.setBearingDeg(msg->headMot * 1e-5); gpsLoc.setAccuracy(msg->hAcc * 1e-03); std::tm timeinfo = std::tm(); timeinfo.tm_year = msg->year - 1900; @@ -216,12 +207,12 @@ kj::Array UbloxMsgParser::gen_solution() { gpsLoc.setVNED(f); gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03); gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03); - gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05); + gpsLoc.setBearingAccuracyDeg(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; + return (bool)(val & (1 << shifts)); } kj::Array UbloxMsgParser::gen_raw() { @@ -282,13 +273,15 @@ kj::Array UbloxMsgParser::gen_nav_data() { 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]); + subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid]; + if (subframeId == 1) { + map = subframes_map(); + map[subframeId] = words; + } else if (map.find(subframeId-1) != map.end()) { + map[subframeId] = words; + } + if(map.size() == 5) { + EphemerisData ephem_data(msg->svid, map); MessageBuilder msg_builder; auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); eph.setSvId(ephem_data.svId); @@ -342,10 +335,44 @@ kj::Array UbloxMsgParser::gen_mon_hw() { return capnp::messageToFlatArray(msg_builder); } +kj::Array UbloxMsgParser::gen_mon_hw2() { + mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); + hwStatus.setOfsI(msg->ofsI); + hwStatus.setMagI(msg->magI); + hwStatus.setOfsQ(msg->ofsQ); + hwStatus.setMagQ(msg->magQ); + + switch (msg->cfgSource) { + case 114: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); + break; + case 111: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); + break; + case 112: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); + break; + case 102: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); + break; + default: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); + break; + } + + hwStatus.setLowLevCfg(msg->lowLevCfg); + hwStatus.setPostStatus(msg->postStatus); + + return capnp::messageToFlatArray(msg_builder); +} + 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 ); + bytes_consumed = std::min((uint32_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; diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 4bb103bb5..2e99e94e1 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -3,8 +3,6 @@ #include #include "messaging.hpp" -#define min(x, y) ((x) <= (y) ? (x) : (y)) - // NAV_PVT typedef struct __attribute__((packed)) { uint32_t iTOW; @@ -68,6 +66,7 @@ typedef struct __attribute__((packed)) { int8_t trkStat; int8_t reserved3; } rxm_raw_msg_extra; + // RXM_SFRBX typedef struct __attribute__((packed)) { int8_t gnssId; @@ -106,6 +105,20 @@ typedef struct __attribute__((packed)) { uint32_t pullL; } mon_hw_msg; +// MON_HW2 +typedef struct __attribute__((packed)) { + int8_t ofsI; + uint8_t magI; + int8_t ofsQ; + uint8_t magQ; + uint8_t cfgSource; + uint8_t reserved1[3]; + uint32_t lowLevCfg; + uint8_t reserved2[8]; + uint32_t postStatus; + uint8_t reserved3[4]; +} mon_hw2_msg; + namespace ublox { // protocol constants const uint8_t PREAMBLE1 = 0xb5; @@ -125,6 +138,7 @@ namespace ublox { // MON messages const uint8_t MSG_MON_HW = 0x09; + const uint8_t MSG_MON_HW2 = 0x0B; const int UBLOX_HEADER_SIZE = 6; const int UBLOX_CHECKSUM_SIZE = 2; @@ -139,6 +153,7 @@ namespace ublox { kj::Array gen_solution(); kj::Array gen_raw(); kj::Array gen_mon_hw(); + kj::Array gen_mon_hw2(); kj::Array gen_nav_data(); bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); diff --git a/selfdrive/locationd/ubloxd.cc b/selfdrive/locationd/ubloxd.cc index 4ffc010f3..5fdcebba0 100644 --- a/selfdrive/locationd/ubloxd.cc +++ b/selfdrive/locationd/ubloxd.cc @@ -1,29 +1,11 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "messaging.hpp" -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" - #include "ublox_msg.h" -const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds - Message * poll_ubloxraw_msg(Poller * poller) { - auto p = poller->poll(ZMQ_POLL_TIMEOUT); + auto p = poller->poll(1000); if (p.size()) { return p[0]->receive(); @@ -32,11 +14,10 @@ Message * poll_ubloxraw_msg(Poller * poller) { } } - int send_gps_event(PubSocket *s, const void *buf, size_t len) { return s->send((char*)buf, len); } int main() { return ubloxd_main(poll_ubloxraw_msg, send_gps_event); -} +} \ No newline at end of file diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index ad895f8d3..51924f644 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -1,9 +1,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -21,17 +19,10 @@ #include "ublox_msg.h" -volatile sig_atomic_t do_exit = 0; // Flag for process exit on signal - -void set_do_exit(int sig) { - do_exit = 1; -} - +ExitHandler do_exit; 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; @@ -100,6 +91,13 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) auto bytes = words.asBytes(); pm.send("ubloxGnss", bytes.begin(), bytes.size()); } + } else if(parser.msg_id() == MSG_MON_HW2) { + //LOGD("MSG_MON_HW2"); + auto words = parser.gen_mon_hw2(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + pm.send("ubloxGnss", bytes.begin(), bytes.size()); + } } else { LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); } diff --git a/selfdrive/locationd/ubloxd_test.cc b/selfdrive/locationd/ubloxd_test.cc deleted file mode 100644 index d1118e4d2..000000000 --- a/selfdrive/locationd/ubloxd_test.cc +++ /dev/null @@ -1,93 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "messaging.hpp" -#include "impl_zmq.hpp" - -#include "common/params.h" -#include "common/swaglog.h" -#include "common/timing.h" -#include "common/util.h" -#include "ublox_msg.h" - -using namespace ublox; -extern volatile sig_atomic_t do_exit; - -void write_file(std::string fpath, uint8_t *to_write, int length) { - FILE* f = fopen(fpath.c_str(), "wb"); - if (!f) { - std::cout << "Open " << fpath << " failed" << std::endl; - return; - } - fwrite(to_write, length, 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; - -Message * poll_ubloxraw_msg(Poller * poller) { - assert(poller); - - size_t consuming = min(len - consumed, 128); - if(consumed < len) { - // create message - MessageBuilder msg_builder; - auto ublox_raw = msg_builder.initEvent().initUbloxRaw(consuming); - memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming); - - auto bytes = msg_builder.toBytes(); - - Message * msg = new ZMQMessage(); - msg->init((char*)bytes.begin(), bytes.size()); - consumed += consuming; - return msg; - } else { - do_exit = 1; - return NULL; - } -} - -int send_gps_event(PubSocket *s, const void *buf, size_t length) { - assert(s); - write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, length); - save_idx++; - return length; -} - -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/logcatd/logcatd_android.cc b/selfdrive/logcatd/logcatd_android.cc index 9a1753b1d..95a0c7b70 100644 --- a/selfdrive/logcatd/logcatd_android.cc +++ b/selfdrive/logcatd/logcatd_android.cc @@ -1,58 +1,68 @@ -#include -#include -#include #include - -//#include #include #include -#include "common/timing.h" +#include "common/util.h" #include "messaging.hpp" int main() { - int err; + ExitHandler do_exit; - struct logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0); - assert(logger_list); - struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN); - assert(main_logger); - struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO); - assert(radio_logger); - struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM); - assert(system_logger); - struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH); - assert(crash_logger); - struct logger *kernel_logger = android_logger_open(logger_list, (log_id_t)5); // LOG_ID_KERNEL - assert(kernel_logger); PubMaster pm({"androidLog"}); - while (1) { - log_msg log_msg; - err = android_logger_list_read(logger_list, &log_msg); - if (err <= 0) { - break; + log_time last_log_time = {}; + logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY | ANDROID_LOG_NONBLOCK, 0, 0); + + while (!do_exit) { + // setup android logging + if (!logger_list) { + logger_list = android_logger_list_alloc_time(ANDROID_LOG_RDONLY | ANDROID_LOG_NONBLOCK, last_log_time, 0); + } + assert(logger_list); + + struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN); + assert(main_logger); + struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO); + assert(radio_logger); + struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM); + assert(system_logger); + struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH); + assert(crash_logger); + struct logger *kernel_logger = android_logger_open(logger_list, (log_id_t)5); // LOG_ID_KERNEL + assert(kernel_logger); + + while (!do_exit) { + log_msg log_msg; + int err = android_logger_list_read(logger_list, &log_msg); + if (err <= 0) break; + + AndroidLogEntry entry; + err = android_log_processLogBuffer(&log_msg.entry_v1, &entry); + if (err < 0) continue; + last_log_time.tv_sec = entry.tv_sec; + last_log_time.tv_nsec = entry.tv_nsec; + + MessageBuilder msg; + auto androidEntry = msg.initEvent().initAndroidLog(); + androidEntry.setId(log_msg.id()); + androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec); + androidEntry.setPriority(entry.priority); + androidEntry.setPid(entry.pid); + androidEntry.setTid(entry.tid); + androidEntry.setTag(entry.tag); + androidEntry.setMessage(entry.message); + + pm.send("androidLog", msg); } - AndroidLogEntry entry; - err = android_log_processLogBuffer(&log_msg.entry_v1, &entry); - if (err < 0) { - continue; - } - - MessageBuilder msg; - auto androidEntry = msg.initEvent().initAndroidLog(); - androidEntry.setId(log_msg.id()); - androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec); - androidEntry.setPriority(entry.priority); - androidEntry.setPid(entry.pid); - androidEntry.setTid(entry.tid); - androidEntry.setTag(entry.tag); - androidEntry.setMessage(entry.message); - - pm.send("androidLog", msg); + android_logger_list_free(logger_list); + logger_list = NULL; + util::sleep_for(500); + } + + if (logger_list) { + android_logger_list_free(logger_list); } - android_logger_list_close(logger_list); return 0; } diff --git a/selfdrive/logcatd/logcatd_systemd.cc b/selfdrive/logcatd/logcatd_systemd.cc index adc187914..da9b3bb88 100644 --- a/selfdrive/logcatd/logcatd_systemd.cc +++ b/selfdrive/logcatd/logcatd_systemd.cc @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -7,37 +8,36 @@ #include #include "common/timing.h" +#include "common/util.h" #include "messaging.hpp" +ExitHandler do_exit; int main(int argc, char *argv[]) { + PubMaster pm({"androidLog"}); sd_journal *journal; - assert(sd_journal_open(&journal, 0) >= 0); - assert(sd_journal_get_fd(journal) >= 0); // needed so sd_journal_wait() works properly if files rotate - assert(sd_journal_seek_tail(journal) >= 0); + int err = sd_journal_open(&journal, 0); + assert(err >= 0); + err = sd_journal_get_fd(journal); // needed so sd_journal_wait() works properly if files rotate + assert(err >= 0); + err = sd_journal_seek_tail(journal); + assert(err >= 0); - int r; - while (true) { - r = sd_journal_next(journal); - assert(r >= 0); + while (!do_exit) { + err = sd_journal_next(journal); + assert(err >= 0); // Wait for new message if we didn't receive anything - if (r == 0){ - do { - r = sd_journal_wait(journal, (uint64_t)-1); - assert(r >= 0); - } while (r == SD_JOURNAL_NOP); - - r = sd_journal_next(journal); - assert(r >= 0); - - if (r == 0) continue; // Try again if we still didn't get anything + if (err == 0){ + err = sd_journal_wait(journal, 1000 * 1000); + assert (err >= 0); + continue; // Try again } uint64_t timestamp = 0; - r = sd_journal_get_realtime_usec(journal, ×tamp); - assert(r >= 0); + err = sd_journal_get_realtime_usec(journal, ×tamp); + assert(err >= 0); const void *data; size_t length; diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index f0577ec11..1492d92f3 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,18 +1,27 @@ -Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc') +Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'gpucommon') -src = ['loggerd.cc', 'logger.cc'] -libs = ['zmq', 'capnp', 'kj', 'z', - 'avformat', 'avcodec', 'swscale', 'avutil', - 'yuv', 'bz2', common, cereal, messaging, visionipc] +logger_lib = env.Library('logger', ["logger.cc"]) +libs = [logger_lib, 'zmq', 'capnp', 'kj', 'z', + 'avformat', 'avcodec', 'swscale', 'avutil', + 'yuv', 'bz2', 'OpenCL', common, cereal, messaging, visionipc] + +src = ['loggerd.cc'] if arch in ["aarch64", "larch64"]: - src += ['encoder.c'] - libs += ['OmxVenc', 'OmxCore'] + src += ['omx_encoder.cc'] + libs += ['OmxCore', 'gsl', 'CB'] + gpucommon if arch == "aarch64": - libs += ['cutils'] + libs += ['OmxVenc', 'cutils'] else: libs += ['pthread'] else: + src += ['raw_logger.cc'] libs += ['pthread'] +if arch == "Darwin": + # fix OpenCL + del libs[libs.index('OpenCL')] + env['FRAMEWORKS'] = ['OpenCL'] + env.Program(src, LIBS=libs) +env.Program('bootlog.cc', LIBS=libs) diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc new file mode 100644 index 000000000..16ca95b27 --- /dev/null +++ b/selfdrive/loggerd/bootlog.cc @@ -0,0 +1,42 @@ +#include +#include +#include "common/swaglog.h" +#include "logger.h" +#include "messaging.hpp" + +static kj::Array build_boot_log() { + MessageBuilder msg; + auto boot = msg.initEvent().initBoot(); + + boot.setWallTimeNanos(nanos_since_epoch()); + + std::string lastKmsg = util::read_file("/sys/fs/pstore/console-ramoops"); + boot.setLastKmsg(capnp::Data::Reader((const kj::byte*)lastKmsg.data(), lastKmsg.size())); + + std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0"); + boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size())); + + std::string launchLog = util::read_file("/tmp/launch_log"); + boot.setLaunchLog(capnp::Text::Reader(launchLog.data(), launchLog.size())); + return capnp::messageToFlatArray(msg); +} + +int main(int argc, char** argv) { + + const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name() + ".bz2"; + LOGW("bootlog to %s", path.c_str()); + + // Open bootlog + int r = logger_mkpath((char*)path.c_str()); + assert(r == 0); + + BZFile bz_file(path.c_str()); + + // Write initdata + bz_file.write(logger_build_init_data().asBytes()); + + // Write bootlog + bz_file.write(build_boot_log().asBytes()); + + return 0; +} diff --git a/selfdrive/loggerd/config.py b/selfdrive/loggerd/config.py index f859e8cb4..3f82b9f8b 100644 --- a/selfdrive/loggerd/config.py +++ b/selfdrive/loggerd/config.py @@ -1,8 +1,11 @@ import os +from pathlib import Path +from selfdrive.hardware import PC if os.environ.get('LOGGERD_ROOT', False): ROOT = os.environ['LOGGERD_ROOT'] - print("Custom loggerd root: ", ROOT) +elif PC: + ROOT = os.path.join(str(Path.home()), ".comma", "media", "0", "realdata") else: ROOT = '/data/media/0/realdata/' diff --git a/selfdrive/loggerd/encoder.c b/selfdrive/loggerd/encoder.c deleted file mode 100644 index 1ebb1dd33..000000000 --- a/selfdrive/loggerd/encoder.c +++ /dev/null @@ -1,810 +0,0 @@ -#pragma clang diagnostic ignored "-Wdeprecated-declarations" - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include - -//#include - -#include - -#include "common/mutex.h" -#include "common/swaglog.h" - -#include "encoder.h" - - -//#define ALOG(...) __android_log_print(ANDROID_LOG_VERBOSE, "omxapp", ##__VA_ARGS__) - -// encoder: lossey codec using hardware hevc -static void wait_for_state(EncoderState *s, OMX_STATETYPE state) { - pthread_mutex_lock(&s->state_lock); - while (s->state != state) { - pthread_cond_wait(&s->state_cv, &s->state_lock); - } - pthread_mutex_unlock(&s->state_lock); -} - -static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, - OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data) { - EncoderState *s = app_data; - - switch (event) { - case OMX_EventCmdComplete: - assert(data1 == OMX_CommandStateSet); - LOG("set state event 0x%x", data2); - pthread_mutex_lock(&s->state_lock); - s->state = data2; - pthread_cond_broadcast(&s->state_cv); - pthread_mutex_unlock(&s->state_lock); - break; - case OMX_EventError: - LOGE("OMX error 0x%08x", data1); - // assert(false); - break; - default: - LOGE("unhandled event %d", event); - assert(false); - break; - } - - return OMX_ErrorNone; -} - -static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, - OMX_BUFFERHEADERTYPE *buffer) { - EncoderState *s = app_data; - - // printf("empty_buffer_done\n"); - - queue_push(&s->free_in, (void*)buffer); - - return OMX_ErrorNone; -} - - -static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, - OMX_BUFFERHEADERTYPE *buffer) { - EncoderState *s = app_data; - - // printf("fill_buffer_done\n"); - - queue_push(&s->done_out, (void*)buffer); - - return OMX_ErrorNone; -} - -static OMX_CALLBACKTYPE omx_callbacks = { - .EventHandler = event_handler, - .EmptyBufferDone = empty_buffer_done, - .FillBufferDone = fill_buffer_done, -}; - -#define PORT_INDEX_IN 0 -#define PORT_INDEX_OUT 1 - -static const char* omx_color_fomat_name(uint32_t format) __attribute__((unused)); -static const char* omx_color_fomat_name(uint32_t format) { - switch (format) { - case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused"; - case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome"; - case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332"; - case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444"; - case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444"; - case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555"; - case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565"; - case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565"; - case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666"; - case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665"; - case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666"; - case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888"; - case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888"; - case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887"; - case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888"; - case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888"; - case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888"; - case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar"; - case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar"; - case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar"; - case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar"; - case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar"; - case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar"; - case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar"; - case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar"; - case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr"; - case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb"; - case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY"; - case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY"; - case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved"; - case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit"; - case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit"; - case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed"; - case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2"; - case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4"; - case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8"; - case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16"; - case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24"; - case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32"; - case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar"; - case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666"; - case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666"; - case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666"; - - case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque"; - case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar"; - case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar"; - case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; - case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled"; - case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m"; - - // case QOMX_COLOR_FormatYVU420SemiPlanar: return "QOMX_COLOR_FormatYVU420SemiPlanar"; - case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka"; - case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka"; - // case QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; - // case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m"; - case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView"; - case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed"; - case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888"; - case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed"; - - default: - return "unkn"; - } -} - -void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { - int err; - - memset(s, 0, sizeof(*s)); - s->filename = filename; - s->width = width; - s->height = height; - s->fps = fps; - mutex_init_reentrant(&s->lock); - - if (!h265) { - s->remuxing = true; - } - - if (downscale) { - s->downscale = true; - s->y_ptr2 = malloc(s->width*s->height); - s->u_ptr2 = malloc(s->width*s->height/4); - s->v_ptr2 = malloc(s->width*s->height/4); - } - - s->segment = -1; - - s->state = OMX_StateLoaded; - - s->codec_config = NULL; - - queue_init(&s->free_in); - queue_init(&s->done_out); - - pthread_mutex_init(&s->state_lock, NULL); - pthread_cond_init(&s->state_cv, NULL); - - if (h265) { - err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.hevc", - s, &omx_callbacks); - } else { - err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.avc", - s, &omx_callbacks); - } - if (err != OMX_ErrorNone) { - LOGE("error getting codec: %x", err); - } - assert(err == OMX_ErrorNone); - // printf("handle: %p\n", s->handle); - - // setup input port - - OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; - in_port.nSize = sizeof(in_port); - in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); - - in_port.format.video.nFrameWidth = s->width; - in_port.format.video.nFrameHeight = s->height; - in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); - in_port.format.video.nSliceHeight = s->height; - // in_port.nBufferSize = (s->width * s->height * 3) / 2; - in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); - in_port.format.video.xFramerate = (s->fps * 65536); - in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused; - // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar; - in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; - - err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); - - - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); - s->num_in_bufs = in_port.nBufferCountActual; - - // printf("in width: %d, stride: %d\n", - // in_port.format.video.nFrameWidth, in_port.format.video.nStride); - - // setup output port - - OMX_PARAM_PORTDEFINITIONTYPE out_port = {0}; - out_port.nSize = sizeof(out_port); - out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR)&out_port); - assert(err == OMX_ErrorNone); - out_port.format.video.nFrameWidth = s->width; - out_port.format.video.nFrameHeight = s->height; - out_port.format.video.xFramerate = 0; - out_port.format.video.nBitrate = bitrate; - if (h265) { - out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC; - } else { - out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC; - } - out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; - - err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &out_port); - assert(err == OMX_ErrorNone); - - err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &out_port); - assert(err == OMX_ErrorNone); - s->num_out_bufs = out_port.nBufferCountActual; - - OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0}; - bitrate_type.nSize = sizeof(bitrate_type); - bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamVideoBitrate, - (OMX_PTR) &bitrate_type); - assert(err == OMX_ErrorNone); - - bitrate_type.eControlRate = OMX_Video_ControlRateVariable; - bitrate_type.nTargetBitrate = bitrate; - - err = OMX_SetParameter(s->handle, OMX_IndexParamVideoBitrate, - (OMX_PTR) &bitrate_type); - assert(err == OMX_ErrorNone); - - if (h265) { - // setup HEVC - #ifndef QCOM2 - OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0}; - OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc; - #else - OMX_VIDEO_PARAM_PROFILELEVELTYPE hecv_type = {0}; - OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent; - #endif - hecv_type.nSize = sizeof(hecv_type); - hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, index_type, - (OMX_PTR) &hecv_type); - assert(err == OMX_ErrorNone); - - hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain; - hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; - - err = OMX_SetParameter(s->handle, index_type, - (OMX_PTR) &hecv_type); - assert(err == OMX_ErrorNone); - } else { - // setup h264 - OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; - avc.nSize = sizeof(avc); - avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); - assert(err == OMX_ErrorNone); - - avc.nBFrames = 0; - avc.nPFrames = 15; - - avc.eProfile = OMX_VIDEO_AVCProfileBaseline; - avc.eLevel = OMX_VIDEO_AVCLevel31; - - avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; - avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; - - err = OMX_SetParameter(s->handle, OMX_IndexParamVideoAvc, &avc); - assert(err == OMX_ErrorNone); - } - - - // for (int i = 0; ; i++) { - // OMX_VIDEO_PARAM_PORTFORMATTYPE video_port_format = {0}; - // video_port_format.nSize = sizeof(video_port_format); - // video_port_format.nIndex = i; - // video_port_format.nPortIndex = PORT_INDEX_IN; - // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone) - // break; - // printf("in %d: compression 0x%x format 0x%x %s\n", i, - // video_port_format.eCompressionFormat, video_port_format.eColorFormat, - // omx_color_fomat_name(video_port_format.eColorFormat)); - // } - - // for (int i=0; i<32; i++) { - // OMX_VIDEO_PARAM_PROFILELEVELTYPE params = {0}; - // params.nSize = sizeof(params); - // params.nPortIndex = PORT_INDEX_OUT; - // params.nProfileIndex = i; - // if (OMX_GetParameter(s->handle, OMX_IndexParamVideoProfileLevelQuerySupported, ¶ms) != OMX_ErrorNone) - // break; - // printf("profile %d level 0x%x\n", params.eProfile, params.eLevel); - // } - - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); - assert(err == OMX_ErrorNone); - - s->in_buf_headers = calloc(s->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_in_bufs; i++) { - err = OMX_AllocateBuffer(s->handle, &s->in_buf_headers[i], PORT_INDEX_IN, s, - in_port.nBufferSize); - assert(err == OMX_ErrorNone); - } - - s->out_buf_headers = calloc(s->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_out_bufs; i++) { - err = OMX_AllocateBuffer(s->handle, &s->out_buf_headers[i], PORT_INDEX_OUT, s, - out_port.nBufferSize); - assert(err == OMX_ErrorNone); - } - - wait_for_state(s, OMX_StateIdle); - - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL); - assert(err == OMX_ErrorNone); - - wait_for_state(s, OMX_StateExecuting); - - // give omx all the output buffers - for (int i = 0; i < s->num_out_bufs; i++) { - // printf("fill %p\n", s->out_buf_headers[i]); - err = OMX_FillThisBuffer(s->handle, s->out_buf_headers[i]); - assert(err == OMX_ErrorNone); - } - - // fill the input free queue - for (int i = 0; i < s->num_in_bufs; i++) { - queue_push(&s->free_in, (void*)s->in_buf_headers[i]); - } -} - -static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) { - int err; - uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset; - - if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) { - if (s->codec_config_len < out_buf->nFilledLen) { - s->codec_config = realloc(s->codec_config, out_buf->nFilledLen); - } - s->codec_config_len = out_buf->nFilledLen; - memcpy(s->codec_config, buf_data, out_buf->nFilledLen); -#ifdef QCOM2 - out_buf->nTimeStamp = 0; -#endif - } - - if (s->of) { - //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags); - fwrite(buf_data, out_buf->nFilledLen, 1, s->of); - } - - if (s->remuxing) { - if (!s->wrote_codec_config && s->codec_config_len > 0) { - if (s->codec_ctx->extradata_size < s->codec_config_len) { - s->codec_ctx->extradata = realloc(s->codec_ctx->extradata, s->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); - } - s->codec_ctx->extradata_size = s->codec_config_len; - memcpy(s->codec_ctx->extradata, s->codec_config, s->codec_config_len); - memset(s->codec_ctx->extradata + s->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE); - - err = avcodec_parameters_from_context(s->out_stream->codecpar, s->codec_ctx); - assert(err >= 0); - err = avformat_write_header(s->ofmt_ctx, NULL); - assert(err >= 0); - - s->wrote_codec_config = true; - } - - if (out_buf->nTimeStamp > 0) { - // input timestamps are in microseconds - AVRational in_timebase = {1, 1000000}; - - AVPacket pkt; - av_init_packet(&pkt); - pkt.data = buf_data; - pkt.size = out_buf->nFilledLen; - pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, s->ofmt_ctx->streams[0]->time_base, AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); - pkt.duration = av_rescale_q(50*1000, in_timebase, s->ofmt_ctx->streams[0]->time_base); - - if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { - pkt.flags |= AV_PKT_FLAG_KEY; - } - - err = av_write_frame(s->ofmt_ctx, &pkt); - if (err < 0) { LOGW("ts encoder write issue"); } - - av_free_packet(&pkt); - } - } - - // give omx back the buffer -#ifdef QCOM2 - if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { - out_buf->nTimeStamp = 0; - } -#endif - err = OMX_FillThisBuffer(s->handle, out_buf); - assert(err == OMX_ErrorNone); -} - -int encoder_encode_frame(EncoderState *s, - const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, - int *frame_segment, VIPCBufExtra *extra) { - int err; - pthread_mutex_lock(&s->lock); - - if (s->opening) { - encoder_open(s, s->next_path); - s->opening = false; - } - - if (!s->open) { - pthread_mutex_unlock(&s->lock); - return -1; - } - - // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates... - // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this - //pthread_mutex_unlock(&s->lock); - OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in); - //pthread_mutex_lock(&s->lock); - - // if (s->rotating) { - // encoder_close(s); - // encoder_open(s, s->next_path); - // s->segment = s->next_segment; - // s->rotating = false; - // } - int ret = s->counter; - - uint8_t *in_buf_ptr = in_buf->pBuffer; - // printf("in_buf ptr %p\n", in_buf_ptr); - - uint8_t *in_y_ptr = in_buf_ptr; - int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width); - int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, s->width); - // uint8_t *in_uv_ptr = in_buf_ptr + (s->width * s->height); - uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, s->height)); - - if (s->downscale) { - I420Scale(y_ptr, in_width, - u_ptr, in_width/2, - v_ptr, in_width/2, - in_width, in_height, - s->y_ptr2, s->width, - s->u_ptr2, s->width/2, - s->v_ptr2, s->width/2, - s->width, s->height, - kFilterNone); - y_ptr = s->y_ptr2; - u_ptr = s->u_ptr2; - v_ptr = s->v_ptr2; - } - err = I420ToNV12(y_ptr, s->width, - u_ptr, s->width/2, - v_ptr, s->width/2, - in_y_ptr, in_y_stride, - in_uv_ptr, in_uv_stride, - s->width, s->height); - assert(err == 0); - - // in_buf->nFilledLen = (s->width*s->height) + (s->width*s->height/2); - in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height); - in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; - in_buf->nOffset = 0; - in_buf->nTimeStamp = extra->timestamp_eof/1000LL; // OMX_TICKS, in microseconds - s->last_t = in_buf->nTimeStamp; - - err = OMX_EmptyThisBuffer(s->handle, in_buf); - assert(err == OMX_ErrorNone); - - // pump output - while (true) { - OMX_BUFFERHEADERTYPE *out_buf = queue_try_pop(&s->done_out); - if (!out_buf) { - break; - } - handle_out_buf(s, out_buf); - } - - s->dirty = true; - - s->counter++; - - if (frame_segment) { - *frame_segment = s->segment; - } - - if (s->closing) { - encoder_close(s); - s->closing = false; - } - - pthread_mutex_unlock(&s->lock); - return ret; -} - -void encoder_open(EncoderState *s, const char* path) { - int err; - - pthread_mutex_lock(&s->lock); - - snprintf(s->vid_path, sizeof(s->vid_path), "%s/%s", path, s->filename); - LOGD("encoder_open %s remuxing:%d", s->vid_path, s->remuxing); - - if (s->remuxing) { - avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path); - assert(s->ofmt_ctx); - - s->out_stream = avformat_new_stream(s->ofmt_ctx, NULL); - assert(s->out_stream); - - // set codec correctly - av_register_all(); - - AVCodec *codec = NULL; - codec = avcodec_find_encoder(AV_CODEC_ID_H264); - assert(codec); - - s->codec_ctx = avcodec_alloc_context3(codec); - assert(s->codec_ctx); - s->codec_ctx->width = s->width; - s->codec_ctx->height = s->height; - s->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; - s->codec_ctx->time_base = (AVRational){ 1, s->fps }; - - err = avio_open(&s->ofmt_ctx->pb, s->vid_path, AVIO_FLAG_WRITE); - assert(err >= 0); - - s->wrote_codec_config = false; - } else { - s->of = fopen(s->vid_path, "wb"); - assert(s->of); - if (s->codec_config_len > 0) { -#ifndef QCOM2 - fwrite(s->codec_config, s->codec_config_len, 1, s->of); -#endif - } - } - - // create camera lock file - snprintf(s->lock_path, sizeof(s->lock_path), "%s/%s.lock", path, s->filename); - int lock_fd = open(s->lock_path, O_RDWR | O_CREAT, 0777); - assert(lock_fd >= 0); - close(lock_fd); - - s->open = true; - s->counter = 0; - - pthread_mutex_unlock(&s->lock); -} - -void encoder_close(EncoderState *s) { - int err; - - pthread_mutex_lock(&s->lock); - - if (s->open) { - if (s->dirty) { - // drain output only if there could be frames in the encoder - - OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in); - in_buf->nFilledLen = 0; - in_buf->nOffset = 0; - in_buf->nFlags = OMX_BUFFERFLAG_EOS; - in_buf->nTimeStamp = s->last_t + 1000000LL/s->fps; - - err = OMX_EmptyThisBuffer(s->handle, in_buf); - assert(err == OMX_ErrorNone); - - while (true) { - OMX_BUFFERHEADERTYPE *out_buf = queue_pop(&s->done_out); - - handle_out_buf(s, out_buf); - - if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { - break; - } - } - s->dirty = false; - } - - if (s->remuxing) { - av_write_trailer(s->ofmt_ctx); - avcodec_free_context(&s->codec_ctx); - avio_closep(&s->ofmt_ctx->pb); - avformat_free_context(s->ofmt_ctx); - } else { - fclose(s->of); - } - unlink(s->lock_path); - } - s->open = false; - - pthread_mutex_unlock(&s->lock); -} - -void encoder_rotate(EncoderState *s, const char* new_path, int new_segment) { - pthread_mutex_lock(&s->lock); - snprintf(s->next_path, sizeof(s->next_path), "%s", new_path); - s->next_segment = new_segment; - if (s->open) { - if (s->next_segment == -1) { - s->closing = true; - } else { - s->rotating = true; - } - } else { - s->segment = s->next_segment; - s->opening = true; - } - pthread_mutex_unlock(&s->lock); -} - -void encoder_destroy(EncoderState *s) { - int err; - - assert(!s->open); - - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); - assert(err == OMX_ErrorNone); - - wait_for_state(s, OMX_StateIdle); - - err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); - assert(err == OMX_ErrorNone); - - for (int i=0; inum_in_bufs; i++) { - err = OMX_FreeBuffer(s->handle, PORT_INDEX_IN, s->in_buf_headers[i]); - assert(err == OMX_ErrorNone); - } - free(s->in_buf_headers); - - for (int i=0; inum_out_bufs; i++) { - err = OMX_FreeBuffer(s->handle, PORT_INDEX_OUT, s->out_buf_headers[i]); - assert(err == OMX_ErrorNone); - } - free(s->out_buf_headers); - - wait_for_state(s, OMX_StateLoaded); - - err = OMX_FreeHandle(s->handle); - assert(err == OMX_ErrorNone); - - if (s->downscale) { - free(s->y_ptr2); - free(s->u_ptr2); - free(s->v_ptr2); - } -} - -#if 0 - -// cd one/selfdrive/visiond -// clang -// -fPIC -pie -// -std=gnu11 -O2 -g -// -o encoder -// -I ~/one/selfdrive -// -I ~/one/phonelibs/openmax/include -// -I ~/one/phonelibs/libyuv/include -// -lOmxVenc -lOmxCore -llog -// encoder.c -// buffering.c -// -L ~/one/phonelibs/libyuv/lib -l:libyuv.a - -int main() { - int err; - - EncoderState state; - EncoderState *s = &state; - memset(s, 0, sizeof(*s)); - - int w = 1164; - int h = 874; - - encoder_init(s, w, h, 20); - printf("inited\n"); - - encoder_open(s, "/sdcard/t1"); - - // uint8_t *tmpy = malloc(640*480); - // uint8_t *tmpu = malloc((640/2)*(480/2)); - // uint8_t *tmpv = malloc((640/2)*(480/2)); - - // memset(tmpy, 0, 640*480); - // // memset(tmpu, 0xff, (640/2)*(480/2)); - // memset(tmpv, 0, (640/2)*(480/2)); - -// #if 0 - // FILE *infile = fopen("/sdcard/camera_t2.yuv", "rb"); - uint8_t *inbuf = malloc(w*h*3/2); - memset(inbuf, 0, w*h*3/2); - - for (int i=0; i<20*3+5; i++) { - - // fread(inbuf, w*h*3/2, 1, infile); - - uint8_t *tmpy = inbuf; - uint8_t *tmpu = inbuf + w*h; - uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2); - - for (int y=0; yof); - // s->of = fopen("/sdcard/tmpout2.hevc", "wb"); - // if (s->codec_config) { - // fwrite(s->codec_config, s->codec_config_len, 1, s->of); - // } - // encoder_open(s, "/sdcard/t1"); - - encoder_rotate(s, "/sdcard/t2"); - - for (int i=0; i<20*3+5; i++) { - - // fread(inbuf, w*h*3/2, 1, infile); - - uint8_t *tmpy = inbuf; - uint8_t *tmpu = inbuf + w*h; - uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2); - - for (int y=0; y -#include -#include +#include -#include - -#include -#include - -#include "common/cqueue.h" -#include "common/visionipc.h" - -#ifdef __cplusplus -extern "C" { -#endif - -typedef struct EncoderState { - pthread_mutex_t lock; - int width, height, fps; - const char* path; - char vid_path[1024]; - char lock_path[1024]; - bool open; - bool dirty; - int counter; - int segment; - - bool rotating; - bool closing; - bool opening; - char next_path[1024]; - int next_segment; - - const char* filename; - FILE *of; - - size_t codec_config_len; - uint8_t *codec_config; - bool wrote_codec_config; - - pthread_mutex_t state_lock; - pthread_cond_t state_cv; - OMX_STATETYPE state; - - OMX_HANDLETYPE handle; - - int num_in_bufs; - OMX_BUFFERHEADERTYPE** in_buf_headers; - - int num_out_bufs; - OMX_BUFFERHEADERTYPE** out_buf_headers; - - uint64_t last_t; - - Queue free_in; - Queue done_out; - - AVFormatContext *ofmt_ctx; - AVCodecContext *codec_ctx; - AVStream *out_stream; - bool remuxing; - - bool downscale; - uint8_t *y_ptr2, *u_ptr2, *v_ptr2; -} EncoderState; - -void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); -int encoder_encode_frame(EncoderState *s, - const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, - int in_width, int in_height, - int *frame_segment, VIPCBufExtra *extra); -void encoder_open(EncoderState *s, const char* path); -void encoder_rotate(EncoderState *s, const char* new_path, int new_segment); -void encoder_close(EncoderState *s); -void encoder_destroy(EncoderState *s); - -#ifdef __cplusplus -} -#endif - -#endif +class VideoEncoder { +public: + virtual ~VideoEncoder() {} + virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, uint64_t ts) = 0; + virtual void encoder_open(const char* path) = 0; + virtual void encoder_close() = 0; +}; diff --git a/selfdrive/loggerd/frame_logger.h b/selfdrive/loggerd/frame_logger.h deleted file mode 100644 index 85ddabafd..000000000 --- a/selfdrive/loggerd/frame_logger.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef FRAMELOGGER_H -#define FRAMELOGGER_H - -#include - -#include -#include - -class FrameLogger { -public: - virtual ~FrameLogger() {} - - virtual void Open(const std::string &path) = 0; - virtual void Close() = 0; - - int LogFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, int *frame_segment) { - std::lock_guard guard(lock); - - if (opening) { - Open(next_path); - opening = false; - } - - if (!is_open) return -1; - - if (rotating) { - Close(); - Open(next_path); - segment = next_segment; - rotating = false; - } - - int ret = ProcessFrame(ts, y_ptr, u_ptr, v_ptr); - - if (ret >= 0 && frame_segment) { - *frame_segment = segment; - } - - if (closing) { - Close(); - closing = false; - } - - return ret; - } - - void Rotate(const std::string &new_path, int new_segment) { - std::lock_guard guard(lock); - - next_path = new_path; - next_segment = new_segment; - if (is_open) { - if (next_segment == -1) { - closing = true; - } else { - rotating = true; - } - } else { - segment = next_segment; - opening = true; - } - } - -protected: - - virtual int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) = 0; - - std::recursive_mutex lock; - - bool is_open = false; - int segment = -1; - - std::string vid_path, lock_path; - -private: - int next_segment = -1; - bool opening = false, closing = false, rotating = false; - std::string next_path; -}; - -#endif diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index 0cec20f68..e369a8ed4 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -6,28 +6,33 @@ #include #include #include - #include #include -#include -#include -#include "messaging.hpp" +#include +#include +#include +#ifdef QCOM +#include +#endif #include "common/swaglog.h" - +#include "common/params.h" +#include "common/version.h" +#include "messaging.hpp" #include "logger.h" -static void log_sentinel(LoggerState *s, cereal::Sentinel::SentinelType type) { - MessageBuilder msg; - auto sen = msg.initEvent().initSentinel(); - sen.setType(type); - auto bytes = msg.toBytes(); - logger_log(s, bytes.begin(), bytes.size(), true); +// ***** logging helpers ***** + +void append_property(const char* key, const char* value, void *cookie) { + std::vector > *properties = + (std::vector > *)cookie; + + properties->push_back(std::make_pair(std::string(key), std::string(value))); } -static int mkpath(char* file_path) { +int logger_mkpath(char* file_path) { assert(file_path && *file_path); char* p; for (p=strchr(file_path+1, '/'); p; p=strchr(p+1, '/')) { @@ -43,29 +48,112 @@ static int mkpath(char* file_path) { return 0; } -void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog) { - memset(s, 0, sizeof(*s)); - if (init_data) { - s->init_data = (uint8_t*)malloc(init_data_len); - assert(s->init_data); - memcpy(s->init_data, init_data, init_data_len); - s->init_data_len = init_data_len; +// ***** log metadata ***** +kj::Array logger_build_init_data() { + MessageBuilder msg; + auto init = msg.initEvent().initInitData(); + + if (util::file_exists("/EON")) { + init.setDeviceType(cereal::InitData::DeviceType::NEO); + } else if (util::file_exists("/TICI")) { + init.setDeviceType(cereal::InitData::DeviceType::TICI); + } else { + init.setDeviceType(cereal::InitData::DeviceType::PC); } + init.setVersion(capnp::Text::Reader(COMMA_VERSION)); + + std::ifstream cmdline_stream("/proc/cmdline"); + std::vector kernel_args; + std::string buf; + while (cmdline_stream >> buf) { + kernel_args.push_back(buf); + } + + auto lkernel_args = init.initKernelArgs(kernel_args.size()); + for (int i=0; i > properties; + property_list(append_property, (void*)&properties); + + auto lentries = init.initAndroidProperties().initEntries(properties.size()); + for (int i=0; i params_map; + params.read_db_all(¶ms_map); + auto lparams = init.initParams().initEntries(params_map.size()); + int i = 0; + for (auto& kv : params_map) { + auto lentry = lparams[i]; + lentry.setKey(kv.first); + lentry.setValue(kv.second); + i++; + } + } + return capnp::messageToFlatArray(msg); +} + +std::string logger_get_route_name() { + char route_name[64] = {'\0'}; + time_t rawtime = time(NULL); + struct tm timeinfo; + localtime_r(&rawtime, &timeinfo); + strftime(route_name, sizeof(route_name), "%Y-%m-%d--%H-%M-%S", &timeinfo); + return route_name; +} + +void log_init_data(LoggerState *s) { + auto bytes = s->init_data.asBytes(); + logger_log(s, bytes.begin(), bytes.size(), s->has_qlog); +} + + +static void log_sentinel(LoggerState *s, cereal::Sentinel::SentinelType type) { + MessageBuilder msg; + auto sen = msg.initEvent().initSentinel(); + sen.setType(type); + auto bytes = msg.toBytes(); + + logger_log(s, bytes.begin(), bytes.size(), true); +} + +// ***** logging functions ***** + +void logger_init(LoggerState *s, const char* log_name, bool has_qlog) { umask(0); pthread_mutex_init(&s->lock, NULL); s->part = -1; s->has_qlog = has_qlog; - - time_t rawtime = time(NULL); - struct tm timeinfo; - localtime_r(&rawtime, &timeinfo); - - strftime(s->route_name, sizeof(s->route_name), - "%Y-%m-%d--%H-%M-%S", &timeinfo); + s->route_name = logger_get_route_name(); snprintf(s->log_name, sizeof(s->log_name), "%s", log_name); + s->init_data = logger_build_init_data(); } static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { @@ -81,69 +169,27 @@ static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { assert(h); snprintf(h->segment_path, sizeof(h->segment_path), - "%s/%s--%d", root_path, s->route_name, s->part); + "%s/%s--%d", root_path, s->route_name.c_str(), s->part); snprintf(h->log_path, sizeof(h->log_path), "%s/%s.bz2", h->segment_path, s->log_name); snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog.bz2", h->segment_path); snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path); - err = mkpath(h->log_path); + err = logger_mkpath(h->log_path); if (err) return NULL; FILE* lock_file = fopen(h->lock_path, "wb"); if (lock_file == NULL) return NULL; fclose(lock_file); - h->log_file = fopen(h->log_path, "wb"); - if (h->log_file == NULL) goto fail; - + h->log = std::make_unique(h->log_path); if (s->has_qlog) { - h->qlog_file = fopen(h->qlog_path, "wb"); - if (h->qlog_file == NULL) goto fail; - } - - int bzerror; - h->bz_file = BZ2_bzWriteOpen(&bzerror, h->log_file, 9, 0, 30); - if (bzerror != BZ_OK) goto fail; - - if (s->has_qlog) { - h->bz_qlog = BZ2_bzWriteOpen(&bzerror, h->qlog_file, 9, 0, 30); - if (bzerror != BZ_OK) goto fail; - } - - if (s->init_data) { - BZ2_bzWrite(&bzerror, h->bz_file, s->init_data, s->init_data_len); - if (bzerror != BZ_OK) goto fail; - - if (s->has_qlog) { - // init data goes in the qlog too - BZ2_bzWrite(&bzerror, h->bz_qlog, s->init_data, s->init_data_len); - if (bzerror != BZ_OK) goto fail; - } + h->q_log = std::make_unique(h->qlog_path); } pthread_mutex_init(&h->lock, NULL); h->refcnt++; return h; -fail: - LOGE("logger failed to open files"); - if (h->bz_file) { - BZ2_bzWriteClose(&bzerror, h->bz_file, 0, NULL, NULL); - h->bz_file = NULL; - } - if (h->bz_qlog) { - BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL); - h->bz_qlog = NULL; - } - if (h->qlog_file) { - fclose(h->qlog_file); - h->qlog_file = NULL; - } - if (h->log_file) { - fclose(h->log_file); - h->log_file = NULL; - } - return NULL; } int logger_next(LoggerState *s, const char* root_path, @@ -175,6 +221,8 @@ int logger_next(LoggerState *s, const char* root_path, pthread_mutex_unlock(&s->lock); + // write beggining of log metadata + log_init_data(s); log_sentinel(s, is_start_of_route ? cereal::Sentinel::SentinelType::START_OF_ROUTE : cereal::Sentinel::SentinelType::START_OF_SEGMENT); return 0; } @@ -203,7 +251,6 @@ void logger_close(LoggerState *s) { log_sentinel(s, cereal::Sentinel::SentinelType::END_OF_ROUTE); pthread_mutex_lock(&s->lock); - free(s->init_data); if (s->cur_handle) { lh_close(s->cur_handle); } @@ -213,11 +260,9 @@ void logger_close(LoggerState *s) { void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) { pthread_mutex_lock(&h->lock); assert(h->refcnt > 0); - int bzerror; - BZ2_bzWrite(&bzerror, h->bz_file, data, data_size); - - if (in_qlog && h->bz_qlog != NULL) { - BZ2_bzWrite(&bzerror, h->bz_qlog, data, data_size); + h->log->write(data, data_size); + if (in_qlog && h->q_log) { + h->q_log->write(data, data_size); } pthread_mutex_unlock(&h->lock); } @@ -227,22 +272,8 @@ void lh_close(LoggerHandle* h) { assert(h->refcnt > 0); h->refcnt--; if (h->refcnt == 0) { - if (h->bz_file){ - int bzerror; - BZ2_bzWriteClose(&bzerror, h->bz_file, 0, NULL, NULL); - h->bz_file = NULL; - } - if (h->bz_qlog){ - int bzerror; - BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL); - h->bz_qlog = NULL; - } - if (h->qlog_file) { - fclose(h->qlog_file); - h->qlog_file = NULL; - } - fclose(h->log_file); - h->log_file = NULL; + h->log.reset(nullptr); + h->q_log.reset(nullptr); unlink(h->lock_path); pthread_mutex_unlock(&h->lock); pthread_mutex_destroy(&h->lock); diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h index ab8798481..7e26a2246 100644 --- a/selfdrive/loggerd/logger.h +++ b/selfdrive/loggerd/logger.h @@ -1,39 +1,71 @@ -#ifndef LOGGER_H -#define LOGGER_H +#pragma once #include #include #include +#include #include +#include +#include +#include "common/util.h" -#ifdef __cplusplus -extern "C" { +#if defined(QCOM) || defined(QCOM2) +const std::string LOG_ROOT = "/data/media/0/realdata"; +#else +const std::string LOG_ROOT = util::getenv_default("HOME", "/.comma/media/0/realdata", "/data/media/0/realdata"); #endif #define LOGGER_MAX_HANDLES 16 +class BZFile { + public: + BZFile(const char* path) { + file = fopen(path, "wb"); + assert(file != nullptr); + int bzerror; + bz_file = BZ2_bzWriteOpen(&bzerror, file, 9, 0, 30); + assert(bzerror == BZ_OK); + } + ~BZFile() { + int bzerror; + BZ2_bzWriteClose(&bzerror, bz_file, 0, nullptr, nullptr); + if (bzerror != BZ_OK) { + LOGE("BZ2_bzWriteClose error, bzerror=%d", bzerror); + } + int err = fclose(file); + assert(err == 0); + } + inline void write(void* data, size_t size) { + int bzerror; + BZ2_bzWrite(&bzerror, bz_file, data, size); + if (bzerror != BZ_OK && !error_logged) { + LOGE("BZ2_bzWrite error, bzerror=%d", bzerror); + error_logged = true; + } + } + inline void write(kj::ArrayPtr array) { write(array.begin(), array.size()); } + + private: + bool error_logged = false; + FILE* file = nullptr; + BZFILE* bz_file = nullptr; +}; + typedef struct LoggerHandle { pthread_mutex_t lock; int refcnt; char segment_path[4096]; char log_path[4096]; - char lock_path[4096]; - FILE* log_file; - BZFILE* bz_file; - - FILE* qlog_file; char qlog_path[4096]; - BZFILE* bz_qlog; + char lock_path[4096]; + std::unique_ptr log, q_log; } LoggerHandle; typedef struct LoggerState { pthread_mutex_t lock; - - uint8_t* init_data; - size_t init_data_len; - int part; - char route_name[64]; + kj::Array init_data; + std::string route_name; char log_name[64]; bool has_qlog; @@ -41,7 +73,10 @@ typedef struct LoggerState { LoggerHandle* cur_handle; } LoggerState; -void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog); +int logger_mkpath(char* file_path); +kj::Array logger_build_init_data(); +std::string logger_get_route_name(); +void logger_init(LoggerState *s, const char* log_name, bool has_qlog); int logger_next(LoggerState *s, const char* root_path, char* out_segment_path, size_t out_segment_path_len, int* out_part); @@ -51,9 +86,3 @@ void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog); void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog); void lh_close(LoggerHandle* h); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 8dd328df7..cad256e9f 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -3,20 +3,13 @@ #include #include #include -#include #include -#include #include #include -#include -#include #include +#include #include -#include -#include -#include -#include #include #include #include @@ -24,49 +17,53 @@ #include #include -#ifdef QCOM -#include -#endif -#include "common/version.h" #include "common/timing.h" #include "common/params.h" #include "common/swaglog.h" -#include "common/visionipc.h" -#include "common/utilpp.h" #include "common/util.h" #include "camerad/cameras/camera_common.h" #include "logger.h" #include "messaging.hpp" #include "services.h" -#if !(defined(QCOM) || defined(QCOM2)) -// no encoder on PC -#define DISABLE_ENCODER -#endif +#include "visionipc.h" +#include "visionipc_client.h" -#ifndef DISABLE_ENCODER #include "encoder.h" +#if defined(QCOM) || defined(QCOM2) +#include "omx_encoder.h" +#define Encoder OmxEncoder +#else +#include "raw_logger.h" +#define Encoder RawLogger #endif -#define MAIN_BITRATE 5000000 -#define QCAM_BITRATE 128000 -#define MAIN_FPS 20 +namespace { + +constexpr int MAIN_FPS = 20; + #ifndef QCOM2 -#define MAX_CAM_IDX LOG_CAMERA_ID_DCAMERA -#define DCAM_BITRATE 2500000 +constexpr int MAIN_BITRATE = 5000000; +constexpr int MAX_CAM_IDX = LOG_CAMERA_ID_DCAMERA; +constexpr int DCAM_BITRATE = 2500000; #else -#define MAX_CAM_IDX LOG_CAMERA_ID_ECAMERA -#define DCAM_BITRATE MAIN_BITRATE +constexpr int MAIN_BITRATE = 10000000; +constexpr int MAX_CAM_IDX = LOG_CAMERA_ID_ECAMERA; +constexpr int DCAM_BITRATE = MAIN_BITRATE; #endif #define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead +const int SEGMENT_LENGTH = getenv("LOGGERD_TEST") ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60; + +ExitHandler do_exit; + LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { [LOG_CAMERA_ID_FCAMERA] = { - .stream_type = VISION_STREAM_YUV, + .stream_type = VISION_STREAM_YUV_BACK, .filename = "fcamera.hevc", - .frame_packet_name = "frame", + .frame_packet_name = "roadCameraState", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -76,7 +73,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { [LOG_CAMERA_ID_DCAMERA] = { .stream_type = VISION_STREAM_YUV_FRONT, .filename = "dcamera.hevc", - .frame_packet_name = "frontFrame", + .frame_packet_name = "driverCameraState", .fps = MAIN_FPS, // on EONs, more compressed this way .bitrate = DCAM_BITRATE, .is_h265 = true, @@ -86,7 +83,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { [LOG_CAMERA_ID_ECAMERA] = { .stream_type = VISION_STREAM_YUV_WIDE, .filename = "ecamera.hevc", - .frame_packet_name = "wideFrame", + .frame_packet_name = "wideRoadCameraState", .fps = MAIN_FPS, .bitrate = MAIN_BITRATE, .is_h265 = true, @@ -96,7 +93,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { [LOG_CAMERA_ID_QCAMERA] = { .filename = "qcamera.ts", .fps = MAIN_FPS, - .bitrate = QCAM_BITRATE, + .bitrate = 256000, .is_h265 = false, .downscale = true, #ifndef QCOM2 @@ -106,51 +103,30 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { #endif }, }; -#define SEGMENT_LENGTH 60 - -#define LOG_ROOT "/data/media/0/realdata" - - -namespace { - -double randrange(double a, double b) __attribute__((unused)); -double randrange(double a, double b) { - static std::mt19937 gen(millis_since_boot()); - - std::uniform_real_distribution<> dist(a, b); - return dist(gen); -} - - -volatile sig_atomic_t do_exit = 0; -static void set_do_exit(int sig) { - do_exit = 1; -} - -static bool file_exists (const std::string& fn) { - std::ifstream f(fn); - return f.good(); -} class RotateState { public: SubSocket* fpkt_sock; uint32_t stream_frame_id, log_frame_id, last_rotate_frame_id; bool enabled, should_rotate, initialized; + std::atomic rotating; + std::atomic cur_seg; RotateState() : fpkt_sock(nullptr), stream_frame_id(0), log_frame_id(0), - last_rotate_frame_id(UINT32_MAX), enabled(false), should_rotate(false), initialized(false) {}; + last_rotate_frame_id(UINT32_MAX), enabled(false), should_rotate(false), initialized(false), rotating(false), cur_seg(-1) {}; void waitLogThread() { std::unique_lock lk(fid_lock); - while (stream_frame_id > log_frame_id //if the log camera is older, wait for it to catch up. + while (stream_frame_id > log_frame_id // if the log camera is older, wait for it to catch up. && (stream_frame_id - log_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted) && !do_exit) { cv.wait(lk); } } - void cancelWait() { cv.notify_one(); } + void cancelWait() { + cv.notify_one(); + } void setStreamFrameId(uint32_t frame_id) { fid_lock.lock(); @@ -167,10 +143,11 @@ public: } void rotate() { - if (!enabled) { return; } - std::unique_lock lk(fid_lock); - should_rotate = true; - last_rotate_frame_id = stream_frame_id; + if (enabled) { + std::unique_lock lk(fid_lock); + should_rotate = true; + last_rotate_frame_id = stream_frame_id; + } } void finish_rotate() { @@ -185,202 +162,135 @@ private: struct LoggerdState { Context *ctx; - LoggerState logger; + LoggerState logger = {}; char segment_path[4096]; int rotate_segment; pthread_mutex_t rotate_lock; - int num_encoder; - std::atomic rotate_seq_id; - std::atomic should_close; - std::atomic finish_close; - RotateState rotate_state[LOG_CAMERA_ID_MAX-1]; }; LoggerdState s; -#ifndef DISABLE_ENCODER -void encoder_thread(RotateState *rotate_state, int cam_idx) { +void encoder_thread(int cam_idx) { + assert(cam_idx < LOG_CAMERA_ID_MAX-1); - switch (cam_idx) { - case LOG_CAMERA_ID_DCAMERA: { - LOGW("recording front camera"); - set_thread_name("FrontCameraEncoder"); - break; - } - case LOG_CAMERA_ID_FCAMERA: { - set_thread_name("RearCameraEncoder"); - break; - } - case LOG_CAMERA_ID_ECAMERA: { - set_thread_name("WideCameraEncoder"); - break; - } - default: { - LOGE("unexpected camera index provided"); - assert(false); - } - } + LogCameraInfo &cam_info = cameras_logged[cam_idx]; + RotateState &rotate_state = s.rotate_state[cam_idx]; - VisionStream stream; + set_thread_name(cam_info.filename); - bool encoder_inited = false; - EncoderState encoder; - EncoderState encoder_alt; - bool has_encoder_alt = cameras_logged[cam_idx].has_qcamera; - - int encoder_segment = -1; int cnt = 0; - pthread_mutex_lock(&s.rotate_lock); - int my_idx = s.num_encoder; - s.num_encoder += 1; - pthread_mutex_unlock(&s.rotate_lock); - LoggerHandle *lh = NULL; + std::vector encoders; + VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false); while (!do_exit) { - VisionStreamBufs buf_info; - int err = visionstream_init(&stream, cameras_logged[cam_idx].stream_type, false, &buf_info); - if (err != 0) { - LOGD("visionstream connect fail"); - usleep(100000); + if (!vipc_client.connect(false)){ + util::sleep_for(100); continue; } - if (!encoder_inited) { + // init encoders + if (encoders.empty()) { + VisionBuf buf_info = vipc_client.buffers[0]; LOGD("encoder init %dx%d", buf_info.width, buf_info.height); - encoder_init(&encoder, cameras_logged[cam_idx].filename, - buf_info.width, - buf_info.height, - cameras_logged[cam_idx].fps, - cameras_logged[cam_idx].bitrate, - cameras_logged[cam_idx].is_h265, - cameras_logged[cam_idx].downscale); - if (has_encoder_alt) { - encoder_init(&encoder_alt, cameras_logged[LOG_CAMERA_ID_QCAMERA].filename, - cameras_logged[LOG_CAMERA_ID_QCAMERA].frame_width, - cameras_logged[LOG_CAMERA_ID_QCAMERA].frame_height, - cameras_logged[LOG_CAMERA_ID_QCAMERA].fps, - cameras_logged[LOG_CAMERA_ID_QCAMERA].bitrate, - cameras_logged[LOG_CAMERA_ID_QCAMERA].is_h265, - cameras_logged[LOG_CAMERA_ID_QCAMERA].downscale); + // main encoder + encoders.push_back(new Encoder(cam_info.filename, buf_info.width, buf_info.height, + cam_info.fps, cam_info.bitrate, cam_info.is_h265, cam_info.downscale)); + + // qcamera encoder + if (cam_info.has_qcamera) { + LogCameraInfo &qcam_info = cameras_logged[LOG_CAMERA_ID_QCAMERA]; + encoders.push_back(new Encoder(qcam_info.filename, + qcam_info.frame_width, qcam_info.frame_height, + qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265, qcam_info.downscale)); } - - encoder_inited = true; } while (!do_exit) { - VIPCBufExtra extra; - VIPCBuf* buf = visionstream_get(&stream, &extra); - if (buf == NULL) { - LOG("visionstream get failed"); - break; + VisionIpcBufExtra extra; + VisionBuf* buf = vipc_client.recv(&extra); + if (buf == nullptr){ + continue; } - //uint64_t current_time = nanos_since_boot(); - //uint64_t diff = current_time - extra.timestamp_eof; - //double msdiff = (double) diff / 1000000.0; - // printf("logger latency to tsEof: %f\n", msdiff); - - { // all the rotation stuff + //printf("logger latency to tsEof: %f\n", (double)(nanos_since_boot() - extra.timestamp_eof) / 1000000.0); + // all the rotation stuff + { pthread_mutex_lock(&s.rotate_lock); pthread_mutex_unlock(&s.rotate_lock); // wait if camera pkt id is older than stream - rotate_state->waitLogThread(); + rotate_state.waitLogThread(); if (do_exit) break; // rotate the encoder if the logger is on a newer segment - if (rotate_state->should_rotate) { - if (!rotate_state->initialized) { - rotate_state->last_rotate_frame_id = extra.frame_id - 1; - rotate_state->initialized = true; + if (rotate_state.should_rotate) { + LOGW("camera %d rotate encoder to %s", cam_idx, s.segment_path); + + if (!rotate_state.initialized) { + rotate_state.last_rotate_frame_id = extra.frame_id - 1; + rotate_state.initialized = true; } - while (s.rotate_seq_id != my_idx && !do_exit) { usleep(1000); } - LOGW("camera %d rotate encoder to %s.", cam_idx, s.segment_path); - encoder_rotate(&encoder, s.segment_path, s.rotate_segment); - s.rotate_seq_id = (my_idx + 1) % s.num_encoder; - if (has_encoder_alt) { - encoder_rotate(&encoder_alt, s.segment_path, s.rotate_segment); - } - encoder_segment = s.rotate_segment; + + // get new logger handle for new segment if (lh) { lh_close(lh); } lh = logger_get_handle(&s.logger); - pthread_mutex_lock(&s.rotate_lock); - s.should_close += 1; - pthread_mutex_unlock(&s.rotate_lock); - - while(s.should_close > 0 && s.should_close < s.num_encoder && !do_exit) { usleep(1000); } - - pthread_mutex_lock(&s.rotate_lock); - s.should_close = s.should_close == s.num_encoder ? 1 - s.num_encoder : s.should_close + 1; - - encoder_close(&encoder); - encoder_open(&encoder, encoder.next_path); - encoder.segment = encoder.next_segment; - encoder.rotating = false; - if (has_encoder_alt) { - encoder_close(&encoder_alt); - encoder_open(&encoder_alt, encoder_alt.next_path); - encoder_alt.segment = encoder_alt.next_segment; - encoder_alt.rotating = false; + // wait for all to start rotating + rotate_state.rotating = true; + for(auto &r : s.rotate_state) { + while(r.enabled && !r.rotating && !do_exit) util::sleep_for(5); } - s.finish_close += 1; + + pthread_mutex_lock(&s.rotate_lock); + for (auto &e : encoders) { + e->encoder_close(); + e->encoder_open(s.segment_path); + } + rotate_state.cur_seg = s.rotate_segment; pthread_mutex_unlock(&s.rotate_lock); - while(s.finish_close > 0 && s.finish_close < s.num_encoder && !do_exit) { usleep(1000); } - s.finish_close = 0; - - rotate_state->finish_rotate(); + // wait for all to finish rotating + for(auto &r : s.rotate_state) { + while(r.enabled && r.cur_seg != s.rotate_segment && !do_exit) util::sleep_for(5); + } + rotate_state.rotating = false; + rotate_state.finish_rotate(); } } - rotate_state->setStreamFrameId(extra.frame_id); + rotate_state.setStreamFrameId(extra.frame_id); - uint8_t *y = (uint8_t*)buf->addr; - uint8_t *u = y + (buf_info.width*buf_info.height); - uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2); - { - // encode hevc - int out_segment = -1; - int out_id = encoder_encode_frame(&encoder, - y, u, v, - buf_info.width, buf_info.height, - &out_segment, &extra); - if (has_encoder_alt) { - int out_segment_alt = -1; - encoder_encode_frame(&encoder_alt, - y, u, v, - buf_info.width, buf_info.height, - &out_segment_alt, &extra); - } - - // publish encode index - MessageBuilder msg; - // this is really ugly - auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() : - (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx()); - eidx.setFrameId(extra.frame_id); - eidx.setTimestampSof(extra.timestamp_sof); - eidx.setTimestampEof(extra.timestamp_eof); - #ifdef QCOM2 - eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); - #else - eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT:cereal::EncodeIndex::Type::FULL_H_E_V_C); - #endif - - eidx.setEncodeId(cnt); - eidx.setSegmentNum(out_segment); - eidx.setSegmentId(out_id); - - auto bytes = msg.toBytes(); - if (lh) { - lh_log(lh, bytes.begin(), bytes.size(), false); + // encode a frame + for (int i = 0; i < encoders.size(); ++i) { + int out_id = encoders[i]->encode_frame(buf->y, buf->u, buf->v, + buf->width, buf->height, extra.timestamp_eof); + if (i == 0 && out_id != -1) { + // publish encode index + MessageBuilder msg; + // this is really ugly + auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initDriverEncodeIdx() : + (cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideRoadEncodeIdx() : msg.initEvent().initRoadEncodeIdx()); + eidx.setFrameId(extra.frame_id); + eidx.setTimestampSof(extra.timestamp_sof); + eidx.setTimestampEof(extra.timestamp_eof); + #ifdef QCOM2 + eidx.setType(cereal::EncodeIndex::Type::FULL_H_E_V_C); + #else + eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C); + #endif + eidx.setEncodeId(cnt); + eidx.setSegmentNum(rotate_state.cur_seg); + eidx.setSegmentId(out_id); + if (lh) { + auto bytes = msg.toBytes(); + lh_log(lh, bytes.begin(), bytes.size(), false); + } } } @@ -391,119 +301,16 @@ void encoder_thread(RotateState *rotate_state, int cam_idx) { lh_close(lh); lh = NULL; } - - visionstream_destroy(&stream); } - if (encoder_inited) { - LOG("encoder destroy"); - encoder_close(&encoder); - encoder_destroy(&encoder); - } - - if (has_encoder_alt) { - LOG("encoder alt destroy"); - encoder_close(&encoder_alt); - encoder_destroy(&encoder_alt); + LOG("encoder destroy"); + for(auto &e : encoders) { + e->encoder_close(); + delete e; } } -#endif -} - -void append_property(const char* key, const char* value, void *cookie) { - std::vector > *properties = - (std::vector > *)cookie; - - properties->push_back(std::make_pair(std::string(key), std::string(value))); -} - -kj::Array gen_init_data() { - MessageBuilder msg; - auto init = msg.initEvent().initInitData(); - - if (file_exists("/EON")) - init.setDeviceType(cereal::InitData::DeviceType::NEO); - else if (file_exists("/TICI")) { - init.setDeviceType(cereal::InitData::DeviceType::TICI); - } else { - init.setDeviceType(cereal::InitData::DeviceType::PC); - } - - init.setVersion(capnp::Text::Reader(COMMA_VERSION)); - - std::ifstream cmdline_stream("/proc/cmdline"); - std::vector kernel_args; - std::string buf; - while (cmdline_stream >> buf) { - kernel_args.push_back(buf); - } - - auto lkernel_args = init.initKernelArgs(kernel_args.size()); - for (int i=0; i > properties; - property_list(append_property, (void*)&properties); - - auto lentries = init.initAndroidProperties().initEntries(properties.size()); - for (int i=0; i git_commit = params.read_db_bytes("GitCommit"); - if (git_commit.size() > 0) { - init.setGitCommit(capnp::Text::Reader(git_commit.data(), git_commit.size())); - } - - std::vector git_branch = params.read_db_bytes("GitBranch"); - if (git_branch.size() > 0) { - init.setGitBranch(capnp::Text::Reader(git_branch.data(), git_branch.size())); - } - - std::vector git_remote = params.read_db_bytes("GitRemote"); - if (git_remote.size() > 0) { - init.setGitRemote(capnp::Text::Reader(git_remote.data(), git_remote.size())); - } - - init.setPassive(params.read_db_bool("Passive")); - { - // log params - std::map params_map; - params.read_db_all(¶ms_map); - auto lparams = init.initParams().initEntries(params_map.size()); - int i = 0; - for (auto& kv : params_map) { - auto lentry = lparams[i]; - lentry.setKey(kv.first); - lentry.setValue(kv.second); - i++; - } - } - return capnp::messageToFlatArray(msg); -} - -static int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) { +int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) { const char* dot = strrchr(fpath, '.'); if (dot && strcmp(dot, ".lock") == 0) { unlink(fpath); @@ -511,125 +318,67 @@ static int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupefla return 0; } -static void clear_locks() { - ftw(LOG_ROOT, clear_locks_fn, 16); +void clear_locks() { + ftw(LOG_ROOT.c_str(), clear_locks_fn, 16); } -static void bootlog() { - int err; - - { - auto words = gen_init_data(); - auto bytes = words.asBytes(); - logger_init(&s.logger, "bootlog", bytes.begin(), bytes.size(), false); - } - - err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); - assert(err == 0); - LOGW("bootlog to %s", s.segment_path); - - { - MessageBuilder msg; - auto boot = msg.initEvent().initBoot(); - - boot.setWallTimeNanos(nanos_since_epoch()); - - std::string lastKmsg = util::read_file("/sys/fs/pstore/console-ramoops"); - boot.setLastKmsg(capnp::Data::Reader((const kj::byte*)lastKmsg.data(), lastKmsg.size())); - - std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0"); - boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size())); - - std::string launchLog = util::read_file("/tmp/launch_log"); - boot.setLaunchLog(capnp::Text::Reader(launchLog.data(), launchLog.size())); - - auto bytes = msg.toBytes(); - logger_log(&s.logger, bytes.begin(), bytes.size(), false); - } - - logger_close(&s.logger); -} +} // namespace int main(int argc, char** argv) { - int err; -#ifdef QCOM setpriority(PRIO_PROCESS, 0, -12); -#endif - - if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) { - bootlog(); - return 0; - } - - int segment_length = SEGMENT_LENGTH; - if (getenv("LOGGERD_TEST")) { - segment_length = atoi(getenv("LOGGERD_SEGMENT_LENGTH")); - } - bool record_front = true; -#ifndef QCOM2 - record_front = Params().read_db_bool("RecordFront"); -#endif clear_locks(); - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); + // setup messaging + typedef struct QlogState { + int counter, freq; + } QlogState; + std::map qlog_states; s.ctx = Context::create(); Poller * poller = Poller::create(); - - // subscribe to all services - std::vector socks; - std::map qlog_counter; - std::map qlog_freqs; - + // subscribe to all socks for (const auto& it : services) { - std::string name = it.name; + if (!it.should_log) continue; - if (it.should_log) { - SubSocket * sock = SubSocket::create(s.ctx, name); - assert(sock != NULL); - poller->registerSocket(sock); - socks.push_back(sock); + SubSocket * sock = SubSocket::create(s.ctx, it.name); + assert(sock != NULL); + poller->registerSocket(sock); + socks.push_back(sock); - for (int cid=0;cid<=MAX_CAM_IDX;cid++) { - if (name == cameras_logged[cid].frame_packet_name) { s.rotate_state[cid].fpkt_sock = sock; } + for (int cid=0; cid<=MAX_CAM_IDX; cid++) { + if (std::string(it.name) == cameras_logged[cid].frame_packet_name) { + s.rotate_state[cid].fpkt_sock = sock; } - - qlog_counter[sock] = (it.decimation == -1) ? -1 : 0; - qlog_freqs[sock] = it.decimation; } + qlog_states[sock] = {.counter = 0, .freq = it.decimation}; } - { - auto words = gen_init_data(); - auto bytes = words.asBytes(); - logger_init(&s.logger, "rlog", bytes.begin(), bytes.size(), true); - } + // init logger + logger_init(&s.logger, "rlog", true); - s.rotate_seq_id = 0; - s.should_close = 0; - s.finish_close = 0; - s.num_encoder = 0; + // init encoders pthread_mutex_init(&s.rotate_lock, NULL); -#ifndef DISABLE_ENCODER - // rear camera - std::thread encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_FCAMERA], LOG_CAMERA_ID_FCAMERA); + + // TODO: create these threads dynamically on frame packet presence + std::vector encoder_threads; + encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_FCAMERA)); s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true; - // front camera - std::thread front_encoder_thread_handle; + +#if defined(QCOM) || defined(QCOM2) + bool record_front = Params().read_db_bool("RecordFront"); if (record_front) { - front_encoder_thread_handle = std::thread(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_DCAMERA], LOG_CAMERA_ID_DCAMERA); + encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_DCAMERA)); s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true; } - #ifdef QCOM2 - // wide camera - std::thread wide_encoder_thread_handle(encoder_thread, &s.rotate_state[LOG_CAMERA_ID_ECAMERA], LOG_CAMERA_ID_ECAMERA); + +#ifdef QCOM2 + encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_ECAMERA)); s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true; - #endif +#endif #endif uint64_t msg_count = 0; @@ -640,33 +389,42 @@ int main(int argc, char** argv) { double last_rotate_tms = millis_since_boot(); double last_camera_seen_tms = millis_since_boot(); while (!do_exit) { - for (auto sock : poller->poll(100 * 1000)) { - Message * last_msg = nullptr; - while (true) { + // TODO: fix msgs from the first poll getting dropped + // poll for new messages on all sockets + for (auto sock : poller->poll(1000)) { + + // drain socket + Message * last_msg = nullptr; + while (!do_exit) { Message * msg = sock->receive(true); - if (msg == NULL){ + if (!msg){ break; } delete last_msg; last_msg = msg; - logger_log(&s.logger, (uint8_t*)msg->getData(), msg->getSize(), qlog_counter[sock] == 0); - - if (qlog_counter[sock] != -1) { - //printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]); - qlog_counter[sock]++; - qlog_counter[sock] %= qlog_freqs[sock]; + QlogState& qs = qlog_states[sock]; + logger_log(&s.logger, (uint8_t*)msg->getData(), msg->getSize(), qs.counter == 0 && qs.freq != -1); + if (qs.freq != -1) { + qs.counter = (qs.counter + 1) % qs.freq; } + bytes_count += msg->getSize(); - msg_count++; + if ((++msg_count % 1000) == 0) { + double ts = seconds_since_boot(); + LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count * 1.0 / (ts - start_ts), bytes_count * 0.001 / (ts - start_ts)); + } } if (last_msg) { int fpkt_id = -1; - for (int cid=0;cid<=MAX_CAM_IDX;cid++) { - if (sock == s.rotate_state[cid].fpkt_sock) {fpkt_id=cid; break;} + for (int cid = 0; cid <=MAX_CAM_IDX; cid++) { + if (sock == s.rotate_state[cid].fpkt_sock) { + fpkt_id=cid; + break; + } } - if (fpkt_id>=0) { + if (fpkt_id >= 0) { // track camera frames to sync to encoder // only process last frame const uint8_t* data = (uint8_t*)last_msg->getData(); @@ -681,85 +439,71 @@ int main(int argc, char** argv) { cereal::Event::Reader event = cmsg.getRoot(); if (fpkt_id == LOG_CAMERA_ID_FCAMERA) { - s.rotate_state[fpkt_id].setLogFrameId(event.getFrame().getFrameId()); + s.rotate_state[fpkt_id].setLogFrameId(event.getRoadCameraState().getFrameId()); } else if (fpkt_id == LOG_CAMERA_ID_DCAMERA) { - s.rotate_state[fpkt_id].setLogFrameId(event.getFrontFrame().getFrameId()); + s.rotate_state[fpkt_id].setLogFrameId(event.getDriverCameraState().getFrameId()); } else if (fpkt_id == LOG_CAMERA_ID_ECAMERA) { - s.rotate_state[fpkt_id].setLogFrameId(event.getWideFrame().getFrameId()); + s.rotate_state[fpkt_id].setLogFrameId(event.getWideRoadCameraState().getFrameId()); } last_camera_seen_tms = millis_since_boot(); } - delete last_msg; } + delete last_msg; } - double ts = seconds_since_boot(); - double tms = millis_since_boot(); - - bool new_segment = false; - + bool new_segment = s.logger.part == -1; if (s.logger.part > -1) { - new_segment = true; - if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && s.num_encoder > 0) { - for (int cid=0;cid<=MAX_CAM_IDX;cid++) { + double tms = millis_since_boot(); + if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && encoder_threads.size() > 0) { + new_segment = true; + for (auto &r : s.rotate_state) { // this *should* be redundant on tici since all camera frames are synced - new_segment &= (((s.rotate_state[cid].stream_frame_id >= s.rotate_state[cid].last_rotate_frame_id + segment_length * MAIN_FPS) && - (!s.rotate_state[cid].should_rotate) && (s.rotate_state[cid].initialized)) || - (!s.rotate_state[cid].enabled)); + new_segment &= (((r.stream_frame_id >= r.last_rotate_frame_id + SEGMENT_LENGTH * MAIN_FPS) && + (!r.should_rotate) && (r.initialized)) || + (!r.enabled)); #ifndef QCOM2 break; // only look at fcamera frame id if not QCOM2 #endif } } else { - new_segment &= tms - last_rotate_tms > segment_length * 1000; - if (new_segment) { LOGW("no camera packet seen. auto rotated"); } + if (tms - last_rotate_tms > SEGMENT_LENGTH * 1000) { + new_segment = true; + LOGW("no camera packet seen. auto rotated"); + } } - } else if (s.logger.part == -1) { - // always starts first segment immediately - new_segment = true; } + // rotate to new segment if (new_segment) { pthread_mutex_lock(&s.rotate_lock); last_rotate_tms = millis_since_boot(); - err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment); + int err = logger_next(&s.logger, LOG_ROOT.c_str(), s.segment_path, sizeof(s.segment_path), &s.rotate_segment); assert(err == 0); - if (s.logger.part == 0) { LOGW("logging to %s", s.segment_path); } - LOGW("rotated to %s", s.segment_path); + LOGW((s.logger.part == 0) ? "logging to %s" : "rotated to %s", s.segment_path); - // rotate the encoders - for (int cid=0;cid<=MAX_CAM_IDX;cid++) { s.rotate_state[cid].rotate(); } + // rotate encoders + for (auto &r : s.rotate_state) r.rotate(); pthread_mutex_unlock(&s.rotate_lock); } - - if ((msg_count%1000) == 0) { - LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count*1.0/(ts-start_ts), bytes_count*0.001/(ts-start_ts)); - } } - LOGW("joining threads"); - for (int cid=0;cid<=MAX_CAM_IDX;cid++) { s.rotate_state[cid].cancelWait(); } - -#ifndef DISABLE_ENCODER -#ifdef QCOM2 - wide_encoder_thread_handle.join(); -#endif - if (record_front) { - front_encoder_thread_handle.join(); - } - encoder_thread_handle.join(); - LOGW("encoder joined"); -#endif + LOGW("closing encoders"); + for (auto &r : s.rotate_state) r.cancelWait(); + for (auto &t : encoder_threads) t.join(); + LOGW("closing logger"); logger_close(&s.logger); - LOGW("logger closed"); - for (auto s : socks){ - delete s; + if (do_exit.power_failure){ + LOGE("power failure"); + sync(); } + // messaging cleanup + for (auto sock : socks) delete sock; delete poller; delete s.ctx; + return 0; } diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc new file mode 100644 index 000000000..1496e82d2 --- /dev/null +++ b/selfdrive/loggerd/omx_encoder.cc @@ -0,0 +1,601 @@ +#pragma clang diagnostic ignored "-Wdeprecated-declarations" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "common/util.h" +#include "common/swaglog.h" + +#include "omx_encoder.h" + +// Check the OMX error code and assert if an error occurred. +#define OMX_CHECK(_expr) \ + do { \ + assert(OMX_ErrorNone == _expr); \ + } while (0) + +extern ExitHandler do_exit; + +// ***** OMX callback functions ***** + +void OmxEncoder::wait_for_state(OMX_STATETYPE state) { + std::unique_lock lk(this->state_lock); + while (this->state != state) { + this->state_cv.wait(lk); + } +} + +static OMX_CALLBACKTYPE omx_callbacks = { + .EventHandler = OmxEncoder::event_handler, + .EmptyBufferDone = OmxEncoder::empty_buffer_done, + .FillBufferDone = OmxEncoder::fill_buffer_done, +}; + +OMX_ERRORTYPE OmxEncoder::event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, + OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data) { + OmxEncoder *e = (OmxEncoder*)app_data; + if (event == OMX_EventCmdComplete) { + assert(data1 == OMX_CommandStateSet); + LOG("set state event 0x%x", data2); + { + std::unique_lock lk(e->state_lock); + e->state = (OMX_STATETYPE)data2; + } + e->state_cv.notify_all(); + } else if (event == OMX_EventError) { + LOGE("OMX error 0x%08x", data1); + } else { + LOGE("OMX unhandled event %d", event); + assert(false); + } + + return OMX_ErrorNone; +} + +OMX_ERRORTYPE OmxEncoder::empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { + // printf("empty_buffer_done\n"); + OmxEncoder *e = (OmxEncoder*)app_data; + e->free_in.push(buffer); + return OMX_ErrorNone; +} + +OMX_ERRORTYPE OmxEncoder::fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer) { + // printf("fill_buffer_done\n"); + OmxEncoder *e = (OmxEncoder*)app_data; + e->done_out.push(buffer); + return OMX_ErrorNone; +} + +#define PORT_INDEX_IN 0 +#define PORT_INDEX_OUT 1 + +static const char* omx_color_fomat_name(uint32_t format) __attribute__((unused)); +static const char* omx_color_fomat_name(uint32_t format) { + switch (format) { + case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused"; + case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome"; + case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332"; + case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444"; + case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444"; + case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555"; + case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565"; + case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565"; + case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666"; + case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665"; + case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666"; + case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888"; + case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888"; + case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887"; + case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888"; + case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888"; + case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888"; + case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar"; + case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar"; + case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar"; + case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar"; + case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar"; + case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar"; + case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar"; + case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar"; + case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr"; + case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb"; + case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY"; + case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY"; + case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved"; + case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit"; + case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit"; + case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed"; + case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2"; + case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4"; + case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8"; + case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16"; + case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24"; + case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32"; + case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar"; + case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar"; + case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666"; + case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666"; + case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666"; + + case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque"; + case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar"; + case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar"; + case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; + case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled"; + case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m"; + + // case QOMX_COLOR_FormatYVU420SemiPlanar: return "QOMX_COLOR_FormatYVU420SemiPlanar"; + case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka"; + case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka"; + // case QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka"; + // case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m"; + case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView"; + case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed"; + case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888"; + case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed"; + + default: + return "unkn"; + } +} + + +// ***** encoder functions ***** + +OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { + this->filename = filename; + this->width = width; + this->height = height; + this->fps = fps; + this->remuxing = !h265; + + this->downscale = downscale; + if (this->downscale) { + this->y_ptr2 = (uint8_t *)malloc(this->width*this->height); + this->u_ptr2 = (uint8_t *)malloc(this->width*this->height/4); + this->v_ptr2 = (uint8_t *)malloc(this->width*this->height/4); + } + + auto component = (OMX_STRING)(h265 ? "OMX.qcom.video.encoder.hevc" : "OMX.qcom.video.encoder.avc"); + int err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks); + if (err != OMX_ErrorNone) { + LOGE("error getting codec: %x", err); + } + assert(err == OMX_ErrorNone); + // printf("handle: %p\n", this->handle); + + // setup input port + + OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; + in_port.nSize = sizeof(in_port); + in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + + in_port.format.video.nFrameWidth = this->width; + in_port.format.video.nFrameHeight = this->height; + in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); + in_port.format.video.nSliceHeight = this->height; + // in_port.nBufferSize = (this->width * this->height * 3) / 2; + in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); + in_port.format.video.xFramerate = (this->fps * 65536); + in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused; + // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar; + in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; + + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &in_port)); + this->in_buf_headers.resize(in_port.nBufferCountActual); + + // setup output port + + OMX_PARAM_PORTDEFINITIONTYPE out_port = {0}; + out_port.nSize = sizeof(out_port); + out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR)&out_port)); + out_port.format.video.nFrameWidth = this->width; + out_port.format.video.nFrameHeight = this->height; + out_port.format.video.xFramerate = 0; + out_port.format.video.nBitrate = bitrate; + if (h265) { + out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC; + } else { + out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC; + } + out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; + + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port)); + + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port)); + this->out_buf_headers.resize(out_port.nBufferCountActual); + + OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0}; + bitrate_type.nSize = sizeof(bitrate_type); + bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); + bitrate_type.eControlRate = OMX_Video_ControlRateVariable; + bitrate_type.nTargetBitrate = bitrate; + + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); + + if (h265) { + // setup HEVC + #ifndef QCOM2 + OMX_VIDEO_PARAM_HEVCTYPE hevc_type = {0}; + OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc; + #else + OMX_VIDEO_PARAM_PROFILELEVELTYPE hevc_type = {0}; + OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent; + #endif + hevc_type.nSize = sizeof(hevc_type); + hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + OMX_CHECK(OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type)); + + hevc_type.eProfile = OMX_VIDEO_HEVCProfileMain; + hevc_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; + + OMX_CHECK(OMX_SetParameter(this->handle, index_type, (OMX_PTR) &hevc_type)); + } else { + // setup h264 + OMX_VIDEO_PARAM_AVCTYPE avc = { 0 }; + avc.nSize = sizeof(avc); + avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT; + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); + + avc.nBFrames = 0; + avc.nPFrames = 15; + + avc.eProfile = OMX_VIDEO_AVCProfileHigh; + avc.eLevel = OMX_VIDEO_AVCLevel31; + + avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; + avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; + + avc.nRefFrames = 1; + avc.bUseHadamard = OMX_TRUE; + avc.bEntropyCodingCABAC = OMX_TRUE; + avc.bWeightedPPrediction = OMX_TRUE; + avc.bconstIpred = OMX_TRUE; + + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); + } + + + // for (int i = 0; ; i++) { + // OMX_VIDEO_PARAM_PORTFORMATTYPE video_port_format = {0}; + // video_port_format.nSize = sizeof(video_port_format); + // video_port_format.nIndex = i; + // video_port_format.nPortIndex = PORT_INDEX_IN; + // if (OMX_GetParameter(this->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone) + // break; + // printf("in %d: compression 0x%x format 0x%x %s\n", i, + // video_port_format.eCompressionFormat, video_port_format.eColorFormat, + // omx_color_fomat_name(video_port_format.eColorFormat)); + // } + + // for (int i=0; i<32; i++) { + // OMX_VIDEO_PARAM_PROFILELEVELTYPE params = {0}; + // params.nSize = sizeof(params); + // params.nPortIndex = PORT_INDEX_OUT; + // params.nProfileIndex = i; + // if (OMX_GetParameter(this->handle, OMX_IndexParamVideoProfileLevelQuerySupported, ¶ms) != OMX_ErrorNone) + // break; + // printf("profile %d level 0x%x\n", params.eProfile, params.eLevel); + // } + + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); + + for (auto &buf : this->in_buf_headers) { + OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_IN, this, + in_port.nBufferSize)); + } + + for (auto &buf : this->out_buf_headers) { + OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_OUT, this, + out_port.nBufferSize)); + } + + wait_for_state(OMX_StateIdle); + + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL)); + + wait_for_state(OMX_StateExecuting); + + // give omx all the output buffers + for (auto &buf : this->out_buf_headers) { + // printf("fill %p\n", this->out_buf_headers[i]); + OMX_CHECK(OMX_FillThisBuffer(this->handle, buf)); + } + + // fill the input free queue + for (auto &buf : this->in_buf_headers) { + this->free_in.push(buf); + } +} + +void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) { + int err; + uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset; + + if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) { + if (e->codec_config_len < out_buf->nFilledLen) { + e->codec_config = (uint8_t *)realloc(e->codec_config, out_buf->nFilledLen); + } + e->codec_config_len = out_buf->nFilledLen; + memcpy(e->codec_config, buf_data, out_buf->nFilledLen); +#ifdef QCOM2 + out_buf->nTimeStamp = 0; +#endif + } + + if (e->of) { + //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags); + fwrite(buf_data, out_buf->nFilledLen, 1, e->of); + } + + if (e->remuxing) { + if (!e->wrote_codec_config && e->codec_config_len > 0) { + if (e->codec_ctx->extradata_size < e->codec_config_len) { + e->codec_ctx->extradata = (uint8_t *)realloc(e->codec_ctx->extradata, e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE); + } + e->codec_ctx->extradata_size = e->codec_config_len; + memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len); + memset(e->codec_ctx->extradata + e->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE); + + err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx); + assert(err >= 0); + err = avformat_write_header(e->ofmt_ctx, NULL); + assert(err >= 0); + + e->wrote_codec_config = true; + } + + if (out_buf->nTimeStamp > 0) { + // input timestamps are in microseconds + AVRational in_timebase = {1, 1000000}; + + AVPacket pkt; + av_init_packet(&pkt); + pkt.data = buf_data; + pkt.size = out_buf->nFilledLen; + + enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX); + pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, e->ofmt_ctx->streams[0]->time_base, rnd); + pkt.duration = av_rescale_q(50*1000, in_timebase, e->ofmt_ctx->streams[0]->time_base); + + if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) { + pkt.flags |= AV_PKT_FLAG_KEY; + } + + err = av_write_frame(e->ofmt_ctx, &pkt); + if (err < 0) { LOGW("ts encoder write issue"); } + + av_free_packet(&pkt); + } + } + + // give omx back the buffer +#ifdef QCOM2 + if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { + out_buf->nTimeStamp = 0; + } +#endif + OMX_CHECK(OMX_FillThisBuffer(e->handle, out_buf)); +} + +int OmxEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, uint64_t ts) { + int err; + if (!this->is_open) { + return -1; + } + + // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates... + // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this + //pthread_mutex_unlock(&this->lock); + OMX_BUFFERHEADERTYPE* in_buf = nullptr; + while (!this->free_in.try_pop(in_buf, 20)) { + if (do_exit) { + return -1; + } + } + + //pthread_mutex_lock(&this->lock); + + int ret = this->counter; + + uint8_t *in_buf_ptr = in_buf->pBuffer; + // printf("in_buf ptr %p\n", in_buf_ptr); + + uint8_t *in_y_ptr = in_buf_ptr; + int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, this->width); + int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, this->width); + // uint8_t *in_uv_ptr = in_buf_ptr + (this->width * this->height); + uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, this->height)); + + if (this->downscale) { + I420Scale(y_ptr, in_width, + u_ptr, in_width/2, + v_ptr, in_width/2, + in_width, in_height, + this->y_ptr2, this->width, + this->u_ptr2, this->width/2, + this->v_ptr2, this->width/2, + this->width, this->height, + libyuv::kFilterNone); + y_ptr = this->y_ptr2; + u_ptr = this->u_ptr2; + v_ptr = this->v_ptr2; + } + err = libyuv::I420ToNV12(y_ptr, this->width, + u_ptr, this->width/2, + v_ptr, this->width/2, + in_y_ptr, in_y_stride, + in_uv_ptr, in_uv_stride, + this->width, this->height); + assert(err == 0); + + // in_buf->nFilledLen = (this->width*this->height) + (this->width*this->height/2); + in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, this->width, this->height); + in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME; + in_buf->nOffset = 0; + in_buf->nTimeStamp = ts/1000LL; // OMX_TICKS, in microseconds + this->last_t = in_buf->nTimeStamp; + + OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); + + // pump output + while (true) { + OMX_BUFFERHEADERTYPE *out_buf; + if (!this->done_out.try_pop(out_buf)) { + break; + } + handle_out_buf(this, out_buf); + } + + this->dirty = true; + + this->counter++; + + return ret; +} + +void OmxEncoder::encoder_open(const char* path) { + int err; + + snprintf(this->vid_path, sizeof(this->vid_path), "%s/%s", path, this->filename); + LOGD("encoder_open %s remuxing:%d", this->vid_path, this->remuxing); + + if (this->remuxing) { + avformat_alloc_output_context2(&this->ofmt_ctx, NULL, NULL, this->vid_path); + assert(this->ofmt_ctx); + + this->out_stream = avformat_new_stream(this->ofmt_ctx, NULL); + assert(this->out_stream); + + // set codec correctly + av_register_all(); + + AVCodec *codec = NULL; + codec = avcodec_find_encoder(AV_CODEC_ID_H264); + assert(codec); + + this->codec_ctx = avcodec_alloc_context3(codec); + assert(this->codec_ctx); + this->codec_ctx->width = this->width; + this->codec_ctx->height = this->height; + this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P; + this->codec_ctx->time_base = (AVRational){ 1, this->fps }; + + err = avio_open(&this->ofmt_ctx->pb, this->vid_path, AVIO_FLAG_WRITE); + assert(err >= 0); + + this->wrote_codec_config = false; + } else { + this->of = fopen(this->vid_path, "wb"); + assert(this->of); +#ifndef QCOM2 + if (this->codec_config_len > 0) { + fwrite(this->codec_config, this->codec_config_len, 1, this->of); + } +#endif + } + + // create camera lock file + snprintf(this->lock_path, sizeof(this->lock_path), "%s/%s.lock", path, this->filename); + int lock_fd = open(this->lock_path, O_RDWR | O_CREAT, 0777); + assert(lock_fd >= 0); + close(lock_fd); + + this->is_open = true; + this->counter = 0; +} + +void OmxEncoder::encoder_close() { + if (this->is_open) { + if (this->dirty) { + // drain output only if there could be frames in the encoder + + OMX_BUFFERHEADERTYPE* in_buf = this->free_in.pop(); + in_buf->nFilledLen = 0; + in_buf->nOffset = 0; + in_buf->nFlags = OMX_BUFFERFLAG_EOS; + in_buf->nTimeStamp = this->last_t + 1000000LL/this->fps; + + OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); + + while (true) { + OMX_BUFFERHEADERTYPE *out_buf = this->done_out.pop(); + + handle_out_buf(this, out_buf); + + if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) { + break; + } + } + this->dirty = false; + } + + if (this->remuxing) { + av_write_trailer(this->ofmt_ctx); + avcodec_free_context(&this->codec_ctx); + avio_closep(&this->ofmt_ctx->pb); + avformat_free_context(this->ofmt_ctx); + } else { + fclose(this->of); + this->of = nullptr; + } + unlink(this->lock_path); + } + this->is_open = false; +} + +OmxEncoder::~OmxEncoder() { + assert(!this->is_open); + + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); + + wait_for_state(OMX_StateIdle); + + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL)); + + for (auto &buf : this->in_buf_headers) { + OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_IN, buf)); + } + + for (auto &buf : this->out_buf_headers) { + OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, buf)); + } + + wait_for_state(OMX_StateLoaded); + + OMX_CHECK(OMX_FreeHandle(this->handle)); + + OMX_BUFFERHEADERTYPE *out_buf; + while (this->free_in.try_pop(out_buf)); + while (this->done_out.try_pop(out_buf)); + + if (this->codec_config) { + free(this->codec_config); + } + + if (this->downscale) { + free(this->y_ptr2); + free(this->u_ptr2); + free(this->v_ptr2); + } +} diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h new file mode 100644 index 000000000..08bb30ec3 --- /dev/null +++ b/selfdrive/loggerd/omx_encoder.h @@ -0,0 +1,73 @@ +#pragma once + +#include +#include +#include +#include +#include + +extern "C" { + #include +} + +#include "encoder.h" +#include "common/queue.h" + +// OmxEncoder, lossey codec using hardware hevc +class OmxEncoder : public VideoEncoder { +public: + OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale); + ~OmxEncoder(); + int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, uint64_t ts); + void encoder_open(const char* path); + void encoder_close(); + + // OMX callbacks + static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event, + OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data); + static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer); + static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data, + OMX_BUFFERHEADERTYPE *buffer); + +private: + void wait_for_state(OMX_STATETYPE state); + static void handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf); + + int width, height, fps; + char vid_path[1024]; + char lock_path[1024]; + bool is_open = false; + bool dirty = false; + int counter = 0; + + const char* filename; + FILE *of; + + size_t codec_config_len; + uint8_t *codec_config = NULL; + bool wrote_codec_config; + + std::mutex state_lock; + std::condition_variable state_cv; + OMX_STATETYPE state = OMX_StateLoaded; + + OMX_HANDLETYPE handle; + + std::vector in_buf_headers; + std::vector out_buf_headers; + + uint64_t last_t; + + SafeQueue free_in; + SafeQueue done_out; + + AVFormatContext *ofmt_ctx; + AVCodecContext *codec_ctx; + AVStream *out_stream; + bool remuxing; + + bool downscale; + uint8_t *y_ptr2, *u_ptr2, *v_ptr2; +}; diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc index d9fc289de..71703c1ff 100644 --- a/selfdrive/loggerd/raw_logger.cc +++ b/selfdrive/loggerd/raw_logger.cc @@ -16,15 +16,14 @@ extern "C" { } #include "common/swaglog.h" -#include "common/utilpp.h" +#include "common/util.h" #include "raw_logger.h" -RawLogger::RawLogger(const std::string &afilename, int awidth, int aheight, int afps) - : filename(afilename), - width(awidth), - height(aheight), - fps(afps) { +RawLogger::RawLogger(const char* filename, int width, int height, int fps, + int bitrate, bool h265, bool downscale) + : filename(filename), + fps(fps) { int err = 0; @@ -65,15 +64,15 @@ RawLogger::~RawLogger() { av_free(codec_ctx); } -void RawLogger::Open(const std::string &path) { +void RawLogger::encoder_open(const char* path) { int err = 0; std::lock_guard guard(lock); - vid_path = util::string_format("%s/%s.mkv", path.c_str(), filename.c_str()); + vid_path = util::string_format("%s/%s.mkv", path, filename); // create camera lock file - lock_path = util::string_format("%s/%s.lock", path.c_str(), filename.c_str()); + lock_path = util::string_format("%s/%s.lock", path, filename); LOG("open %s\n", lock_path.c_str()); @@ -105,7 +104,7 @@ void RawLogger::Open(const std::string &path) { counter = 0; } -void RawLogger::Close() { +void RawLogger::encoder_close() { int err = 0; std::lock_guard guard(lock); @@ -127,7 +126,8 @@ void RawLogger::Close() { is_open = false; } -int RawLogger::ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) { +int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, uint64_t ts) { int err = 0; AVPacket pkt; @@ -148,7 +148,6 @@ int RawLogger::ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ LOGE("encoding error\n"); ret = -1; } else if (got_output) { - av_packet_rescale_ts(&pkt, codec_ctx->time_base, stream->time_base); pkt.stream_index = 0; diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h index 249be40db..28bfdf906 100644 --- a/selfdrive/loggerd/raw_logger.h +++ b/selfdrive/loggerd/raw_logger.h @@ -1,5 +1,4 @@ -#ifndef FFV1LOGGER_H -#define FFV1LOGGER_H +#pragma once #include #include @@ -15,21 +14,27 @@ extern "C" { #include } -#include "frame_logger.h" +#include "encoder.h" -class RawLogger : public FrameLogger { +class RawLogger : public VideoEncoder { public: - RawLogger(const std::string &filename, int awidth, int aheight, int afps); + RawLogger(const char* filename, int width, int height, int fps, + int bitrate, bool h265, bool downscale); ~RawLogger(); - - int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr); - void Open(const std::string &path); - void Close(); + int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, + int in_width, int in_height, uint64_t ts); + void encoder_open(const char* path); + void encoder_close(); private: - std::string filename; - int width, height, fps; + const char* filename; + int fps; int counter = 0; + bool is_open = false; + + std::string vid_path, lock_path; + + std::recursive_mutex lock; AVCodec *codec = NULL; AVCodecContext *codec_ctx = NULL; @@ -39,5 +44,3 @@ private: AVFrame *frame = NULL; }; - -#endif diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index d2cbb94ee..8fdb2a77a 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -1,6 +1,4 @@ #!/usr/bin/env python3 -import ctypes -import inspect import json import os import random @@ -10,42 +8,22 @@ import time import traceback from cereal import log +import cereal.messaging as messaging from common.api import Api from common.params import Params -from selfdrive.hardware import HARDWARE from selfdrive.loggerd.xattr_cache import getxattr, setxattr from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog -NetworkType = log.ThermalData.NetworkType +NetworkType = log.DeviceState.NetworkType UPLOAD_ATTR_NAME = 'user.upload' UPLOAD_ATTR_VALUE = b'1' +allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1")) +force_wifi = os.getenv("FORCEWIFI") is not None fake_upload = os.getenv("FAKEUPLOAD") is not None -def raise_on_thread(t, exctype): - '''Raises an exception in the threads with id tid''' - for ctid, tobj in threading._active.items(): - if tobj is t: - tid = ctid - break - else: - raise Exception("Could not find thread") - - if not inspect.isclass(exctype): - raise TypeError("Only types can be raised (not instances)") - - res = ctypes.pythonapi.PyThreadState_SetAsyncExc(ctypes.c_long(tid), - ctypes.py_object(exctype)) - if res == 0: - raise ValueError("invalid thread id") - elif res != 1: - # "if it returns a number greater than one, you're in trouble, - # and you should call it again with exc=NULL to revert the effect" - ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, 0) - raise SystemError("PyThreadState_SetAsyncExc failed") - def get_directory_sort(d): return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1))) @@ -68,8 +46,6 @@ def clear_locks(root): except OSError: cloudlog.exception("clear_locks failed") -def is_on_wifi(): - return HARDWARE.get_network_type() == NetworkType.wifi class Uploader(): def __init__(self, dongle_id, root): @@ -82,6 +58,7 @@ class Uploader(): self.last_resp = None self.last_exc = None + self.immediate_folders = ["crash/", "boot/"] self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1} self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2, "ecamera.hevc": 3} @@ -122,7 +99,7 @@ class Uploader(): # try to upload qlog files first for name, key, fn in upload_files: - if name in self.immediate_priority: + if name in self.immediate_priority or any(f in fn for f in self.immediate_folders): return (key, fn) if with_raw: @@ -221,21 +198,20 @@ def uploader_fn(exit_event): cloudlog.info("uploader missing dongle_id") raise Exception("uploader can't start without dongle id") + sm = messaging.SubMaster(['deviceState']) uploader = Uploader(dongle_id, ROOT) backoff = 0.1 - counter = 0 - on_wifi = False while not exit_event.is_set(): + sm.update(0) + on_wifi = force_wifi or sm['deviceState'].networkType == NetworkType.wifi offroad = params.get("IsOffroad") == b'1' - allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") and offroad - if offroad and counter % 12 == 0: - on_wifi = is_on_wifi() - counter += 1 + allow_raw_upload = params.get("IsUploadRawEnabled") != b"0" d = uploader.next_file_to_upload(with_raw=allow_raw_upload and on_wifi and offroad) if d is None: # Nothing to upload - time.sleep(60 if offroad else 5) + if allow_sleep: + time.sleep(60 if offroad else 5) continue key, fn = d @@ -245,7 +221,7 @@ def uploader_fn(exit_event): success = uploader.upload(key, fn) if success: backoff = 0.1 - else: + elif allow_sleep: cloudlog.info("backoff %r", backoff) time.sleep(backoff + random.uniform(0, backoff)) backoff = min(backoff*2, 120) diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index 91dcbb7e0..953f3d477 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -19,8 +19,6 @@ def main(): dat = b''.join(sock.recv_multipart()) dat = dat.decode('utf8') - # print "RECV", repr(dat) - levelnum = ord(dat[0]) dat = dat[1:] diff --git a/selfdrive/manager.py b/selfdrive/manager.py index e0fd94291..1e3d9d688 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -13,18 +13,22 @@ import time import traceback from multiprocessing import Process -from typing import Dict, List +from typing import Dict from common.basedir import BASEDIR from common.spinner import Spinner from common.text_window import TextWindow -from selfdrive.hardware import HARDWARE, EON, PC +import selfdrive.crash as crash +from selfdrive.hardware import HARDWARE, EON, PC, TICI +from selfdrive.hardware.eon.apk import update_apks, pm_apply_packages, start_offroad from selfdrive.swaglog import cloudlog, add_logentries_handler +from selfdrive.version import version, dirty os.environ['BASEDIR'] = BASEDIR sys.path.append(os.path.join(BASEDIR, "pyextra")) -TOTAL_SCONS_NODES = 1040 +TOTAL_SCONS_NODES = 1225 +MAX_BUILD_PROGRESS = 70 WEBCAM = os.getenv("WEBCAM") is not None PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) @@ -61,26 +65,24 @@ def unblock_stdout(): exit_status = os.wait()[1] >> 8 os._exit(exit_status) - if __name__ == "__main__": unblock_stdout() -# Run scons +# Start spinner spinner = Spinner() -spinner.update("0") +spinner.update_progress(0, 100) if __name__ != "__main__": spinner.close() def build(): - for retry in [True, False]: - # run scons - env = os.environ.copy() - env['SCONS_PROGRESS'] = "1" - env['SCONS_CACHE'] = "1" + env = os.environ.copy() + env['SCONS_PROGRESS'] = "1" + env['SCONS_CACHE'] = "1" + nproc = os.cpu_count() + j_flag = "" if nproc is None else f"-j{nproc - 1}" - nproc = os.cpu_count() - j_flag = "" if nproc is None else f"-j{nproc - 1}" + for retry in [True, False]: scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) compile_output = [] @@ -88,7 +90,7 @@ def build(): # Read progress from stderr and update spinner while scons.poll() is None: try: - line = scons.stderr.readline() # type: ignore + line = scons.stderr.readline() if line is None: continue line = line.rstrip() @@ -96,7 +98,7 @@ def build(): prefix = b'progress: ' if line.startswith(prefix): i = int(line[len(prefix):]) - spinner.update("%d" % (70.0 * (i / TOTAL_SCONS_NODES))) + spinner.update_progress(MAX_BUILD_PROGRESS * min(1., i / TOTAL_SCONS_NODES), 100.) elif len(line): compile_output.append(line) print(line.decode('utf8', 'replace')) @@ -105,10 +107,10 @@ def build(): if scons.returncode != 0: # Read remaining output - r = scons.stderr.read().split(b'\n') # type: ignore + r = scons.stderr.read().split(b'\n') compile_output += r - if retry: + if retry and (not dirty): if not os.getenv("CI"): print("scons build failed, cleaning in") for i in range(3, -1, -1): @@ -141,14 +143,11 @@ if __name__ == "__main__" and not PREBUILT: build() import cereal.messaging as messaging +from cereal import log from common.params import Params -import selfdrive.crash as crash from selfdrive.registration import register -from selfdrive.version import version, dirty -from selfdrive.loggerd.config import ROOT from selfdrive.launcher import launcher -from selfdrive.hardware.eon.apk import update_apks, pm_apply_packages, start_offroad # comment out anything you don't want to run @@ -167,7 +166,6 @@ managed_processes = { "tombstoned": "selfdrive.tombstoned", "logcatd": ("selfdrive/logcatd", ["./logcatd"]), "proclogd": ("selfdrive/proclogd", ["./proclogd"]), - "boardd": ("selfdrive/boardd", ["./boardd"]), # not used directly "pandad": "selfdrive.pandad", "ui": ("selfdrive/ui", ["./ui"]), "calibrationd": "selfdrive.locationd.calibrationd", @@ -175,7 +173,6 @@ managed_processes = { "camerad": ("selfdrive/camerad", ["./camerad"]), "sensord": ("selfdrive/sensord", ["./sensord"]), "clocksd": ("selfdrive/clocksd", ["./clocksd"]), - "gpsd": ("selfdrive/sensord", ["./gpsd"]), "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), @@ -193,13 +190,15 @@ def get_running(): # due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption unkillable_processes = ['camerad'] -# processes to end with SIGINT instead of SIGTERM -interrupt_processes: List[str] = [] - # processes to end with SIGKILL instead of SIGTERM -kill_processes = ['sensord'] +kill_processes = [] +if EON: + kill_processes += [ + 'sensord', + ] persistent_processes = [ + 'pandad', 'thermald', 'logmessaged', 'ui', @@ -210,11 +209,18 @@ persistent_processes = [ if not PC: persistent_processes += [ 'updated', - 'logcatd', 'tombstoned', + ] + +if EON: + persistent_processes += [ 'sensord', ] +if TICI: + managed_processes["timezoned"] = "selfdrive.timezoned" + persistent_processes += ['timezoned'] + car_started_processes = [ 'controlsd', 'plannerd', @@ -227,6 +233,7 @@ car_started_processes = [ 'proclogd', 'locationd', 'clocksd', + 'logcatd', ] driver_view_processes = [ @@ -244,10 +251,12 @@ if not PC or WEBCAM: if EON: car_started_processes += [ - 'gpsd', 'rtshield', ] - +else: + car_started_processes += [ + 'sensord', + ] def register_managed_process(name, desc, car_started=False): global managed_processes, car_started_processes, persistent_processes @@ -261,10 +270,6 @@ def register_managed_process(name, desc, car_started=False): def nativelauncher(pargs, cwd): # exec the process os.chdir(cwd) - - # because when extracted from pex zips permissions get lost -_- - os.chmod(pargs[0], 0o700) - os.execvp(pargs[0], pargs) def start_managed_process(name): @@ -332,22 +337,21 @@ def join_process(process, timeout): time.sleep(0.001) -def kill_managed_process(name): +def kill_managed_process(name, retry=True): if name not in running or name not in managed_processes: return - cloudlog.info("killing %s" % name) + cloudlog.info(f"killing {name}") if running[name].exitcode is None: - if name in interrupt_processes: - os.kill(running[name].pid, signal.SIGINT) - elif name in kill_processes: - os.kill(running[name].pid, signal.SIGKILL) - else: - running[name].terminate() + sig = signal.SIGKILL if name in kill_processes else signal.SIGINT + os.kill(running[name].pid, sig) join_process(running[name], 5) if running[name].exitcode is None: + if not retry: + raise Exception(f"{name} failed to die") + if name in unkillable_processes: cloudlog.critical("unkillable process %s failed to exit! rebooting in 15 if it doesn't die" % name) join_process(running[name], 15) @@ -362,8 +366,10 @@ def kill_managed_process(name): os.kill(running[name].pid, signal.SIGKILL) running[name].join() - cloudlog.info("%s is dead with %d" % (name, running[name].exitcode)) + ret = running[name].exitcode + cloudlog.info(f"{name} is dead with {ret}") del running[name] + return ret def cleanup_all_processes(signal, frame): @@ -389,6 +395,8 @@ def send_managed_process_signal(name, sig): # ****************** run loop ****************** def manager_init(): + os.umask(0) # Make sure we can create files with 777 permissions + # Create folders needed for msgq try: os.mkdir("/dev/shm") @@ -398,7 +406,7 @@ def manager_init(): print("WARNING: failed to make /dev/shm") # set dongle id - reg_res = register() + reg_res = register(spinner) if reg_res: dongle_id = reg_res else: @@ -408,15 +416,10 @@ def manager_init(): if not dirty: os.environ['CLEAN'] = '1' - cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=True) + cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, + device=HARDWARE.get_device_type()) crash.bind_user(id=dongle_id) - crash.bind_extra(version=version, dirty=dirty, is_eon=True) - - os.umask(0) - try: - os.mkdir(ROOT, 0o777) - except OSError: - pass + crash.bind_extra(version=version, dirty=dirty, device=HARDWARE.get_device_type()) # ensure shared libraries are readable by apks if EON: @@ -431,7 +434,7 @@ def manager_thread(): cloudlog.info({"environ": os.environ}) # save boot log - subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) + subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd")) # start daemon processes for p in daemon_processes: @@ -446,8 +449,8 @@ def manager_thread(): pm_apply_packages('enable') start_offroad() - if os.getenv("NOBOARD") is None: - start_managed_process("pandad") + if os.getenv("NOBOARD") is not None: + del managed_processes["pandad"] if os.getenv("BLOCK") is not None: for k in os.getenv("BLOCK").split(","): @@ -456,15 +459,16 @@ def manager_thread(): started_prev = False logger_dead = False params = Params() - thermal_sock = messaging.sub_sock('thermal') + device_state_sock = messaging.sub_sock('deviceState') + pm = messaging.PubMaster(['managerState']) while 1: - msg = messaging.recv_sock(thermal_sock, wait=True) + msg = messaging.recv_sock(device_state_sock, wait=True) - if msg.thermal.freeSpace < 0.05: + if msg.deviceState.freeSpacePercent < 5: logger_dead = True - if msg.thermal.started: + if msg.deviceState.started: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) @@ -490,12 +494,26 @@ def manager_thread(): os.sync() send_managed_process_signal("updated", signal.SIGHUP) - started_prev = msg.thermal.started + started_prev = msg.deviceState.started # check the status of all processes, did any of them die? running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running] cloudlog.debug(' '.join(running_list)) + # send managerState + states = [] + for p in managed_processes: + state = log.ManagerState.ProcessState.new_message() + state.name = p + if p in running: + state.running = running[p].is_alive() + state.pid = running[p].pid + state.exitCode = running[p].exitcode or 0 + states.append(state) + msg = messaging.new_message('managerState') + msg.managerState.processes = states + pm.send('managerState', msg) + # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break @@ -504,12 +522,11 @@ def manager_prepare(): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) - # Spinner has to start from 70 here - total = 100.0 if PREBUILT else 30.0 + total = 100.0 - (0 if PREBUILT else MAX_BUILD_PROGRESS) for i, p in enumerate(managed_processes): perc = (100.0 - total) + total * (i + 1) / len(managed_processes) - spinner.update(str(int(perc))) + spinner.update_progress(perc, 100.) prepare_managed_process(p) def main(): @@ -528,6 +545,7 @@ def main(): ("IsLdwEnabled", "1"), ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), + ("VisionRadarToggle", "0"), ("LaneChangeEnabled", "1"), ("IsDriverViewEnabled", "0"), ] diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 4fcded106..44a2fc7f2 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -3,6 +3,15 @@ lenv = env.Clone() libs = [cereal, messaging, common, 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] +def get_dlsym_offset(): + """Returns the offset between dlopen and dlsym in libdl.so""" + import ctypes + libdl = ctypes.PyDLL('libdl.so') + dlopen = ctypes.cast(libdl.dlopen, ctypes.c_void_p).value + dlsym = ctypes.cast(libdl.dlsym, ctypes.c_void_p).value + return dlsym - dlopen + + common_src = [ "models/commonmodel.cc", "runners/snpemodel.cc", @@ -10,16 +19,20 @@ common_src = [ "transforms/transform.cc" ] -if arch == "aarch64": - libs += ['gsl', 'CB', 'gnustl_shared'] - common_src += ["thneed/thneed.cc"] - lenv['CFLAGS'].append("-DUSE_THNEED") - lenv['CXXFLAGS'].append("-DUSE_THNEED") -elif arch == "larch64": - libs += ['gsl', 'CB', 'pthread', 'dl'] - common_src += ["thneed/thneed.cc"] - lenv['CFLAGS'].append("-DUSE_THNEED") +thneed_src = [ + "thneed/thneed.cc", + "thneed/serialize.cc", + "runners/thneedmodel.cc", +] + +if arch == "aarch64" or arch == "larch64": + libs += ['gsl', 'CB'] + libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl'] + + common_src += thneed_src + dlsym_offset = get_dlsym_offset() lenv['CXXFLAGS'].append("-DUSE_THNEED") + lenv['CXXFLAGS'].append(f"-DDLSYM_OFFSET={dlsym_offset}") else: libs += ['pthread'] @@ -40,15 +53,25 @@ else: del libs[libs.index('symphony-cpu')] del common_src[common_src.index('runners/snpemodel.cc')] -common = lenv.Object(common_src) +common_model = lenv.Object(common_src) + + +# build thneed model +if arch == "aarch64" or arch == "larch64": + compiler = lenv.Program('thneed/compile', ["thneed/compile.cc" ]+common_model, LIBS=libs) + cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} ../../models/supercombo.dlc ../../models/supercombo.thneed --binary" + + lib_paths = ':'.join([Dir(p).abspath for p in lenv["LIBPATH"]]) + cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}"}) + cenv.Command("../../models/supercombo.thneed", ["../../models/supercombo.dlc", compiler], cmd) lenv.Program('_dmonitoringmodeld', [ "dmonitoringmodeld.cc", "models/dmonitoring.cc", - ]+common, LIBS=libs) + ]+common_model, LIBS=libs) lenv.Program('_modeld', [ "modeld.cc", "models/driving.cc", - ]+common, LIBS=libs) + ]+common_model, LIBS=libs) diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 80af643d0..48b16980b 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -1,13 +1,13 @@ #include #include #include -#include #include #include -#include "common/visionbuf.h" -#include "common/visionipc.h" +#include "visionbuf.h" +#include "visionipc_client.h" #include "common/swaglog.h" +#include "common/util.h" #include "models/dmonitoring.h" @@ -16,59 +16,47 @@ #endif -volatile sig_atomic_t do_exit = 0; - -static void set_do_exit(int sig) { - do_exit = 1; -} +ExitHandler do_exit; int main(int argc, char **argv) { - int err; setpriority(PRIO_PROCESS, 0, -15); - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); - PubMaster pm({"driverState"}); // init the models DMonitoringModelState dmonitoringmodel; dmonitoring_init(&dmonitoringmodel); - // loop - VisionStream stream; - while (!do_exit) { - VisionStreamBufs buf_info; - err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, true, &buf_info); - if (err) { - printf("visionstream connect fail\n"); - usleep(100000); + VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true); + while (!do_exit){ + if (!vipc_client.connect(false)){ + util::sleep_for(100); continue; } - LOGW("connected with buffer size: %d", buf_info.buf_len); + break; + } + + while (!do_exit) { + LOGW("connected with buffer size: %d", vipc_client.buffers[0].len); double last = 0; while (!do_exit) { - VIPCBuf *buf; - VIPCBufExtra extra; - buf = visionstream_get(&stream, &extra); - if (buf == NULL) { - printf("visionstream get failed\n"); - break; + VisionIpcBufExtra extra = {0}; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr){ + continue; } double t1 = millis_since_boot(); - DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf_info.width, buf_info.height); + DMonitoringResult res = dmonitoring_eval_frame(&dmonitoringmodel, buf->addr, buf->width, buf->height); double t2 = millis_since_boot(); // send dm packet - const float* raw_pred_ptr = send_raw_pred ? (const float *)dmonitoringmodel.output : nullptr; - dmonitoring_publish(pm, extra.frame_id, res, raw_pred_ptr, (t2-t1)/1000.0); + dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0, dmonitoringmodel.output); LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); last = t1; } - visionstream_destroy(&stream); } dmonitoring_free(&dmonitoringmodel); diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 2febb42ad..c43e9588b 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -1,22 +1,18 @@ #include #include -#include #include #include -#include "common/visionbuf.h" -#include "common/visionipc.h" +#include "visionbuf.h" +#include "visionipc_client.h" #include "common/swaglog.h" #include "common/clutil.h" +#include "common/util.h" #include "models/driving.h" #include "messaging.hpp" -volatile sig_atomic_t do_exit = 0; - -static void set_do_exit(int sig) { - do_exit = 1; -} +ExitHandler do_exit; // globals bool run_model; mat3 cur_transform; @@ -102,9 +98,6 @@ int main(int argc, char **argv) { set_core_affinity(4); #endif - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); - pthread_mutex_init(&transform_lock, NULL); // start calibration thread @@ -113,8 +106,8 @@ int main(int argc, char **argv) { assert(err == 0); // messaging - PubMaster pm({"modelV2", "model", "cameraOdometry"}); - SubMaster sm({"pathPlan", "frame"}); + PubMaster pm({"modelV2", "cameraOdometry"}); + SubMaster sm({"lateralPlan", "roadCameraState"}); // cl init cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); @@ -125,17 +118,20 @@ int main(int argc, char **argv) { model_init(&model, device_id, context); LOGW("models loaded, modeld starting"); - // loop - VisionStream stream; - while (!do_exit) { - VisionStreamBufs buf_info; - err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); - if (err) { - LOGW("visionstream connect failed"); - usleep(100000); + VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_BACK, true, device_id, context); + + while (!do_exit){ + if (!vipc_client.connect(false)){ + util::sleep_for(100); continue; } - LOGW("connected with buffer size: %d", buf_info.buf_len); + break; + } + + // loop + while (!do_exit) { + VisionBuf *b = &vipc_client.buffers[0]; + LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height); // setup filter to track dropped frames const float dt = 1. / MODEL_FREQ; @@ -143,20 +139,16 @@ int main(int argc, char **argv) { const float frame_filter_k = (dt / ts) / (1. + dt / ts); float frames_dropped = 0; - // one frame in memory - VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context); - uint32_t frame_id = 0, last_vipc_frame_id = 0; double last = 0; int desire = -1; uint32_t run_count = 0; + while (!do_exit) { - VIPCBuf *buf; - VIPCBufExtra extra; - buf = visionstream_get(&stream, &extra); - if (buf == NULL) { - LOGW("visionstream get failed"); - break; + VisionIpcBufExtra extra; + VisionBuf *buf = vipc_client.recv(&extra); + if (buf == nullptr){ + continue; } pthread_mutex_lock(&transform_lock); @@ -166,8 +158,8 @@ int main(int argc, char **argv) { if (sm.update(0) > 0){ // TODO: path planner timeout? - desire = ((int)sm["pathPlan"].getPathPlan().getDesire()); - frame_id = sm["frame"].getFrame().getFrameId(); + desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire()); + frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId(); } double mt1 = 0, mt2 = 0; @@ -181,12 +173,8 @@ int main(int argc, char **argv) { mt1 = millis_since_boot(); - // TODO: don't make copies! - memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); - visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE); - ModelDataRaw model_buf = - model_eval_frame(&model, yuv_ion.buf_cl, buf_info.width, buf_info.height, + model_eval_frame(&model, buf->buf_cl, buf->width, buf->height, model_transform, vec_desire); mt2 = millis_since_boot(); float model_execution_time = (mt2 - mt1) / 1000.0; @@ -197,18 +185,16 @@ int main(int argc, char **argv) { if (run_count < 10) frames_dropped = 0; // let frame drops warm up float frame_drop_ratio = frames_dropped / (1 + frames_dropped); - const float *raw_pred_ptr = send_raw_pred ? &model.output[0] : nullptr; - model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); + model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time, + kj::ArrayPtr(model.output.data(), model.output.size())); posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof); - LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); last = mt1; last_vipc_frame_id = extra.frame_id; } } - visionbuf_free(&yuv_ion); - visionstream_destroy(&stream); } model_free(&model); diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index cc7d4b157..7280a1561 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -8,19 +8,16 @@ void frame_init(ModelFrame* frame, int width, int height, cl_device_id device_id, cl_context context) { transform_init(&frame->transform, context, device_id); - frame->transformed_width = width; - frame->transformed_height = height; + frame->width = width; + frame->height = height; - frame->transformed_y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)frame->transformed_width*frame->transformed_height, NULL, &err)); - frame->transformed_u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err)); - frame->transformed_v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, - (size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err)); + frame->y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (size_t)width*height, NULL, &err)); + frame->u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (size_t)(width/2)*(height/2), NULL, &err)); + frame->v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (size_t)(width/2)*(height/2), NULL, &err)); frame->net_input_size = ((width*height*3)/2)*sizeof(float); frame->net_input = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, frame->net_input_size, (void*)NULL, &err)); - loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height); + loadyuv_init(&frame->loadyuv, context, device_id, width, height); } float *frame_prepare(ModelFrame* frame, cl_command_queue q, @@ -28,11 +25,11 @@ float *frame_prepare(ModelFrame* frame, cl_command_queue q, const mat3 &transform) { transform_queue(&frame->transform, q, yuv_cl, width, height, - frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, - frame->transformed_width, frame->transformed_height, + frame->y_cl, frame->u_cl, frame->v_cl, + frame->width, frame->height, transform); loadyuv_queue(&frame->loadyuv, q, - frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, + frame->y_cl, frame->u_cl, frame->v_cl, frame->net_input); float *net_input_buf = (float *)CL_CHECK_ERR(clEnqueueMapBuffer(q, frame->net_input, CL_TRUE, CL_MAP_READ, 0, frame->net_input_size, @@ -45,9 +42,9 @@ void frame_free(ModelFrame* frame) { transform_destroy(&frame->transform); loadyuv_destroy(&frame->loadyuv); CL_CHECK(clReleaseMemObject(frame->net_input)); - CL_CHECK(clReleaseMemObject(frame->transformed_v_cl)); - CL_CHECK(clReleaseMemObject(frame->transformed_u_cl)); - CL_CHECK(clReleaseMemObject(frame->transformed_y_cl)); + CL_CHECK(clReleaseMemObject(frame->v_cl)); + CL_CHECK(clReleaseMemObject(frame->u_cl)); + CL_CHECK(clReleaseMemObject(frame->y_cl)); } void softmax(const float* input, float* output, size_t len) { diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 1e1200db6..05bb3e464 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -20,8 +20,8 @@ float sigmoid(float input); typedef struct ModelFrame { Transform transform; - int transformed_width, transformed_height; - cl_mem transformed_y_cl, transformed_u_cl, transformed_v_cl; + int width, height; + cl_mem y_cl, u_cl, v_cl; LoadYUVState loadyuv; cl_mem net_input; size_t net_input_size; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index f13287f03..33900579f 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -24,18 +24,23 @@ void dmonitoring_init(DMonitoringModelState* s) { #endif int runtime = USE_DSP_RUNTIME; - s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, runtime); + s->m = new DefaultRunModel(model_path, &s->output[0], OUTPUT_SIZE, runtime); s->is_rhd = Params().read_db_bool("IsRHD"); } template static inline T *get_buffer(std::vector &buf, const size_t size) { - if (buf.size() < size) { - buf.resize(size); - } + if (buf.size() < size) buf.resize(size); return buf.data(); } +static inline auto get_yuv_buf(std::vector &buf, const int width, int height) { + uint8_t *y = get_buffer(buf, width * height * 3 / 2); + uint8_t *u = y + width * height; + uint8_t *v = u + (width /2) * (height / 2); + return std::make_tuple(y, u, v); +} + DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) { uint8_t *raw_buf = (uint8_t*) stream_buf; uint8_t *raw_y_buf = raw_buf; @@ -52,39 +57,34 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ #else const int full_width_tici = 1928; const int full_height_tici = 1208; - const int adapt_width_tici = 636; + const int adapt_width_tici = 668; const int cropped_height = adapt_width_tici / 1.33; const int cropped_width = cropped_height / 2; const int global_x_offset = full_width_tici / 2 - adapt_width_tici / 2; const int global_y_offset = full_height_tici / 2 - cropped_height / 2; - const int crop_x_offset = adapt_width_tici - cropped_width; - const int crop_y_offset = 0; + const int crop_x_offset = adapt_width_tici - cropped_width + 32; + const int crop_y_offset = -196; #endif int resized_width = MODEL_WIDTH; int resized_height = MODEL_HEIGHT; - uint8_t *cropped_y_buf = get_buffer(s->cropped_buf, cropped_width*cropped_height*3/2); - uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height); - uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); - + auto [cropped_y_buf, cropped_u_buf, cropped_v_buf] = get_yuv_buf(s->cropped_buf, cropped_width, cropped_height); if (!s->is_rhd) { for (int r = 0; r < cropped_height/2; r++) { memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset)*width + global_x_offset + crop_x_offset, cropped_width); memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset + 1)*width + global_x_offset + crop_x_offset, cropped_width); - memcpy(cropped_u_buf + r*cropped_width/2, raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); - memcpy(cropped_v_buf + r*cropped_width/2, raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); + memcpy(cropped_u_buf + r*(cropped_width/2), raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); + memcpy(cropped_v_buf + r*(cropped_width/2), raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + (global_x_offset + crop_x_offset)/2, cropped_width/2); } } else { - uint8_t *premirror_cropped_y_buf = get_buffer(s->premirror_cropped_buf, cropped_width*cropped_height*3/2); - uint8_t *premirror_cropped_u_buf = premirror_cropped_y_buf + (cropped_width * cropped_height); - uint8_t *premirror_cropped_v_buf = premirror_cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); + auto [premirror_cropped_y_buf, premirror_cropped_u_buf, premirror_cropped_v_buf] = get_yuv_buf(s->premirror_cropped_buf, cropped_width, cropped_height); for (int r = 0; r < cropped_height/2; r++) { memcpy(premirror_cropped_y_buf + (2*r)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset)*width + global_x_offset, cropped_width); memcpy(premirror_cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r + global_y_offset + crop_y_offset + 1)*width + global_x_offset, cropped_width); - memcpy(premirror_cropped_u_buf + r*cropped_width/2, raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); - memcpy(premirror_cropped_v_buf + r*cropped_width/2, raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); + memcpy(premirror_cropped_u_buf + r*(cropped_width/2), raw_u_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); + memcpy(premirror_cropped_v_buf + r*(cropped_width/2), raw_v_buf + (r + (global_y_offset + crop_y_offset)/2)*width/2 + global_x_offset/2, cropped_width/2); } libyuv::I420Mirror(premirror_cropped_y_buf, cropped_width, premirror_cropped_u_buf, cropped_width/2, @@ -95,11 +95,8 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ cropped_width, cropped_height); } - uint8_t *resized_buf = get_buffer(s->resized_buf, resized_width*resized_height*3/2); + auto [resized_buf, resized_u_buf, resized_v_buf] = get_yuv_buf(s->resized_buf, resized_width, resized_height); uint8_t *resized_y_buf = resized_buf; - uint8_t *resized_u_buf = resized_y_buf + (resized_width * resized_height); - uint8_t *resized_v_buf = resized_u_buf + ((resized_width/2) * (resized_height/2)); - libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; libyuv::I420Scale(cropped_y_buf, cropped_width, cropped_u_buf, cropped_width/2, @@ -111,39 +108,24 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ resized_width, resized_height, mode); - // prerotate to be cache aware - uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2); - uint8_t *resized_y_buf_rot = resized_buf_rot; - uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height); - uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2)); - - libyuv::I420Rotate(resized_y_buf, resized_width, - resized_u_buf, resized_width/2, - resized_v_buf, resized_width/2, - resized_y_buf_rot, resized_height, - resized_u_buf_rot, resized_height/2, - resized_v_buf_rot, resized_height/2, - // negative height causes a vertical flip to match previous - resized_width, -resized_height, libyuv::kRotate90); - int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); // one shot conversion, O(n) anyway // yuvframe2tensor, normalize - for (int c = 0; c < MODEL_WIDTH/2; c++) { - for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r)*resized_width + (2*c)]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r+1)*resized_width + (2*c)]); // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r)*resized_width + (2*c+1)]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r+1)*resized_width + (2*c+1)]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + r*resized_width/2 + c]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + c + (r*resized_width/2)]); } } @@ -165,26 +147,29 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ double t2 = millis_since_boot(); DMonitoringResult ret = {0}; - memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); - memcpy(&ret.face_orientation_meta, &s->output[6], sizeof ret.face_orientation_meta); - memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); - memcpy(&ret.face_position_meta, &s->output[9], sizeof ret.face_position_meta); - memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob); - memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob); - memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob); - memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob); - memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob); - memcpy(&ret.sg_prob, &s->output[33], sizeof ret.sg_prob); - ret.face_orientation_meta[0] = softplus(ret.face_orientation_meta[0]); - ret.face_orientation_meta[1] = softplus(ret.face_orientation_meta[1]); - ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]); - ret.face_position_meta[0] = softplus(ret.face_position_meta[0]); - ret.face_position_meta[1] = softplus(ret.face_position_meta[1]); + for (int i = 0; i < 3; ++i) { + ret.face_orientation[i] = s->output[i]; + ret.face_orientation_meta[i] = softplus(s->output[6 + i]); + } + for (int i = 0; i < 2; ++i) { + ret.face_position[i] = s->output[3 + i]; + ret.face_position_meta[i] = softplus(s->output[9 + i]); + } + ret.face_prob = s->output[12]; + ret.left_eye_prob = s->output[21]; + ret.right_eye_prob = s->output[30]; + ret.left_blink_prob = s->output[31]; + ret.right_blink_prob = s->output[32]; + ret.sg_prob = s->output[33]; + ret.poor_vision = s->output[34]; + ret.partial_face = s->output[35]; + ret.distracted_pose = s->output[36]; + ret.distracted_eyes = s->output[37]; ret.dsp_execution_time = (t2 - t1) / 1000.; return ret; } -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time){ +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred){ // make msg MessageBuilder msg; auto framed = msg.initEvent().initDriverState(); @@ -201,9 +186,13 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setRightEyeProb(res.right_eye_prob); framed.setLeftBlinkProb(res.left_blink_prob); framed.setRightBlinkProb(res.right_blink_prob); - framed.setSgProb(res.sg_prob); + framed.setSunglassesProb(res.sg_prob); + framed.setPoorVision(res.poor_vision); + framed.setPartialFace(res.partial_face); + framed.setDistractedPose(res.distracted_pose); + framed.setDistractedEyes(res.distracted_eyes); if (send_raw_pred) { - framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, OUTPUT_SIZE*sizeof(float))); + framed.setRawPredictions(raw_pred.asBytes()); } pm.send("driverState", msg); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 7df3355bb..dd015d409 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -5,7 +5,7 @@ #include "runners/run.h" #include "messaging.hpp" -#define OUTPUT_SIZE 34 +#define OUTPUT_SIZE 38 typedef struct DMonitoringResult { float face_orientation[3]; @@ -18,6 +18,10 @@ typedef struct DMonitoringResult { float left_blink_prob; float right_blink_prob; float sg_prob; + float poor_vision; + float partial_face; + float distracted_pose; + float distracted_eyes; float dsp_execution_time; } DMonitoringResult; @@ -26,7 +30,6 @@ typedef struct DMonitoringModelState { bool is_rhd; float output[OUTPUT_SIZE]; std::vector resized_buf; - std::vector resized_buf_rot; std::vector cropped_buf; std::vector premirror_cropped_buf; std::vector net_input_buf; @@ -34,6 +37,6 @@ typedef struct DMonitoringModelState { void dmonitoring_init(DMonitoringModelState* s); DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height); -void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, const float* raw_pred, float execution_time); +void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr raw_pred); void dmonitoring_free(DMonitoringModelState* s); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ae4b7f9e9..767bc2fe1 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,6 +10,9 @@ #include "driving.h" #include "clutil.h" +constexpr int DESIRE_PRED_SIZE = 32; +constexpr int OTHER_META_SIZE = 4; + constexpr int MODEL_WIDTH = 512; constexpr int MODEL_HEIGHT = 256; constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2; @@ -27,9 +30,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); constexpr int POSE_SIZE = 12; -constexpr int MIN_VALID_LEN = 10; -constexpr int TRAJECTORY_TIME = 10; -constexpr float TRAJECTORY_DISTANCE = 192.0; constexpr int PLAN_IDX = 0; constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; @@ -48,17 +48,18 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE; // #define DUMP_YUV -Eigen::Matrix vander; -float X_IDXS[TRAJECTORY_SIZE]; -float T_IDXS[TRAJECTORY_SIZE]; - void model_init(ModelState* s, cl_device_id device_id, cl_context context) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); s->input_frames = std::make_unique(MODEL_FRAME_SIZE * 2); constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE; - s->output = std::make_unique(output_size); + s->output.resize(output_size); + +#if defined(QCOM) || defined(QCOM2) + s->m = std::make_unique("../../models/supercombo.thneed", &s->output[0], output_size, USE_GPU_RUNTIME); +#else s->m = std::make_unique("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME); +#endif #ifdef TEMPORAL s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); @@ -74,15 +75,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif - // Build Vandermonde matrix - for(int i = 0; i < TRAJECTORY_SIZE; i++) { - for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { - X_IDXS[i] = (TRAJECTORY_DISTANCE/1024.0) * (pow(i,2)); - T_IDXS[i] = (TRAJECTORY_TIME/1024.0) * (pow(i,2)); - vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); - } - } - s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); } @@ -137,31 +129,6 @@ void model_free(ModelState* s) { CL_CHECK(clReleaseCommandQueue(s->q)); } -void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { - // References to inputs - Eigen::Map > pts(in_pts, valid_len); - Eigen::Map > std(in_stds, valid_len); - Eigen::Map > p(out, POLYFIT_DEGREE - 1); - - float y0 = pts[0]; - pts = pts.array() - y0; - - // Build Least Squares equations - Eigen::Matrix lhs = vander.topRows(valid_len).array().colwise() / std.array(); - Eigen::Matrix rhs = pts.array() / std.array(); - - // Improve numerical stability - Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); - lhs = lhs * scale.asDiagonal(); - - // Solve inplace - p = lhs.colPivHouseholderQr().solve(rhs); - - // Apply scale to output - p = p.transpose() * scale.asDiagonal(); - out[3] = y0; -} - static const float *get_best_data(const float *data, int size, int group_size, int offset) { int max_idx = 0; for (int i = 1; i < size; i++) { @@ -181,30 +148,6 @@ static const float *get_lead_data(const float *lead, int t_offset) { return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION); } -void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob, - float valid_len, int valid_len_idx, int ll_idx) { - float points[TRAJECTORY_SIZE] = {}; - float stds[TRAJECTORY_SIZE] = {}; - float poly[POLYFIT_DEGREE] = {}; - - for (int i=0; i -void fill_meta(MetaBuilder meta, const float *meta_data) { +void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) { float desire_state_softmax[DESIRE_LEN]; float desire_pred_softmax[4*DESIRE_LEN]; softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); @@ -333,54 +261,22 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou } } -void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) { - // Find the distribution that corresponds to the most probable plan - const float *best_plan = get_plan_data(net_outputs.plan); - // x pos at 10s is a good valid_len - float valid_len = 0; - for (int i=1; i= valid_len){ - valid_len = len; - } - } - // clamp to 10 and MODEL_PATH_DISTANCE - valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); - int valid_len_idx = 0; - for (int i=1; i= X_IDXS[valid_len_idx]){ - valid_len_idx = i; - } - } - fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0); - fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1); - fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2); - - fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0); - fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1); - - fill_meta(framed.initMeta(), net_outputs.meta); -} - void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop, - const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof, - float model_execution_time) { + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time, kj::ArrayPtr raw_pred) { const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - auto do_publish = [&](auto init_model_func, const char *pub_name) { - MessageBuilder msg; - auto framed = (msg.initEvent().*(init_model_func))(); - framed.setFrameId(vipc_frame_id); - framed.setFrameAge(frame_age); - framed.setFrameDropPerc(frame_drop * 100); - framed.setTimestampEof(timestamp_eof); - framed.setModelExecutionTime(model_execution_time); - if (send_raw_pred) { - framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float))); - } - fill_model(framed, net_outputs); - pm.send(pub_name, msg); - }; - do_publish(&cereal::Event::Builder::initModel, "model"); - do_publish(&cereal::Event::Builder::initModelV2, "modelV2"); + MessageBuilder msg; + auto framed = msg.initEvent().initModelV2(); + framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPredictions(raw_pred.asBytes()); + } + fill_model(framed, net_outputs); + pm.send("modelV2", msg); } void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index c5891013b..2a857f277 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -34,7 +34,7 @@ struct ModelDataRaw { typedef struct ModelState { ModelFrame frame; - std::unique_ptr output; + std::vector output; std::unique_ptr input_frames; std::unique_ptr m; cl_command_queue q; @@ -53,7 +53,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop, - const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof, - float model_execution_time); + const ModelDataRaw &net_outputs, uint64_t timestamp_eof, + float model_execution_time, kj::ArrayPtr raw_pred); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, const ModelDataRaw &net_outputs, uint64_t timestamp_eof); diff --git a/selfdrive/modeld/runners/run.h b/selfdrive/modeld/runners/run.h index dea340a0a..98a3fb8b6 100644 --- a/selfdrive/modeld/runners/run.h +++ b/selfdrive/modeld/runners/run.h @@ -1,10 +1,10 @@ -#ifndef RUN_H -#define RUN_H +#pragma once #include "runmodel.h" #include "snpemodel.h" -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) + #include "thneedmodel.h" #define DefaultRunModel SNPEModel #else #ifdef USE_ONNX_MODEL @@ -14,5 +14,3 @@ #define DefaultRunModel SNPEModel #endif #endif - -#endif diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 2197c24cd..538c24656 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -140,7 +140,8 @@ void SNPEModel::execute(float *net_input_buf, int buf_size) { if (Runtime == zdl::DlSystem::Runtime_t::GPU) { float *inputs[4] = {recurrent, trafficConvention, desire, net_input_buf}; if (thneed == NULL) { - assert(inputBuffer->setBufferAddress(net_input_buf)); + bool ret = inputBuffer->setBufferAddress(net_input_buf); + assert(ret == true); if (!snpe->execute(inputMap, outputMap)) { PrintErrorStringAndExit(); } @@ -173,7 +174,8 @@ void SNPEModel::execute(float *net_input_buf, int buf_size) { } } else { #endif - assert(inputBuffer->setBufferAddress(net_input_buf)); + bool ret = inputBuffer->setBufferAddress(net_input_buf); + assert(ret == true); if (!snpe->execute(inputMap, outputMap)) { PrintErrorStringAndExit(); } diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 90c26664f..76339642f 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -31,13 +31,14 @@ public: void addTrafficConvention(float *state, int state_size); void addDesire(float *state, int state_size); void execute(float *net_input_buf, int buf_size); -private: - uint8_t *model_data = NULL; #ifdef USE_THNEED Thneed *thneed = NULL; #endif +private: + uint8_t *model_data = NULL; + #if defined(QCOM) || defined(QCOM2) zdl::DlSystem::Runtime_t Runtime; #endif diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc new file mode 100644 index 000000000..0ebe7226e --- /dev/null +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -0,0 +1,41 @@ +#include "thneedmodel.h" +#include + +ThneedModel::ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime) { + thneed = new Thneed(true); + thneed->record = 0; + thneed->load(path); + thneed->clexec(); + thneed->find_inputs_outputs(); + + recorded = false; + output = loutput; +} + +void ThneedModel::addRecurrent(float *state, int state_size) { + recurrent = state; +} + +void ThneedModel::addTrafficConvention(float *state, int state_size) { + trafficConvention = state; +} + +void ThneedModel::addDesire(float *state, int state_size) { + desire = state; +} + +void ThneedModel::execute(float *net_input_buf, int buf_size) { + float *inputs[4] = {recurrent, trafficConvention, desire, net_input_buf}; + if (!recorded) { + thneed->record = THNEED_RECORD; + thneed->copy_inputs(inputs); + thneed->clexec(); + thneed->copy_output(output); + thneed->stop(); + + recorded = true; + } else { + thneed->execute(inputs, output); + } +} + diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h new file mode 100644 index 000000000..05cb2438e --- /dev/null +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -0,0 +1,24 @@ +#pragma once + +#include "runmodel.h" +#include "thneed/thneed.h" + +class ThneedModel : public RunModel { +public: + ThneedModel(const char *path, float *loutput, size_t loutput_size, int runtime); + void addRecurrent(float *state, int state_size); + void addTrafficConvention(float *state, int state_size); + void addDesire(float *state, int state_size); + void execute(float *net_input_buf, int buf_size); +private: + Thneed *thneed = NULL; + bool recorded; + + float *output; + + // recurrent and desire + float *recurrent; + float *trafficConvention; + float *desire; +}; + diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc new file mode 100644 index 000000000..63955a6f9 --- /dev/null +++ b/selfdrive/modeld/thneed/compile.cc @@ -0,0 +1,34 @@ +#include +#include "thneed.h" +#include "../runners/snpemodel.h" + +#define TEMPORAL_SIZE 512 +#define DESIRE_LEN 8 +#define TRAFFIC_CONVENTION_LEN 2 + +// TODO: This should probably use SNPE directly. +int main(int argc, char* argv[]) { + #define OUTPUT_SIZE 0x10000 + float *output = (float*)calloc(OUTPUT_SIZE, sizeof(float)); + SNPEModel mdl(argv[1], output, 0, USE_GPU_RUNTIME); + + float state[TEMPORAL_SIZE] = {0}; + float desire[DESIRE_LEN] = {0}; + float traffic_convention[TRAFFIC_CONVENTION_LEN] = {0}; + float *input = (float*)calloc(0x1000000, sizeof(float));; + + mdl.addRecurrent(state, TEMPORAL_SIZE); + mdl.addDesire(desire, DESIRE_LEN); + mdl.addTrafficConvention(traffic_convention, TRAFFIC_CONVENTION_LEN); + + // first run + printf("************** execute 1 **************\n"); + memset(output, 0, OUTPUT_SIZE * sizeof(float)); + mdl.execute(input, 0); + + // save model + bool save_binaries = (argc > 3) && (strcmp(argv[3], "--binary") == 0); + mdl.thneed->save(argv[2], save_binaries); + return 0; +} + diff --git a/selfdrive/modeld/thneed/serialize.cc b/selfdrive/modeld/thneed/serialize.cc new file mode 100644 index 000000000..7f22d631f --- /dev/null +++ b/selfdrive/modeld/thneed/serialize.cc @@ -0,0 +1,290 @@ +#include +#include +#include "thneed.h" +#include "json11.hpp" +using namespace json11; + +extern map g_program_source; + +void Thneed::load(const char *filename) { + printf("Thneed::load: loading from %s\n", filename); + + FILE *f = fopen(filename, "rb"); + fseek(f, 0L, SEEK_END); + int sz = ftell(f); + fseek(f, 0L, SEEK_SET); + char *buf = (char*)malloc(sz); + fread(buf, 1, sz, f); + fclose(f); + + int jsz = *(int *)buf; + string jj(buf+4, jsz); + string err; + Json jdat = Json::parse(jj, err); + + map real_mem; + real_mem[NULL] = NULL; + + int ptr = 4+jsz; + for (auto &obj : jdat["objects"].array_items()) { + auto mobj = obj.object_items(); + int sz = mobj["size"].int_value(); + cl_mem clbuf = NULL; + + if (mobj["buffer_id"].string_value().size() > 0) { + // image buffer must already be allocated + clbuf = real_mem[*(cl_mem*)(mobj["buffer_id"].string_value().data())]; + assert(mobj["needs_load"].bool_value() == false); + } else { + if (mobj["needs_load"].bool_value()) { + //printf("loading %p %d @ 0x%X\n", clbuf, sz, ptr); + clbuf = clCreateBuffer(context, CL_MEM_COPY_HOST_PTR | CL_MEM_READ_WRITE, sz, &buf[ptr], NULL); + ptr += sz; + } else { + clbuf = clCreateBuffer(context, CL_MEM_READ_WRITE, sz, NULL, NULL); + } + } + assert(clbuf != NULL); + + if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") { + cl_image_desc desc = {0}; + desc.image_type = (mobj["arg_type"] == "image2d_t") ? CL_MEM_OBJECT_IMAGE2D : CL_MEM_OBJECT_IMAGE1D_BUFFER; + desc.image_width = mobj["width"].int_value(); + desc.image_height = mobj["height"].int_value(); + desc.image_row_pitch = mobj["row_pitch"].int_value(); + desc.buffer = clbuf; + + cl_image_format format; + format.image_channel_order = CL_RGBA; + format.image_channel_data_type = CL_HALF_FLOAT; + + clbuf = clCreateImage(context, CL_MEM_READ_WRITE, &format, &desc, NULL, NULL); + assert(clbuf != NULL); + } + + real_mem[*(cl_mem*)(mobj["id"].string_value().data())] = clbuf; + } + + map g_programs; + for (auto &obj : jdat["programs"].object_items()) { + const char *srcs[1]; + srcs[0] = (const char *)obj.second.string_value().c_str(); + size_t length = obj.second.string_value().size(); + + if (record & THNEED_DEBUG) printf("building %s with size %zu\n", obj.first.c_str(), length); + + cl_program program = clCreateProgramWithSource(context, 1, srcs, &length, NULL); + int err = clBuildProgram(program, 1, &device_id, "", NULL, NULL); + if (err != 0) { + printf("got err %d\n", err); + size_t length; + char buffer[2048]; + clGetProgramBuildInfo(program, device_id, CL_PROGRAM_BUILD_LOG, sizeof(buffer), buffer, &length); + buffer[length] = '\0'; + printf("%s\n", buffer); + } + assert(err == 0); + + g_programs[obj.first] = program; + } + + for (auto &obj : jdat["binaries"].array_items()) { + string name = obj["name"].string_value(); + size_t length = obj["length"].int_value(); + const unsigned char *srcs[1]; + srcs[0] = (const unsigned char *)&buf[ptr]; + ptr += length; + + if (record & THNEED_DEBUG) printf("binary %s with size %zu\n", name.c_str(), length); + + cl_int err; + cl_program program = clCreateProgramWithBinary(context, 1, &device_id, &length, srcs, NULL, &err); + assert(program != NULL && err == CL_SUCCESS); + err = clBuildProgram(program, 1, &device_id, "", NULL, NULL); + assert(err == CL_SUCCESS); + + g_programs[name] = program; + } + + for (auto &obj : jdat["kernels"].array_items()) { + auto gws = obj["global_work_size"]; + auto lws = obj["local_work_size"]; + auto kk = shared_ptr(new CLQueuedKernel(this)); + + kk->name = obj["name"].string_value(); + kk->program = g_programs[kk->name]; + kk->work_dim = obj["work_dim"].int_value(); + for (int i = 0; i < kk->work_dim; i++) { + kk->global_work_size[i] = gws[i].int_value(); + kk->local_work_size[i] = lws[i].int_value(); + } + kk->num_args = obj["num_args"].int_value(); + for (int i = 0; i < kk->num_args; i++) { + string arg = obj["args"].array_items()[i].string_value(); + int arg_size = obj["args_size"].array_items()[i].int_value(); + kk->args_size.push_back(arg_size); + if (arg_size == 8) { + cl_mem val = *(cl_mem*)(arg.data()); + val = real_mem[val]; + kk->args.push_back(string((char*)&val, sizeof(val))); + } else { + kk->args.push_back(arg); + } + } + kq.push_back(kk); + } + + free(buf); + clFinish(command_queue); +} + +void Thneed::save(const char *filename, bool save_binaries) { + printf("Thneed::save: saving to %s\n", filename); + + // get kernels + std::vector kernels; + std::set saved_objects; + std::vector objects; + std::map programs; + std::map binaries; + + for (auto &k : kq) { + kernels.push_back(k->to_json()); + + // check args for objects + int i = 0; + for (auto &a : k->args) { + if (a.size() == 8) { + if (saved_objects.find(a) == saved_objects.end()) { + saved_objects.insert(a); + cl_mem val = *(cl_mem*)(a.data()); + if (val != NULL) { + bool needs_load = k->arg_names[i] == "weights" || k->arg_names[i] == "biases"; + + auto jj = Json::object({ + {"id", a}, + {"arg_type", k->arg_types[i]}, + }); + + if (k->arg_types[i] == "image2d_t" || k->arg_types[i] == "image1d_t") { + cl_mem buf; + clGetImageInfo(val, CL_IMAGE_BUFFER, sizeof(buf), &buf, NULL); + string aa = string((char *)&buf, sizeof(buf)); + jj["buffer_id"] = aa; + + size_t width, height, row_pitch; + clGetImageInfo(val, CL_IMAGE_WIDTH, sizeof(width), &width, NULL); + clGetImageInfo(val, CL_IMAGE_HEIGHT, sizeof(height), &height, NULL); + clGetImageInfo(val, CL_IMAGE_ROW_PITCH, sizeof(row_pitch), &row_pitch, NULL); + jj["width"] = (int)width; + jj["height"] = (int)height; + jj["row_pitch"] = (int)row_pitch; + jj["size"] = (int)(height * row_pitch); + jj["needs_load"] = false; + + if (saved_objects.find(aa) == saved_objects.end()) { + saved_objects.insert(aa); + size_t sz; + clGetMemObjectInfo(buf, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + // save the buffer + objects.push_back(Json::object({ + {"id", aa}, + {"arg_type", ""}, + {"needs_load", needs_load}, + {"size", (int)sz} + })); + if (needs_load) assert(sz == height * row_pitch); + } + } else { + size_t sz = 0; + clGetMemObjectInfo(val, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + jj["size"] = (int)sz; + jj["needs_load"] = needs_load; + } + + objects.push_back(jj); + } + } + } + i++; + } + + if (save_binaries) { + int err; + size_t binary_size = 0; + err = clGetProgramInfo(k->program, CL_PROGRAM_BINARY_SIZES, sizeof(binary_size), &binary_size, NULL); + assert(err == 0); + assert(binary_size > 0); + string sv(binary_size, '\x00'); + + uint8_t* bufs[1] = { (uint8_t*)sv.data(), }; + err = clGetProgramInfo(k->program, CL_PROGRAM_BINARIES, sizeof(bufs), &bufs, NULL); + assert(err == 0); + + binaries[k->name] = sv; + } else { + programs[k->name] = g_program_source[k->program]; + } + } + + vector saved_buffers; + for (auto &obj : objects) { + auto mobj = obj.object_items(); + cl_mem val = *(cl_mem*)(mobj["id"].string_value().data()); + int sz = mobj["size"].int_value(); + if (mobj["needs_load"].bool_value()) { + char *buf = (char *)malloc(sz); + if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") { + assert(false); + } else { + // buffers alloced with CL_MEM_HOST_WRITE_ONLY, hence this hack + //hexdump((uint32_t*)val, 0x100); + + // the worst hack in thneed, the flags are at 0x14 + ((uint32_t*)val)[0x14] &= ~CL_MEM_HOST_WRITE_ONLY; + cl_int ret = clEnqueueReadBuffer(command_queue, val, CL_TRUE, 0, sz, buf, 0, NULL, NULL); + assert(ret == CL_SUCCESS); + } + //printf("saving buffer: %d %p %s\n", sz, buf, mobj["arg_type"].string_value().c_str()); + saved_buffers.push_back(string(buf, sz)); + free(buf); + } + } + + std::vector jbinaries; + for (auto &obj : binaries) { + jbinaries.push_back(Json::object({{"name", obj.first}, {"length", (int)obj.second.size()}})); + saved_buffers.push_back(obj.second); + } + + Json jdat = Json::object({ + {"kernels", kernels}, + {"objects", objects}, + {"programs", programs}, + {"binaries", jbinaries}, + }); + + string str = jdat.dump(); + int jsz = str.length(); + + FILE *f = fopen(filename, "wb"); + fwrite(&jsz, 1, sizeof(jsz), f); + fwrite(str.data(), 1, jsz, f); + for (auto &s : saved_buffers) { + fwrite(s.data(), 1, s.length(), f); + } + fclose(f); +} + +Json CLQueuedKernel::to_json() const { + return Json::object { + { "name", name }, + { "work_dim", (int)work_dim }, + { "global_work_size", Json::array { (int)global_work_size[0], (int)global_work_size[1], (int)global_work_size[2] } }, + { "local_work_size", Json::array { (int)local_work_size[0], (int)local_work_size[1], (int)local_work_size[2] } }, + { "num_args", (int)num_args }, + { "args", args }, + { "args_size", args_size }, + }; +} + diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc index 443285653..481a07750 100644 --- a/selfdrive/modeld/thneed/thneed.cc +++ b/selfdrive/modeld/thneed/thneed.cc @@ -5,10 +5,9 @@ #include #include #include +#include "common/clutil.h" #include "thneed.h" -//#define SAVE_KERNELS - //#define RUN_DISASSEMBLER //#define RUN_OPTIMIZER @@ -83,7 +82,8 @@ int ioctl(int filedes, unsigned long request, void *argp) { } if (thneed->record & THNEED_RECORD) { - thneed->syncobjs.push_back(string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count)); + thneed->cmds.push_back(unique_ptr(new + CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count)))); } } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) { struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp; @@ -103,6 +103,14 @@ int ioctl(int filedes, unsigned long request, void *argp) { } } } + } else if (request == IOCTL_KGSL_DRAWCTXT_CREATE || request == IOCTL_KGSL_DRAWCTXT_DESTROY) { + // this happens + } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) { + // this happens + } else { + if (thneed->record & THNEED_DEBUG) { + printf("other ioctl %lx\n", request); + } } } @@ -133,13 +141,27 @@ GPUMalloc::~GPUMalloc() { } void *GPUMalloc::alloc(int size) { - if (size > remaining) return NULL; - remaining -= size; void *ret = (void*)base; - base += (size+0xff) & (~0xFF); + size = (size+0xff) & (~0xFF); + assert(size <= remaining); + remaining -= size; + base += size; return ret; } +// *********** CachedSync, at the ioctl layer *********** + +void CachedSync::exec() { + struct kgsl_gpuobj_sync cmd; + + cmd.objs = (uint64_t)data.data(); + cmd.obj_len = data.length(); + cmd.count = data.length() / sizeof(struct kgsl_gpuobj_sync_obj); + + int ret = ioctl(thneed->fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); + assert(ret == 0); +} + // *********** CachedCommand, at the ioctl layer *********** CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { @@ -174,24 +196,11 @@ CachedCommand::CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd) { thneed->ckq.clear(); } -void CachedCommand::exec(bool wait) { +void CachedCommand::exec() { cache.timestamp = ++thneed->timestamp; int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache); - if (wait) { - struct kgsl_device_waittimestamp_ctxtid wait; - wait.context_id = cache.context_id; - wait.timestamp = cache.timestamp; - wait.timeout = -1; - - uint64_t tb = nanos_since_boot(); - int wret = ioctl(thneed->fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); - uint64_t te = nanos_since_boot(); - - if (thneed->record & THNEED_DEBUG) printf("exec %d wait %d after %lu us\n", ret, wret, (te-tb)/1000); - } else { - if (thneed->record & THNEED_DEBUG) printf("CachedCommand::exec got %d\n", ret); - } + if (thneed->record & THNEED_DEBUG) printf("CachedCommand::exec got %d\n", ret); if (thneed->record & THNEED_VERBOSE_DEBUG) { for (auto &it : kq) { @@ -213,32 +222,85 @@ Thneed::Thneed(bool do_clinit) { if (do_clinit) clinit(); assert(g_fd != -1); fd = g_fd; - ram = make_unique(0x40000, fd); + ram = make_unique(0x80000, fd); record = THNEED_RECORD; timestamp = -1; g_thneed = this; } void Thneed::stop() { + find_inputs_outputs(); + printf("Thneed::stop: recorded %lu commands\n", cmds.size()); record = 0; } +void Thneed::find_inputs_outputs() { + cl_int err; + if (inputs.size() > 0) return; + + // save the global inputs/outputs + for (auto &k : kq) { + for (int i = 0; i < k->num_args; i++) { + if (k->name == "zero_pad_image_float" && k->arg_names[i] == "input") { + cl_mem aa = *(cl_mem*)(k->args[i].data()); + + size_t sz; + clGetMemObjectInfo(aa, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + input_sizes.push_back(sz); + + void *ret = clEnqueueMapBuffer(command_queue, aa, CL_TRUE, CL_MAP_WRITE, 0, sz, 0, NULL, NULL, &err); + assert(err == CL_SUCCESS); + inputs.push_back(ret); + } + + if (k->name == "image2d_to_buffer_float" && k->arg_names[i] == "output") { + output = *(cl_mem*)(k->args[i].data()); + } + } + } +} + +void Thneed::copy_inputs(float **finputs) { + //cl_int ret; + for (int idx = 0; idx < inputs.size(); ++idx) { + if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]); + memcpy(inputs[idx], finputs[idx], input_sizes[idx]); + } +} + +void Thneed::copy_output(float *foutput) { + if (output != NULL) { + size_t sz; + clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); + if (record & THNEED_DEBUG) printf("copying %lu for output %p -> %p\n", sz, output, foutput); + clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL); + } else { + printf("CAUTION: model output is NULL, does it have no outputs?\n"); + } +} + +void Thneed::wait() { + struct kgsl_device_waittimestamp_ctxtid wait; + wait.context_id = context_id; + wait.timestamp = timestamp; + wait.timeout = -1; + + uint64_t tb = nanos_since_boot(); + int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait); + uint64_t te = nanos_since_boot(); + + if (record & THNEED_DEBUG) printf("wait %d after %lu us\n", wret, (te-tb)/1000); +} + void Thneed::execute(float **finputs, float *foutput, bool slow) { - int ret; uint64_t tb, te; if (record & THNEED_DEBUG) tb = nanos_since_boot(); // ****** copy inputs - for (int idx = 0; idx < inputs.size(); ++idx) { - size_t sz; - clGetMemObjectInfo(inputs[idx], CL_MEM_SIZE, sizeof(sz), &sz, NULL); - - if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", sz, finputs[idx], inputs[idx]); - // TODO: This shouldn't have to block - clEnqueueWriteBuffer(command_queue, inputs[idx], CL_TRUE, 0, sz, finputs[idx], 0, NULL, NULL); - } + copy_inputs(finputs); // ****** set power constraint + int ret; struct kgsl_device_constraint_pwrlevel pwrlevel; pwrlevel.level = KGSL_CONSTRAINT_PWR_MAX; @@ -260,30 +322,12 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) { for (auto &it : cmds) { ++i; if (record & THNEED_DEBUG) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000); - it->exec((i == cmds.size()) || slow); - } - - // ****** sync objects - for (auto &it : syncobjs) { - struct kgsl_gpuobj_sync cmd; - - cmd.objs = (uint64_t)it.data(); - cmd.obj_len = it.length(); - cmd.count = it.length() / sizeof(struct kgsl_gpuobj_sync_obj); - - ret = ioctl(fd, IOCTL_KGSL_GPUOBJ_SYNC, &cmd); - assert(ret == 0); + it->exec(); + if ((i == cmds.size()) || slow) wait(); } // ****** copy outputs - if (output != NULL) { - size_t sz; - clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL); - if (record & THNEED_DEBUG) printf("copying %lu for output %p -> %p\n", sz, output, foutput); - clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL); - } else { - printf("CAUTION: model output is NULL, does it have no outputs?\n"); - } + copy_output(foutput); // ****** unset power constraint constraint.type = KGSL_CONSTRAINT_NONE; @@ -300,26 +344,11 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) { } void Thneed::clinit() { - cl_int err; - - cl_platform_id platform_id[2]; - cl_uint num_devices; - cl_uint num_platforms; - - err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms); - assert(err == 0); - - err = clGetDeviceIDs(platform_id[0], CL_DEVICE_TYPE_DEFAULT, 1, &device_id, &num_devices); - assert(err == 0); - - context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); - assert(err == 0); - + device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); //cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, CL_QUEUE_PROFILING_ENABLE, 0}; - cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0}; - command_queue = clCreateCommandQueueWithProperties(context, device_id, props, &err); - assert(err == 0); - + cl_command_queue_properties props[3] = {CL_QUEUE_PROPERTIES, 0, 0}; + command_queue = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); printf("Thneed::clinit done\n"); } @@ -404,11 +433,8 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c } void *dlsym(void *handle, const char *symbol) { - // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so -#if defined QCOM - void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4); -#elif defined QCOM2 - void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138); +#if defined(QCOM) || defined(QCOM2) + void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen + DLSYM_OFFSET); #else #error "Unsupported platform for thneed" #endif @@ -453,6 +479,9 @@ CLQueuedKernel::CLQueuedKernel(Thneed *lthneed, char arg_name[0x100]; clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); + args.push_back(g_args[make_pair(kernel, i)]); args_size.push_back(g_args_size[make_pair(kernel, i)]); } @@ -473,11 +502,14 @@ cl_int CLQueuedKernel::exec() { if (kernel == NULL) { kernel = clCreateKernel(program, name.c_str(), NULL); arg_names.clear(); + arg_types.clear(); for (int j = 0; j < num_args; j++) { char arg_name[0x100]; clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_NAME, sizeof(arg_name), arg_name, NULL); arg_names.push_back(string(arg_name)); + clGetKernelArgInfo(kernel, j, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_name), arg_name, NULL); + arg_types.push_back(string(arg_name)); cl_int ret; if (args[j].size() != 0) { @@ -490,19 +522,6 @@ cl_int CLQueuedKernel::exec() { } } - // save the global inputs/outputs - if (thneed->record & THNEED_RECORD) { - for (int i = 0; i < num_args; i++) { - if (name == "zero_pad_image_float" && arg_names[i] == "input") { - thneed->inputs.push_back(*(cl_mem*)(args[i].data())); - } - - if (name == "image2d_to_buffer_float" && arg_names[i] == "output") { - thneed->output = *(cl_mem*)(args[i].data()); - } - } - } - if (thneed->record & THNEED_DEBUG) { debug_print(thneed->record & THNEED_VERBOSE_DEBUG); } @@ -524,10 +543,8 @@ void CLQueuedKernel::debug_print(bool verbose) { if (verbose) { for (int i = 0; i < num_args; i++) { - char arg_type[0x100]; - clGetKernelArgInfo(kernel, i, CL_KERNEL_ARG_TYPE_NAME, sizeof(arg_type), arg_type, NULL); string arg = args[i]; - printf(" %s %s", arg_type, arg_names[i].c_str()); + printf(" %s %s", arg_types[i].c_str(), arg_names[i].c_str()); void *arg_value = (void*)arg.data(); int arg_size = arg.size(); if (arg_size == 0) { @@ -537,7 +554,7 @@ void CLQueuedKernel::debug_print(bool verbose) { } else if (arg_size == 2) { printf(" = %d", *((short*)arg_value)); } else if (arg_size == 4) { - if (strcmp(arg_type, "float") == 0) { + if (arg_types[i] == "float") { printf(" = %f", *((float*)arg_value)); } else { printf(" = %d", *((int*)arg_value)); @@ -546,7 +563,7 @@ void CLQueuedKernel::debug_print(bool verbose) { cl_mem val = (cl_mem)(*((uintptr_t*)arg_value)); printf(" = %p", val); if (val != NULL) { - if (strcmp("image2d_t", arg_type) == 0 || strcmp("image1d_t", arg_type) == 0) { + if (arg_types[i] == "image2d_t" || arg_types[i] == "image1d_t") { cl_image_format format; size_t width, height, depth, array_size, row_pitch, slice_pitch; cl_mem buf; @@ -578,4 +595,3 @@ void CLQueuedKernel::debug_print(bool verbose) { } } } - diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h index e1039efdf..c36aaff70 100644 --- a/selfdrive/modeld/thneed/thneed.h +++ b/selfdrive/modeld/thneed/thneed.h @@ -48,6 +48,7 @@ class CLQueuedKernel { string name; cl_uint num_args; vector arg_names; + vector arg_types; vector args; vector args_size; cl_kernel kernel = NULL; @@ -60,12 +61,26 @@ class CLQueuedKernel { Thneed *thneed; }; -class CachedCommand { +class CachedIoctl { + public: + virtual void exec() {} +}; + +class CachedSync: public CachedIoctl { + public: + CachedSync(Thneed *lthneed, string ldata) { thneed = lthneed; data = ldata; } + void exec(); + private: + Thneed *thneed; + string data; +}; + +class CachedCommand: public CachedIoctl { public: CachedCommand(Thneed *lthneed, struct kgsl_gpu_command *cmd); - void exec(bool wait); - void disassemble(int cmd_index); + void exec(); private: + void disassemble(int cmd_index); struct kgsl_gpu_command cache; unique_ptr cmds; unique_ptr objs; @@ -78,9 +93,11 @@ class Thneed { Thneed(bool do_clinit=false); void stop(); void execute(float **finputs, float *foutput, bool slow=false); + void wait(); int optimize(); - vector inputs; + vector inputs; + vector input_sizes; cl_mem output = NULL; cl_context context = NULL; @@ -92,11 +109,13 @@ class Thneed { int record; int timestamp; unique_ptr ram; - vector > cmds; - vector syncobjs; + vector > cmds; int fd; // all CL kernels + void find_inputs_outputs(); + void copy_inputs(float **finputs); + void copy_output(float *foutput); cl_int clexec(); vector > kq; @@ -105,9 +124,8 @@ class Thneed { // loading and saving void load(const char *filename); - void save(const char *filename); + void save(const char *filename, bool save_binaries=false); private: void clinit(); - json11::Json to_json(); }; diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index d8fa4932c..83313b820 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -9,23 +9,16 @@ from selfdrive.locationd.calibrationd import Calibration def dmonitoringd_thread(sm=None, pm=None): if pm is None: - pm = messaging.PubMaster(['dMonitoringState']) + pm = messaging.PubMaster(['driverMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'model'], poll=['driverState']) + sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) - driver_status = DriverStatus() - driver_status.is_rhd_region = Params().get("IsRHD") == b"1" - - offroad = Params().get("IsOffroad") == b"1" + driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") sm['liveCalibration'].calStatus = Calibration.INVALID sm['liveCalibration'].rpyCalib = [0, 0, 0] - sm['carState'].vEgo = 0. - sm['carState'].cruiseState.speed = 0. sm['carState'].buttonEvents = [] - sm['carState'].steeringPressed = False - sm['carState'].gasPressed = False sm['carState'].standstill = True v_cruise_last = 0 @@ -49,8 +42,8 @@ def dmonitoringd_thread(sm=None, pm=None): driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise - if sm.updated['model']: - driver_status.set_policy(sm['model']) + if sm.updated['modelV2']: + driver_status.set_policy(sm['modelV2']) # Get data from dmonitoringmodeld events = Events() @@ -63,14 +56,13 @@ def dmonitoringd_thread(sm=None, pm=None): # Update events from driver state driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) - # build dMonitoringState packet - dat = messaging.new_message('dMonitoringState') - dat.dMonitoringState = { + # build driverMonitoringState packet + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, "isDistracted": driver_status.driver_distracted, "awarenessStatus": driver_status.awareness, - "isRHD": driver_status.is_rhd_region, "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), @@ -80,9 +72,9 @@ def dmonitoringd_thread(sm=None, pm=None): "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, - "isPreview": offroad, + "isActiveMode": driver_status.active_monitoring_mode, } - pm.send('dMonitoringState', dat) + pm.send('driverMonitoringState', dat) def main(sm=None, pm=None): dmonitoringd_thread(sm, pm) diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 33005e19c..1e7c01203 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -1,11 +1,11 @@ -from common.numpy_fast import interp from math import atan2, sqrt + +from cereal import car +from common.numpy_fast import interp from common.realtime import DT_DMON from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter -from cereal import car - EventName = car.CarEvent.EventName # ****************************************************************************************** @@ -21,8 +21,9 @@ _DISTRACTED_TIME = 11. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. -_FACE_THRESHOLD = 0.6 -_EYE_THRESHOLD = 0.6 +_FACE_THRESHOLD = 0.5 +_PARTIAL_FACE_THRESHOLD = 0.5 +_EYE_THRESHOLD = 0.5 _SG_THRESHOLD = 0.5 _BLINK_THRESHOLD = 0.5 _BLINK_THRESHOLD_SLACK = 0.65 @@ -54,7 +55,7 @@ MAX_TERMINAL_DURATION = 300 # 30s RESIZED_FOCAL = 320.0 H, W, FULL_W = 320, 160, 426 -class DistractedType(): +class DistractedType: NOT_DISTRACTED = 0 BAD_POSE = 1 BAD_BLINK = 2 @@ -63,22 +64,19 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right - pitch_net = angles_desc[0] - yaw_net = angles_desc[1] - roll_net = angles_desc[2] + pitch_net, yaw_net, roll_net = angles_desc face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) yaw_focal_angle = atan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) pitch_focal_angle = atan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) - roll = roll_net pitch = pitch_net + pitch_focal_angle yaw = -yaw_net + yaw_focal_angle # no calib for roll pitch -= rpy_calib[1] yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> += - return roll, pitch, yaw + return roll_net, pitch, yaw class DriverPose(): def __init__(self): @@ -100,7 +98,8 @@ class DriverBlink(): self.cfactor = 1. class DriverStatus(): - def __init__(self): + def __init__(self, rhd=False): + self.is_rhd_region = rhd self.pose = DriverPose() self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT @@ -111,17 +110,16 @@ class DriverStatus(): self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON) self.face_detected = False + self.face_partial = False self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. self.active_monitoring_mode = True + self.is_model_uncertain = False self.hi_stds = 0 self.hi_std_alert_enabled = True self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME - self.is_rhd_region = False - self.is_rhd_region_checked = False - self._set_timers(active_monitoring=True) def _set_timers(self, active_monitoring): @@ -181,23 +179,23 @@ class DriverStatus(): self.blink.cfactor = interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged): - # 10 Hz - if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0: + if not all(len(x) > 0 for x in [driver_state.faceOrientation, driver_state.facePosition, + driver_state.faceOrientationStd, driver_state.facePositionStd]): return + self.face_partial = driver_state.partialFace > _PARTIAL_FACE_THRESHOLD + self.face_detected = driver_state.faceProb > _FACE_THRESHOLD or self.face_partial self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region) self.pose.pitch_std = driver_state.faceOrientationStd[0] self.pose.yaw_std = driver_state.faceOrientationStd[1] # self.pose.roll_std = driver_state.faceOrientationStd[2] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) - self.pose.low_std = model_std_max < _POSESTD_THRESHOLD - self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) - self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) - self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \ - abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 + self.pose.low_std = model_std_max < _POSESTD_THRESHOLD and not self.face_partial + self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD) + self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sunglassesProb < _SG_THRESHOLD) - self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 - # first order filters + self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ + driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) # update offseter @@ -209,11 +207,9 @@ class DriverStatus(): self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT - is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME - self._set_timers(self.face_detected and not is_model_uncertain) + self.is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME + self._set_timers(self.face_detected and not self.is_model_uncertain) if self.face_detected and not self.pose.low_std: - if not is_model_uncertain: - self.step_change *= min(1.0, max(0.6, 1.6*(model_std_max-0.5)*(model_std_max-2))) self.hi_stds += 1 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index cb630eabe..b0d2cf73f 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -3,15 +3,13 @@ import os import time -from panda import BASEDIR, Panda, PandaDFU, build_st +from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU, build_st +from common.basedir import BASEDIR from common.gpio import gpio_init, gpio_set from selfdrive.hardware import TICI from selfdrive.hardware.tici.pins import GPIO_HUB_RST_N, GPIO_STM_BOOT0, GPIO_STM_RST_N from selfdrive.swaglog import cloudlog -def is_legacy_panda_reset(): - return os.path.isfile("/persist/LEGACY_PANDA_RESET") - def set_panda_power(power=True): if not TICI: return @@ -19,16 +17,16 @@ def set_panda_power(power=True): gpio_init(GPIO_STM_RST_N, True) gpio_init(GPIO_STM_BOOT0, True) - gpio_set(GPIO_STM_RST_N, False if is_legacy_panda_reset() else True) + gpio_set(GPIO_STM_RST_N, True) gpio_set(GPIO_HUB_RST_N, True) time.sleep(0.1) - gpio_set(GPIO_STM_RST_N, power if is_legacy_panda_reset() else (not power)) + gpio_set(GPIO_STM_RST_N, not power) def get_firmware_fn(): - signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed") + signed_fn = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed") if os.path.exists(signed_fn): cloudlog.info("Using prebuilt signed firmware") return signed_fn @@ -36,7 +34,7 @@ def get_firmware_fn(): cloudlog.info("Building panda firmware") fn = "obj/panda.bin" build_st(fn, clean=False) - return os.path.join(BASEDIR, "board", fn) + return os.path.join(PANDA_BASEDIR, "board", fn) def get_expected_signature(fw_fn=None): @@ -110,12 +108,14 @@ def update_panda(): cloudlog.info("Version mismatch after flashing, exiting") raise AssertionError + cloudlog.info("Resetting panda") + panda.reset() def main(): set_panda_power() update_panda() - os.chdir("boardd") + os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) os.execvp("./boardd", ["./boardd"]) diff --git a/selfdrive/proclogd/proclogd.cc b/selfdrive/proclogd/proclogd.cc index 24b42c5bf..66f2f5025 100644 --- a/selfdrive/proclogd/proclogd.cc +++ b/selfdrive/proclogd/proclogd.cc @@ -1,10 +1,10 @@ +#include +#include + #include #include #include #include - -#include -#include #include #include #include @@ -16,10 +16,11 @@ #include "messaging.hpp" #include "common/timing.h" -#include "common/utilpp.h" +#include "common/util.h" + +ExitHandler do_exit; namespace { - struct ProcCache { std::string name; std::vector cmdline; @@ -36,7 +37,7 @@ int main() { std::unordered_map proc_cache; - while (1) { + while (!do_exit) { MessageBuilder msg; auto procLog = msg.initEvent().initProcLog(); @@ -223,7 +224,7 @@ int main() { publisher.send("procLog", msg); - usleep(2000000); // 2 secs + util::sleep_for(2000); // 2 secs } return 0; diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 07c1e374c..d74cc091a 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -1,6 +1,9 @@ import os +import time import json +import jwt + from datetime import datetime, timedelta from common.api import api_get from common.params import Params @@ -12,7 +15,7 @@ from selfdrive.version import version, terms_version, training_version, get_git_ get_git_branch, get_git_remote -def register(): +def register(spinner=None): params = Params() params.put("Version", version) params.put("TermsVersion", terms_version) @@ -23,10 +26,16 @@ def register(): params.put("GitRemote", get_git_remote(default="")) params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) + IMEI = params.get("IMEI", encoding='utf8') + HardwareSerial = params.get("HardwareSerial", encoding='utf8') + + needs_registration = (None in [IMEI, HardwareSerial]) + # create a key for auth # your private key is kept on your device persist partition and never sent to our servers # do not erase your persist partition if not os.path.isfile(PERSIST+"/comma/id_rsa.pub"): + needs_registration = True cloudlog.warning("generating your personal RSA key") mkdirs_exists_ok(PERSIST+"/comma") assert os.system("openssl genrsa -out "+PERSIST+"/comma/id_rsa.tmp 2048") == 0 @@ -39,31 +48,44 @@ def register(): os.chmod(PERSIST+'/comma/id_rsa', 0o744) dongle_id = params.get("DongleId", encoding='utf8') - public_key = open(PERSIST+"/comma/id_rsa.pub").read() + needs_registration = needs_registration or dongle_id is None - # create registration token - # in the future, this key will make JWTs directly - private_key = open(PERSIST+"/comma/id_rsa").read() + if needs_registration: + if spinner is not None: + spinner.update("registering device") - # late import - import jwt - register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') + # Create registration token, in the future, this key will make JWTs directly + private_key = open(PERSIST+"/comma/id_rsa").read() + public_key = open(PERSIST+"/comma/id_rsa.pub").read() + register_token = jwt.encode({'register': True, 'exp': datetime.utcnow() + timedelta(hours=1)}, private_key, algorithm='RS256') - try: - cloudlog.info("getting pilotauth") - resp = api_get("v2/pilotauth/", method='POST', timeout=15, - imei=HARDWARE.get_imei(0), imei2=HARDWARE.get_imei(1), serial=HARDWARE.get_serial(), public_key=public_key, register_token=register_token) - dongleauth = json.loads(resp.text) - dongle_id = dongleauth["dongle_id"] + # Block until we get the imei + imei1, imei2 = None, None + while imei1 is None and imei2 is None: + try: + imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) + except Exception: + cloudlog.exception("Error getting imei, trying again...") + time.sleep(1) - params.put("DongleId", dongle_id) - return dongle_id - except Exception: - cloudlog.exception("failed to authenticate") - if dongle_id is not None: - return dongle_id - else: - return None + serial = HARDWARE.get_serial() + params.put("IMEI", imei1) + params.put("HardwareSerial", serial) + + while True: + try: + cloudlog.info("getting pilotauth") + resp = api_get("v2/pilotauth/", method='POST', timeout=15, + imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) + dongleauth = json.loads(resp.text) + dongle_id = dongleauth["dongle_id"] + params.put("DongleId", dongle_id) + break + except Exception: + cloudlog.exception("failed to authenticate") + time.sleep(1) + + return dongle_id if __name__ == "__main__": print(register()) diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript index 01b0eec84..0817a4d30 100644 --- a/selfdrive/sensord/SConscript +++ b/selfdrive/sensord/SConscript @@ -1,9 +1,7 @@ Import('env', 'arch', 'common', 'cereal', 'messaging') + if arch == "aarch64": env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj']) - lenv = env.Clone() - lenv['LIBPATH'] += ['/system/vendor/lib64'] - lenv.Program('_gpsd', ['gpsd.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', cereal, messaging, 'capnp', 'zmq', 'kj']) else: sensors = [ 'sensors/file_sensor.cc', diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd deleted file mode 100755 index c30108268..000000000 --- a/selfdrive/sensord/gpsd +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/sh -export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" -exec ./_gpsd diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc deleted file mode 100644 index 1978a239c..000000000 --- a/selfdrive/sensord/gpsd.cc +++ /dev/null @@ -1,162 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -#include "messaging.hpp" -#include "common/timing.h" -#include "common/swaglog.h" - -volatile sig_atomic_t do_exit = 0; - -namespace { - -PubMaster *pm; - -const GpsInterface* gGpsInterface = NULL; -const AGpsInterface* gAGpsInterface = NULL; - -void set_do_exit(int sig) { - do_exit = 1; -} - -void nmea_callback(GpsUtcTime timestamp, const char* nmea, int length) { - - uint64_t log_time_wall = nanos_since_epoch(); - - MessageBuilder msg; - auto nmeaData = msg.initEvent().initGpsNMEA(); - nmeaData.setTimestamp(timestamp); - nmeaData.setLocalWallTime(log_time_wall); - nmeaData.setNmea(nmea); - - pm->send("gpsNMEA", msg); -} - -void location_callback(GpsLocation* location) { - //printf("got location callback\n"); - - MessageBuilder msg; - auto locationData = msg.initEvent().initGpsLocation(); - locationData.setFlags(location->flags); - locationData.setLatitude(location->latitude); - locationData.setLongitude(location->longitude); - locationData.setAltitude(location->altitude); - locationData.setSpeed(location->speed); - locationData.setBearing(location->bearing); - locationData.setAccuracy(location->accuracy); - locationData.setTimestamp(location->timestamp); - locationData.setSource(cereal::GpsLocationData::SensorSource::ANDROID); - - pm->send("gpsLocation", msg); -} - -pthread_t create_thread_callback(const char* name, void (*start)(void *), void* arg) { - LOG("creating thread: %s", name); - pthread_t thread; - pthread_attr_t attr; - int err; - - err = pthread_attr_init(&attr); - err = pthread_create(&thread, &attr, (void*(*)(void*))start, arg); - - return thread; -} - -GpsCallbacks gps_callbacks = { - sizeof(GpsCallbacks), - location_callback, - NULL, - NULL, - nmea_callback, - NULL, - NULL, - NULL, - create_thread_callback, -}; - -void agps_status_cb(AGpsStatus *status) { - switch (status->status) { - case GPS_REQUEST_AGPS_DATA_CONN: - fprintf(stdout, "*** data_conn_open\n"); - gAGpsInterface->data_conn_open("internet"); - break; - case GPS_RELEASE_AGPS_DATA_CONN: - fprintf(stdout, "*** data_conn_closed\n"); - gAGpsInterface->data_conn_closed(); - break; - } -} - -AGpsCallbacks agps_callbacks = { - agps_status_cb, - create_thread_callback, -}; - -void gps_init() { - pm = new PubMaster({"gpsNMEA", "gpsLocation"}); - LOG("*** init GPS"); - hw_module_t* module = NULL; - hw_get_module(GPS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); - assert(module); - - static hw_device_t* device = NULL; - module->methods->open(module, GPS_HARDWARE_MODULE_ID, &device); - assert(device); - - // ** get gps interface ** - gps_device_t* gps_device = (gps_device_t *)device; - gGpsInterface = gps_device->get_gps_interface(gps_device); - assert(gGpsInterface); - - gAGpsInterface = (const AGpsInterface*)gGpsInterface->get_extension(AGPS_INTERFACE); - assert(gAGpsInterface); - - - gGpsInterface->init(&gps_callbacks); - gAGpsInterface->init(&agps_callbacks); - gAGpsInterface->set_server(AGPS_TYPE_SUPL, "supl.google.com", 7276); - - // gGpsInterface->delete_aiding_data(GPS_DELETE_ALL); - gGpsInterface->start(); - gGpsInterface->set_position_mode(GPS_POSITION_MODE_MS_BASED, - GPS_POSITION_RECURRENCE_PERIODIC, - 100, 0, 0); - -} - -void gps_destroy() { - delete pm; - gGpsInterface->stop(); - gGpsInterface->cleanup(); -} - -} - -int main() { - setpriority(PRIO_PROCESS, 0, -13); - - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); - - gps_init(); - - while(!do_exit) pause(); - - gps_destroy(); - - return 0; -} diff --git a/selfdrive/sensord/sensors/bmx055_magn.cc b/selfdrive/sensord/sensors/bmx055_magn.cc index fbe43c206..f05570313 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.cc +++ b/selfdrive/sensord/sensors/bmx055_magn.cc @@ -1,11 +1,67 @@ #include #include +#include #include #include "common/swaglog.h" +#include "common/util.h" #include "bmx055_magn.hpp" +static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) { + uint16_t process_comp_x0 = data_rhall; + int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384; + uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_x2); + int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval)); + int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128)); + int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128); + int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5; + int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000)); + int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0))); + int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096); + int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9; + retval = ((int16_t)(process_comp_x10 / 8192)); + retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) { + uint16_t process_comp_y0 = trim_data.dig_xyz1; + int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0; + uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000); + int16_t retval = ((int16_t)process_comp_y2); + int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval); + int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128); + int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128)); + int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512); + int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0))); + int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096); + int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8); + retval = (int16_t)(process_comp_y9 / 8192); + retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16; + + return retval; +} + +static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) { + int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1); + int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4; + int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768); + int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2); + int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536); + int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4)); + + /* saturate result to +/- 2 micro-tesla */ + retval = std::clamp(retval, -32767, 32767); + + /* Conversion of LSB to micro-tesla*/ + retval = retval / 16; + + return (int16_t)retval; +} + static int16_t parse_xy(uint8_t lsb, uint8_t msb){ // 13 bit uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3); @@ -18,18 +74,19 @@ static int16_t parse_z(uint8_t lsb, uint8_t msb){ return int16_t(combined << 1) / (1 << 1); } -/* static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){ // 14 bit return (uint16_t(msb) << 6) | uint16_t(lsb >> 2); } -*/ BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {} int BMX055_Magn::init(){ int ret; uint8_t buffer[1]; + uint8_t trim_x1y1[2] = {0}; + uint8_t trim_xyz_data[4] = {0}; + uint8_t trim_xy1xy2[10] = {0}; // suspend -> sleep ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); @@ -37,7 +94,7 @@ int BMX055_Magn::init(){ LOGE("Enabling power failed: %d", ret); goto fail; } - usleep(5 * 1000); // wait until the chip is powered on + util::sleep_for(5); // wait until the chip is powered on // read chip ID ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1); @@ -51,6 +108,37 @@ int BMX055_Magn::init(){ return -1; } + // Load magnetometer trim + ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2); + if(ret < 0){ + goto fail; + } + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4, trim_xyz_data, 4); + if(ret < 0){ + goto fail; + } + ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2, trim_xy1xy2, 10); + if(ret < 0){ + goto fail; + } + + // Read trim data + trim_data.dig_x1 = (int8_t)trim_x1y1[0]; + trim_data.dig_y1 = (int8_t)trim_x1y1[1]; + + trim_data.dig_x2 = (int8_t)trim_xyz_data[2]; + trim_data.dig_y2 = (int8_t)trim_xyz_data[3]; + + trim_data.dig_z1 = read_16_bit(trim_xy1xy2[2], trim_xy1xy2[3]); + trim_data.dig_z2 = read_16_bit(trim_xy1xy2[0], trim_xy1xy2[1]); + trim_data.dig_z3 = read_16_bit(trim_xy1xy2[6], trim_xy1xy2[7]); + trim_data.dig_z4 = read_16_bit(trim_xyz_data[0], trim_xyz_data[1]); + + trim_data.dig_xy1 = trim_xy1xy2[9]; + trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8]; + + trim_data.dig_xyz1 = read_16_bit(trim_xy1xy2[4], trim_xy1xy2[5] & 0x7f); + // TODO: perform self-test // 9 REPXY and 15 REPZ for 100 Hz @@ -81,10 +169,14 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ bool ready = buffer[6] & 0x1; if (ready){ - float x = parse_xy(buffer[0], buffer[1]); - float y = parse_xy(buffer[2], buffer[3]); - float z = parse_z(buffer[4], buffer[5]); - //uint16_t rhall = parse_rhall(buffer[5], buffer[6]); + int16_t x = parse_xy(buffer[0], buffer[1]); + int16_t y = parse_xy(buffer[2], buffer[3]); + int16_t z = parse_z(buffer[4], buffer[5]); + int16_t rhall = parse_rhall(buffer[5], buffer[6]); + + x = compensate_x(trim_data, x, rhall); + y = compensate_y(trim_data, y, rhall); + z = compensate_z(trim_data, z, rhall); // TODO: convert to micro tesla: // https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c#L1614 @@ -95,7 +187,7 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){ event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED); event.setTimestamp(start_time); - float xyz[] = {x, y, z}; + float xyz[] = {(float)x, (float)y, (float)z}; auto svec = event.initMagneticUncalibrated(); svec.setV(xyz); svec.setStatus(true); diff --git a/selfdrive/sensord/sensors/bmx055_magn.hpp b/selfdrive/sensord/sensors/bmx055_magn.hpp index ba4942d72..51fdd7278 100644 --- a/selfdrive/sensord/sensors/bmx055_magn.hpp +++ b/selfdrive/sensord/sensors/bmx055_magn.hpp @@ -14,12 +14,32 @@ #define BMX055_MAGN_I2C_REG_REPXY 0x51 #define BMX055_MAGN_I2C_REG_REPZ 0x52 +#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D +#define BMX055_MAGN_I2C_REG_DIG_Z4 0x62 +#define BMX055_MAGN_I2C_REG_DIG_Z2 0x68 + // Constants #define BMX055_MAGN_CHIP_ID 0x32 #define BMX055_MAGN_FORCED (0b01 << 1) +struct trim_data_t { + int8_t dig_x1; + int8_t dig_y1; + int8_t dig_x2; + int8_t dig_y2; + uint16_t dig_z1; + int16_t dig_z2; + int16_t dig_z3; + int16_t dig_z4; + uint8_t dig_xy1; + int8_t dig_xy2; + uint16_t dig_xyz1; +}; + + class BMX055_Magn : public I2CSensor{ uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;} + trim_data_t trim_data = {0}; public: BMX055_Magn(I2CBus *bus); int init(); diff --git a/selfdrive/sensord/sensors/sensor.hpp b/selfdrive/sensord/sensors/sensor.hpp index 91f417908..3fb58ad2a 100644 --- a/selfdrive/sensord/sensors/sensor.hpp +++ b/selfdrive/sensord/sensors/sensor.hpp @@ -4,6 +4,7 @@ class Sensor { public: + virtual ~Sensor() {}; virtual int init() = 0; virtual void get_event(cereal::SensorEventData::Builder &event) = 0; }; diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index 16b7fc98b..77e670d3f 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -2,7 +2,6 @@ #include #include #include -#include #include #include #include @@ -19,6 +18,7 @@ #include "messaging.hpp" #include "common/timing.h" +#include "common/util.h" #include "common/swaglog.h" // ACCELEROMETER_UNCALIBRATED is only in Android O @@ -32,15 +32,11 @@ #define SENSOR_PROXIMITY 6 #define SENSOR_LIGHT 7 -volatile sig_atomic_t do_exit = 0; +ExitHandler do_exit; volatile sig_atomic_t re_init_sensors = 0; namespace { -void set_do_exit(int sig) { - do_exit = 1; -} - void sigpipe_handler(int sig) { LOGE("SIGPIPE received"); re_init_sensors = true; @@ -53,7 +49,7 @@ void sensor_loop() { bool low_power_mode = false; while (!do_exit) { - SubMaster sm({"thermal"}); + SubMaster sm({"deviceState"}); PubMaster pm({"sensorEvents"}); struct sensors_poll_device_t* device; @@ -202,7 +198,7 @@ void sensor_loop() { // Check whether to go into low power mode at 5Hz if (frame % 20 == 0 && sm.update(0) > 0) { - bool offroad = !sm["thermal"].getThermal().getStarted(); + bool offroad = !sm["deviceState"].getDeviceState().getStarted(); if (low_power_mode != offroad) { for (auto &s : sensors) { device->activate(device, s.first, 0); @@ -224,8 +220,6 @@ void sensor_loop() { int main(int argc, char *argv[]) { setpriority(PRIO_PROCESS, 0, -13); - signal(SIGINT, (sighandler_t)set_do_exit); - signal(SIGTERM, (sighandler_t)set_do_exit); signal(SIGPIPE, (sighandler_t)sigpipe_handler); sensor_loop(); diff --git a/selfdrive/sensord/sensors_qcom2.cc b/selfdrive/sensord/sensors_qcom2.cc index 1dd5ed10a..1cc685fe3 100644 --- a/selfdrive/sensord/sensors_qcom2.cc +++ b/selfdrive/sensord/sensors_qcom2.cc @@ -1,5 +1,4 @@ #include -#include #include #include #include @@ -7,6 +6,7 @@ #include "messaging.hpp" #include "common/i2c.h" #include "common/timing.h" +#include "common/util.h" #include "common/swaglog.h" #include "sensors/sensor.hpp" @@ -23,14 +23,9 @@ #include "sensors/light_sensor.hpp" -volatile sig_atomic_t do_exit = 0; - #define I2C_BUS_IMU 1 - -void set_do_exit(int sig) { - do_exit = 1; -} +ExitHandler do_exit; int sensor_loop() { I2CBus *i2c_bus_imu; @@ -99,8 +94,5 @@ int sensor_loop() { int main(int argc, char *argv[]) { setpriority(PRIO_PROCESS, 0, -13); - signal(SIGINT, set_do_exit); - signal(SIGTERM, set_do_exit); - return sensor_loop(); } diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index d0140366f..b605a8e8c 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -8,14 +8,16 @@ if [ -z "$GIT_COMMIT" ]; then fi if [ -z "$TEST_DIR" ]; then - echo "TEST_DIR must be set" exit 1 fi -# TODO: never clear qcom_replay cache +if [ ! -d "$SOURCE_DIR" ]; then + git clone https://github.com/commaai/openpilot.git "$SOURCE_DIR" +fi + # clear scons cache dirs that haven't been written to in one day -cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime +1 -exec rm -rf '{}' \; +#cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime +1 -exec rm -rf '{}' \; # this can get really big on the CI devices rm -rf /data/core @@ -23,12 +25,12 @@ rm -rf /data/core # set up environment cd $SOURCE_DIR git reset --hard -git fetch origin +git fetch find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; git reset --hard $GIT_COMMIT git checkout $GIT_COMMIT git clean -xdf -git submodule update --init +git submodule update --init --recursive git submodule foreach --recursive git reset --hard git submodule foreach --recursive git clean -xdf echo "git checkout took $SECONDS seconds" diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py deleted file mode 100755 index b6147d179..000000000 --- a/selfdrive/test/test_cpu_usage.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python3 -import os -import time -import sys -import subprocess - -import cereal.messaging as messaging -from common.basedir import BASEDIR -from selfdrive.test.helpers import set_params_enabled - -def cputime_total(ct): - return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem - - -def print_cpu_usage(first_proc, last_proc): - procs = [ - ("selfdrive.controls.controlsd", 47.0), - ("./loggerd", 42.0), - ("selfdrive.locationd.locationd", 35.0), - ("selfdrive.locationd.paramsd", 12.0), - ("selfdrive.controls.plannerd", 10.0), - ("./_modeld", 7.12), - ("./camerad", 7.07), - ("./_sensord", 6.17), - ("./_ui", 5.82), - ("selfdrive.controls.radard", 5.67), - ("./boardd", 3.63), - ("./_dmonitoringmodeld", 2.67), - ("selfdrive.logmessaged", 1.7), - ("selfdrive.thermald.thermald", 2.41), - ("selfdrive.locationd.calibrationd", 2.0), - ("selfdrive.monitoring.dmonitoringd", 1.90), - ("./proclogd", 1.54), - ("./_gpsd", 0.09), - ("./clocksd", 0.02), - ("./ubloxd", 0.02), - ("selfdrive.tombstoned", 0), - ("./logcatd", 0), - ] - - r = True - dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9 - result = "------------------------------------------------\n" - for proc_name, normal_cpu_usage in procs: - try: - first = [p for p in first_proc.procLog.procs if proc_name in p.cmdline][0] - last = [p for p in last_proc.procLog.procs if proc_name in p.cmdline][0] - cpu_time = cputime_total(last) - cputime_total(first) - cpu_usage = cpu_time / dt * 100. - if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0): - result += f"Warning {proc_name} using more CPU than normal\n" - r = False - elif cpu_usage < min(normal_cpu_usage * 0.65, max(normal_cpu_usage - 1.0, 0.0)): - result += f"Warning {proc_name} using less CPU than normal\n" - r = False - result += f"{proc_name.ljust(35)} {cpu_usage:.2f}%\n" - except IndexError: - result += f"{proc_name.ljust(35)} NO METRICS FOUND\n" - r = False - result += "------------------------------------------------\n" - print(result) - return r - -def test_cpu_usage(): - cpu_ok = False - - # start manager - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" - manager_path = os.path.join(BASEDIR, "selfdrive/manager.py") - manager_proc = subprocess.Popen(["python", manager_path]) - try: - proc_sock = messaging.sub_sock('procLog', conflate=True, timeout=2000) - cs_sock = messaging.sub_sock('controlsState', conflate=True) - - # wait until everything's started - start_time = time.monotonic() - while time.monotonic() - start_time < 240: - msg = messaging.recv_sock(cs_sock, wait=True) - if msg is not None: - break - - # take first sample - time.sleep(15) - first_proc = messaging.recv_sock(proc_sock, wait=True) - if first_proc is None: - raise Exception("\n\nTEST FAILED: progLog recv timed out\n\n") - - # run for a minute and get last sample - time.sleep(60) - last_proc = messaging.recv_sock(proc_sock, wait=True) - cpu_ok = print_cpu_usage(first_proc, last_proc) - finally: - manager_proc.terminate() - ret = manager_proc.wait(20) - if ret is None: - manager_proc.kill() - return cpu_ok - -if __name__ == "__main__": - set_params_enabled() - - passed = False - try: - passed = test_cpu_usage() - except Exception as e: - print("\n\n\n", "TEST FAILED:", str(e), "\n\n\n") - finally: - sys.exit(int(not passed)) diff --git a/selfdrive/test/test_manager.py b/selfdrive/test/test_manager.py new file mode 100644 index 000000000..9a666e967 --- /dev/null +++ b/selfdrive/test/test_manager.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +import os +import signal +import time +import unittest + +os.environ['FAKEUPLOAD'] = "1" + +import selfdrive.manager as manager +from selfdrive.hardware import EON + +# TODO: make eon fast +MAX_STARTUP_TIME = 30 if EON else 15 +ALL_PROCESSES = manager.persistent_processes + manager.car_started_processes + +class TestManager(unittest.TestCase): + + def setUp(self): + os.environ['PASSIVE'] = '0' + + def tearDown(self): + manager.cleanup_all_processes(None, None) + + def test_manager_prepare(self): + os.environ['PREPAREONLY'] = '1' + manager.main() + + def test_startup_time(self): + for _ in range(10): + start = time.monotonic() + os.environ['PREPAREONLY'] = '1' + manager.main() + t = time.monotonic() - start + assert t < MAX_STARTUP_TIME, f"startup took {t}s, expected <{MAX_STARTUP_TIME}s" + + # ensure all processes exit cleanly + def test_clean_exit(self): + manager.manager_prepare() + for p in ALL_PROCESSES: + manager.start_managed_process(p) + + time.sleep(10) + + for p in reversed(ALL_PROCESSES): + exit_code = manager.kill_managed_process(p, retry=False) + if (not EON and p == 'ui') or (EON and p == 'logcatd'): + # TODO: make Qt UI exit gracefully and fix OMX encoder exiting + continue + + # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code + exit_codes = [0, 1] + if p in manager.kill_processes: + exit_codes = [-signal.SIGKILL] + assert exit_code in exit_codes, f"{p} died with {exit_code}" + + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py deleted file mode 100644 index 75e8192f4..000000000 --- a/selfdrive/test/test_openpilot.py +++ /dev/null @@ -1,178 +0,0 @@ -# flake8: noqa -import os -os.environ['FAKEUPLOAD'] = "1" - -from common.params import Params -from common.realtime import sec_since_boot -from selfdrive.manager import manager_init, manager_prepare, start_daemon_process -from selfdrive.test.helpers import phone_only, with_processes, set_params_enabled -import json -import requests -import signal -import subprocess -import time - - -# must run first -@phone_only -def test_manager_prepare(): - set_params_enabled() - manager_init() - manager_prepare() - -@phone_only -@with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd']) -def test_logging(): - print("LOGGING IS SET UP") - time.sleep(1.0) - -@phone_only -@with_processes(['camerad', 'modeld', 'dmonitoringmodeld']) -def test_visiond(): - print("VISIOND IS SET UP") - time.sleep(5.0) - -@phone_only -@with_processes(['sensord']) -def test_sensord(): - print("SENSORS ARE SET UP") - time.sleep(1.0) - -@phone_only -@with_processes(['ui']) -def test_ui(): - print("RUNNING UI") - time.sleep(1.0) - -# will have one thing to upload if loggerd ran -# TODO: assert it actually uploaded -@phone_only -@with_processes(['uploader']) -def test_uploader(): - print("UPLOADER") - time.sleep(10.0) - -@phone_only -def test_athena(): - print("ATHENA") - start = sec_since_boot() - start_daemon_process("manage_athenad") - params = Params() - manage_athenad_pid = params.get("AthenadPid") - assert manage_athenad_pid is not None - try: - os.kill(int(manage_athenad_pid), 0) - # process is running - except OSError: - assert False, "manage_athenad is dead" - - def expect_athena_starts(timeout=30): - now = time.time() - athenad_pid = None - while athenad_pid is None: - try: - athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() - return athenad_pid - except subprocess.CalledProcessError: - if time.time() - now > timeout: - assert False, f"Athena did not start within {timeout} seconds" - time.sleep(0.5) - - def athena_post(payload, max_retries=5, wait=5): - tries = 0 - while 1: - try: - resp = requests.post( - "https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"), - headers={ - "Authorization": "JWT " + os.getenv("COMMA_JWT"), - "Content-Type": "application/json" - }, - data=json.dumps(payload), - timeout=30 - ) - resp_json = resp.json() - if resp_json.get('error'): - raise Exception(resp_json['error']) - return resp_json - except Exception as e: - time.sleep(wait) - tries += 1 - if tries == max_retries: - raise - else: - print(f'athena_post failed {e}. retrying...') - - def expect_athena_registers(test_t0): - resp = athena_post({ - "method": "echo", - "params": ["hello"], - "id": 0, - "jsonrpc": "2.0" - }, max_retries=12, wait=5) - assert resp.get('result') == "hello", f'Athena failed to register ({resp})' - - last_pingtime = params.get("LastAthenaPingTime", encoding='utf8') - assert last_pingtime, last_pingtime - assert ((int(last_pingtime)/1e9) - test_t0) < (sec_since_boot() - test_t0) - - try: - athenad_pid = expect_athena_starts() - # kill athenad and ensure it is restarted (check_output will throw if it is not) - os.kill(int(athenad_pid), signal.SIGINT) - expect_athena_starts() - - if not os.getenv('COMMA_JWT'): - print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai') - return - - expect_athena_registers(start) - - print("ATHENA: getSimInfo") - resp = athena_post({ - "method": "getSimInfo", - "id": 0, - "jsonrpc": "2.0" - }) - assert resp.get('result'), resp - assert 'sim_id' in resp['result'], resp['result'] - - print("ATHENA: takeSnapshot") - resp = athena_post({ - "method": "takeSnapshot", - "id": 0, - "jsonrpc": "2.0" - }) - assert resp.get('result'), resp - assert resp['result']['jpegBack'], resp['result'] - - @with_processes(["thermald"]) - def test_athena_thermal(): - print("ATHENA: getMessage(thermal)") - resp = athena_post({ - "method": "getMessage", - "params": {"service": "thermal", "timeout": 5000}, - "id": 0, - "jsonrpc": "2.0" - }) - assert resp.get('result'), resp - assert resp['result']['thermal'], resp['result'] - test_athena_thermal() - finally: - try: - athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() - except subprocess.CalledProcessError: - athenad_pid = None - - try: - os.kill(int(manage_athenad_pid), signal.SIGINT) - os.kill(int(athenad_pid), signal.SIGINT) - except (OSError, TypeError): - pass - -# TODO: re-enable when jenkins test has /data/pythonpath -> /data/openpilot -# @phone_only -# @with_apks() -# def test_apks(): -# print("APKS") -# time.sleep(14.0) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 5d59cf387..c790f8d60 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -26,7 +26,7 @@ class PowerMonitoring: self.last_save_time = 0 # Used for saving current value in a param self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None - self.car_voltage_mV = 12e3 # Low-passed version of health voltage + self.car_voltage_mV = 12e3 # Low-passed version of pandaState voltage self.integration_lock = threading.Lock() car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") @@ -38,12 +38,12 @@ class PowerMonitoring: # Calculation tick - def calculate(self, health): + def calculate(self, pandaState): try: now = sec_since_boot() - # If health is None, we're probably not in a car, so we don't care - if health is None or health.health.hwType == log.HealthData.HwType.unknown: + # If pandaState is None, we're probably not in a car, so we don't care + if pandaState is None or pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None @@ -51,7 +51,7 @@ class PowerMonitoring: return # Low-pass battery voltage - self.car_voltage_mV = ((health.health.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + self.car_voltage_mV = ((pandaState.pandaState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) # Cap the car battery power and save it in a param every 10-ish seconds self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) @@ -66,7 +66,7 @@ class PowerMonitoring: self.last_measurement_time = now return - if (health.health.ignitionLine or health.health.ignitionCan): + if (pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan): # If there is ignition, we integrate the charging rate of the car with self.integration_lock: self.power_used_uWh = 0 @@ -77,9 +77,9 @@ class PowerMonitoring: self.last_measurement_time = now else: # No ignition, we integrate the offroad power used by the device - is_uno = health.health.hwType == log.HealthData.HwType.uno + is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno # Get current power draw somehow - current_power = HARDWARE.get_current_power_draw() + current_power = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none if current_power is not None: pass elif HARDWARE.get_battery_status() == 'Discharging': @@ -154,8 +154,8 @@ class PowerMonitoring: return int(self.car_battery_capacity_uWh) # See if we need to disable charging - def should_disable_charging(self, health, offroad_timestamp): - if health is None or offroad_timestamp is None: + def should_disable_charging(self, pandaState, offroad_timestamp): + if pandaState is None or offroad_timestamp is None: return False now = sec_since_boot() @@ -163,22 +163,23 @@ class PowerMonitoring: disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) disable_charging |= (self.car_battery_capacity_uWh <= 0) - disable_charging &= (not health.health.ignitionLine and not health.health.ignitionCan) + disable_charging &= (not pandaState.pandaState.ignitionLine and not pandaState.pandaState.ignitionCan) disable_charging &= (self.params.get("DisablePowerDown") != b"1") + disable_charging |= (self.params.get("ForcePowerDown") == b"1") return disable_charging # See if we need to shutdown - def should_shutdown(self, health, offroad_timestamp, started_seen, LEON): - if health is None or offroad_timestamp is None: + def should_shutdown(self, pandaState, offroad_timestamp, started_seen, LEON): + if pandaState is None or offroad_timestamp is None: return False now = sec_since_boot() - panda_charging = (health.health.usbPowerMode != log.HealthData.UsbPowerMode.client) + panda_charging = (pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client) BATT_PERC_OFF = 10 if LEON else 3 should_shutdown = False # Wait until we have shut down charging before powering down - should_shutdown |= (not panda_charging and self.should_disable_charging(health, offroad_timestamp)) + should_shutdown |= (not panda_charging and self.should_disable_charging(pandaState, offroad_timestamp)) should_shutdown |= ((HARDWARE.get_battery_capacity() < BATT_PERC_OFF) and (not HARDWARE.get_battery_charging()) and ((now - offroad_timestamp) > 60)) should_shutdown &= started_seen return should_shutdown diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 843f9fa1e..23f8602b2 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -2,7 +2,6 @@ import datetime import os import time -from collections import namedtuple from typing import Dict, Optional, Tuple import psutil @@ -15,20 +14,18 @@ from common.numpy_fast import clip, interp from common.params import Params from common.realtime import DT_TRML, sec_since_boot from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import EON, HARDWARE, TICI +from selfdrive.hardware import EON, HARDWARE from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature from selfdrive.swaglog import cloudlog from selfdrive.thermald.power_monitoring import PowerMonitoring from selfdrive.version import get_git_branch, terms_version, training_version -ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient']) - FW_SIGNATURE = get_expected_signature() -ThermalStatus = log.ThermalData.ThermalStatus -NetworkType = log.ThermalData.NetworkType -NetworkStrength = log.ThermalData.NetworkStrength +ThermalStatus = log.DeviceState.ThermalStatus +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength CURRENT_TAU = 15. # 15s time constant CPU_TEMP_TAU = 5. # 5s time constant DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet @@ -40,35 +37,24 @@ prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} LEON = False last_eon_fan_val = None - -def get_thermal_config(): - # (tz, scale) - if EON: - return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1)) - elif TICI: - return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(70, 1000)) - else: - return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1)) - - def read_tz(x): if x is None: return 0 try: - with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: + with open(f"/sys/devices/virtual/thermal/thermal_zone{x}/temp") as f: return int(f.read()) except FileNotFoundError: return 0 def read_thermal(thermal_config): - dat = messaging.new_message('thermal') - dat.thermal.cpu = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]] - dat.thermal.gpu = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]] - dat.thermal.mem = read_tz(thermal_config.mem[0]) / thermal_config.mem[1] - dat.thermal.ambient = read_tz(thermal_config.ambient[0]) / thermal_config.ambient[1] - dat.thermal.bat = read_tz(thermal_config.bat[0]) / thermal_config.bat[1] + dat = messaging.new_message('deviceState') + dat.deviceState.cpuTempC = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]] + dat.deviceState.gpuTempC = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]] + dat.deviceState.memoryTempC = read_tz(thermal_config.mem[0]) / thermal_config.mem[1] + dat.deviceState.ambientTempC = read_tz(thermal_config.ambient[0]) / thermal_config.ambient[1] + dat.deviceState.batteryTempC = read_tz(thermal_config.bat[0]) / thermal_config.bat[1] return dat @@ -162,12 +148,12 @@ def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_tex def thermald_thread(): - health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency - # now loop - thermal_sock = messaging.pub_sock('thermal') - health_sock = messaging.sub_sock('health', timeout=health_timeout) - location_sock = messaging.sub_sock('gpsLocation') + pm = messaging.PubMaster(['deviceState']) + + pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency + pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout) + location_sock = messaging.sub_sock('gpsLocationExternal') fan_speed = 0 count = 0 @@ -189,28 +175,26 @@ def thermald_thread(): current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML) - health_prev = None + pandaState_prev = None should_start_prev = False handle_fan = None is_uno = False params = Params() - pm = PowerMonitoring() + power_monitor = PowerMonitoring() no_panda_cnt = 0 - thermal_config = get_thermal_config() + thermal_config = HARDWARE.get_thermal_config() while 1: - health = messaging.recv_sock(health_sock, wait=True) - location = messaging.recv_sock(location_sock) - location = location.gpsLocation if location else None + pandaState = messaging.recv_sock(pandaState_sock, wait=True) msg = read_thermal(thermal_config) - if health is not None: - usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client + if pandaState is not None: + usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client # If we lose connection to the panda, wait 5 seconds before going offroad - if health.health.hwType == log.HealthData.HwType.unknown: + if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: @@ -218,11 +202,11 @@ def thermald_thread(): startup_conditions["ignition"] = False else: no_panda_cnt = 0 - startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan + startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan # Setup fan handler on first connect to panda - if handle_fan is None and health.health.hwType != log.HealthData.HwType.unknown: - is_uno = health.health.hwType == log.HealthData.HwType.uno + if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown: + is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno if (not EON) or is_uno: cloudlog.info("Setting up UNO fan handler") @@ -233,11 +217,11 @@ def thermald_thread(): handle_fan = handle_fan_eon # Handle disconnect - if health_prev is not None: - if health.health.hwType == log.HealthData.HwType.unknown and \ - health_prev.health.hwType != log.HealthData.HwType.unknown: + if pandaState_prev is not None: + if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ + pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: params.panda_disconnect() - health_prev = health + pandaState_prev = pandaState # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: @@ -247,33 +231,33 @@ def thermald_thread(): except Exception: cloudlog.exception("Error getting network status") - msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 - msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) - msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) - msg.thermal.networkType = network_type - msg.thermal.networkStrength = network_strength - msg.thermal.batteryPercent = HARDWARE.get_battery_capacity() - msg.thermal.batteryStatus = HARDWARE.get_battery_status() - msg.thermal.batteryCurrent = HARDWARE.get_battery_current() - msg.thermal.batteryVoltage = HARDWARE.get_battery_voltage() - msg.thermal.usbOnline = HARDWARE.get_usb_present() + msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) + msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) + msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent())) + msg.deviceState.networkType = network_type + msg.deviceState.networkStrength = network_strength + msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() + msg.deviceState.batteryStatus = HARDWARE.get_battery_status() + msg.deviceState.batteryCurrent = HARDWARE.get_battery_current() + msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage() + msg.deviceState.usbOnline = HARDWARE.get_usb_present() # Fake battery levels on uno for frame if (not EON) or is_uno: - msg.thermal.batteryPercent = 100 - msg.thermal.batteryStatus = "Charging" - msg.thermal.bat = 0 + msg.deviceState.batteryPercent = 100 + msg.deviceState.batteryStatus = "Charging" + msg.deviceState.batteryTempC = 0 - current_filter.update(msg.thermal.batteryCurrent / 1e6) + current_filter.update(msg.deviceState.batteryCurrent / 1e6) # TODO: add car battery voltage check - max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu)) - max_comp_temp = max(max_cpu_temp, msg.thermal.mem, max(msg.thermal.gpu)) - bat_temp = msg.thermal.bat + max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC)) + max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC)) + bat_temp = msg.deviceState.batteryTempC if handle_fan is not None: fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"]) - msg.thermal.fanSpeed = fan_speed + msg.deviceState.fanSpeedPercentDesired = fan_speed # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 @@ -295,8 +279,7 @@ def thermald_thread(): # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) else: - # all good - thermal_status = ThermalStatus.green + thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** @@ -341,6 +324,7 @@ def thermald_thread(): set_offroad_alert_if_changed("Offroad_ConnectivityNeeded", False) set_offroad_alert_if_changed("Offroad_ConnectivityNeededPrompt", False) + startup_conditions["up_to_date"] = params.get("Offroad_ConnectivityNeeded") is None or params.get("DisableUpdates") == b"1" startup_conditions["not_uninstalling"] = not params.get("DoUninstall") == b"1" startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version @@ -349,7 +333,7 @@ def thermald_thread(): set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"])) # with 2% left, we killall, otherwise the phone will take a long time to boot - startup_conditions["free_space"] = msg.thermal.freeSpace > 0.02 + startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2 startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \ (current_branch in ['dashcam', 'dashcam-staging']) startup_conditions["not_driver_view"] = not params.get("IsDriverViewEnabled") == b"1" @@ -358,12 +342,13 @@ def thermald_thread(): # controls will warn with CPU above 95 or battery above 60 startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"])) + + startup_conditions["hardware_supported"] = pandaState is not None and pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda, + log.PandaState.PandaType.greyPanda] + set_offroad_alert_if_changed("Offroad_HardwareUnsupported", pandaState is not None and not startup_conditions["hardware_supported"]) + + # Handle offroad/onroad transition should_start = all(startup_conditions.values()) - - startup_conditions["hardware_supported"] = health is not None and health.health.hwType not in [log.HealthData.HwType.whitePanda, - log.HealthData.HwType.greyPanda] - set_offroad_alert_if_changed("Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"]) - if should_start: if not should_start_prev: params.delete("IsOffroad") @@ -375,6 +360,7 @@ def thermald_thread(): else: if startup_conditions["ignition"] and (startup_conditions != startup_conditions_prev): cloudlog.event("Startup blocked", startup_conditions=startup_conditions) + if should_start_prev or (count == 0): params.put("IsOffroad", "1") @@ -383,26 +369,26 @@ def thermald_thread(): off_ts = sec_since_boot() # Offroad power monitoring - pm.calculate(health) - msg.thermal.offroadPowerUsage = pm.get_power_used() - msg.thermal.carBatteryCapacity = max(0, pm.get_car_battery_capacity()) + power_monitor.calculate(pandaState) + msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() + msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) - msg.thermal.chargingDisabled = pm.should_disable_charging(health, off_ts) + msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts) # Check if we need to shut down - if pm.should_shutdown(health, off_ts, started_seen, LEON): + if power_monitor.should_shutdown(pandaState, off_ts, started_seen, LEON): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) - os.system('LD_LIBRARY_PATH="" svc power shutdown') + HARDWARE.shutdown() - msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # 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.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged + msg.deviceState.started = started_ts is not None + msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0)) - msg.thermal.thermalStatus = thermal_status - thermal_sock.send(msg.to_bytes()) + msg.deviceState.thermalStatus = thermal_status + pm.send("deviceState", msg) set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power)) @@ -411,11 +397,12 @@ def thermald_thread(): # report to server once per minute if (count % int(60. / DT_TRML)) == 0: + location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, - health=(health.to_dict() if health else None), - location=(location.to_dict() if location else None), - thermal=msg.to_dict()) + pandaState=(pandaState.to_dict() if pandaState else None), + location=(location.gpsLocationExternal.to_dict() if location else None), + deviceState=msg.to_dict()) count += 1 diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 8ae0a147b..e9ec73b7f 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -1,21 +1,70 @@ #!/usr/bin/env python3 +import datetime import os +import re +import shutil +import signal +import subprocess import time +import glob from raven import Client from raven.transport.http import HTTPTransport -from selfdrive.version import version, origin, branch, dirty +from common.file_helpers import mkdirs_exists_ok +from selfdrive.hardware import TICI +from selfdrive.loggerd.config import ROOT from selfdrive.swaglog import cloudlog +from selfdrive.version import branch, commit, dirty, origin, version -MAX_SIZE = 100000 * 10 # Normal size is 40-100k, allow up to 1M +MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M +if TICI: + MAX_SIZE = MAX_SIZE * 100 # Allow larger size for tici since files contain coredump +MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") + +TOMBSTONE_DIR = "/data/tombstones/" +APPORT_DIR = "/var/crash/" + + +def safe_fn(s): + extra = ['_'] + return "".join(c for c in s if c.isalnum() or c in extra).rstrip() + + +def sentry_report(client, fn, message, contents): + cloudlog.error({'tombstone': message}) + client.captureMessage( + message=message, + sdk={'name': 'tombstoned', 'version': '0'}, + extra={ + 'tombstone_fn': fn, + 'tombstone': contents + }, + ) + +def clear_apport_folder(): + for f in glob.glob(APPORT_DIR + '*'): + try: + os.remove(f) + except Exception: + pass + + +def get_apport_stacktrace(fn): + try: + cmd = f'apport-retrace -s <(cat <(echo "Package: openpilot") "{fn}")' + return subprocess.check_output(cmd, shell=True, encoding='utf8', timeout=30, executable='/bin/bash') # pylint: disable=unexpected-keyword-arg + except subprocess.CalledProcessError: + return "Error getting stacktrace" + except subprocess.TimeoutExpired: + return "Timeout getting stacktrace" def get_tombstones(): """Returns list of (filename, ctime) for all tombstones in /data/tombstones and apport crashlogs in /var/crash""" files = [] - for folder in ["/data/tombstones/", "/var/crash/"]: + for folder in [TOMBSTONE_DIR, APPORT_DIR]: if os.path.exists(folder): with os.scandir(folder) as d: @@ -28,7 +77,7 @@ def get_tombstones(): return files -def report_tombstone(fn, client): +def report_tombstone_android(fn, client): f_size = os.path.getsize(fn) if f_size > MAX_SIZE: cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") @@ -37,41 +86,103 @@ def report_tombstone(fn, client): with open(fn, encoding='ISO-8859-1') as f: contents = f.read() - # Get summary for sentry title - if fn.endswith(".crash"): - lines = contents.split('\n') - message = lines[6] + message = " ".join(contents.split('\n')[5:7]) - status_idx = contents.find('ProcStatus') - if status_idx >= 0: - lines = contents[status_idx:].split('\n') - message += " " + lines[1] - else: - message = " ".join(contents.split('\n')[5:7]) + # Cut off pid/tid, since that varies per run + name_idx = message.find('name') + if name_idx >= 0: + message = message[name_idx:] - # Cut off pid/tid, since that varies per run - name_idx = message.find('name') - if name_idx >= 0: - message = message[name_idx:] + # Cut off fault addr + fault_idx = message.find(', fault addr') + if fault_idx >= 0: + message = message[:fault_idx] - # Cut off fault addr - fault_idx = message.find(', fault addr') - if fault_idx >= 0: - message = message[:fault_idx] + sentry_report(client, fn, message, contents) - cloudlog.error({'tombstone': message}) - client.captureMessage( - message=message, - sdk={'name': 'tombstoned', 'version': '0'}, - extra={ - 'tombstone_fn': fn, - 'tombstone': contents - }, - ) +def report_tombstone_apport(fn, client): + f_size = os.path.getsize(fn) + if f_size > MAX_SIZE: + cloudlog.error(f"Tombstone {fn} too big, {f_size}. Skipping...") + return + + message = "" # One line description of the crash + contents = "" # Full file contents without coredump + path = "" # File path relative to openpilot directory + + proc_maps = False + + with open(fn) as f: + for line in f: + if "CoreDump" in line: + break + elif "ProcMaps" in line: + proc_maps = True + elif "ProcStatus" in line: + proc_maps = False + + if not proc_maps: + contents += line + + if "ExecutablePath" in line: + path = line.strip().split(': ')[-1] + path = path.replace('/data/openpilot/', '') + message += path + elif "Signal" in line: + message += " - " + line.strip() + + try: + sig_num = int(line.strip().split(': ')[-1]) + message += " (" + signal.Signals(sig_num).name + ")" # pylint: disable=no-member + except ValueError: + pass + + stacktrace = get_apport_stacktrace(fn) + stacktrace_s = stacktrace.split('\n') + crash_function = "No stacktrace" + + if len(stacktrace_s) > 2: + found = False + + # Try to find first entry in openpilot, fall back to first line + for line in stacktrace_s: + if "at selfdrive/" in line: + crash_function = line + found = True + break + + if not found: + crash_function = stacktrace_s[1] + + # Remove arguments that can contain pointers to make sentry one-liner unique + crash_function = " ".join(x for x in crash_function.split(' ')[1:] if not x.startswith('0x')) + crash_function = re.sub(r'\(.*?\)', '', crash_function) + + contents = stacktrace + "\n\n" + contents + message = message + " - " + crash_function + sentry_report(client, fn, message, contents) + + # Copy crashlog to upload folder + clean_path = path.replace('/', '_') + date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") + + new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] + + crashlog_dir = os.path.join(ROOT, "crash") + mkdirs_exists_ok(crashlog_dir) + + # Files could be on different filesystems, copy, then delete + shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) + + try: + os.remove(fn) + except PermissionError: + pass def main(): + clear_apport_folder() # Clear apport folder on start, otherwise duplicate crashes won't register initial_tombstones = set(get_tombstones()) tags = { @@ -89,7 +200,10 @@ def main(): for fn, _ in (now_tombstones - initial_tombstones): try: cloudlog.info(f"reporting new tombstone {fn}") - report_tombstone(fn, client) + if fn.endswith(".crash"): + report_tombstone_apport(fn, client) + else: + report_tombstone_android(fn, client) except Exception: cloudlog.exception(f"Error reporting tombstone {fn}") diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ce39c4fcb..0dd5d56ec 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,51 +1,10 @@ import os -Import('env', 'arch', 'real_arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') - -qt_env = None -if arch in ["x86_64", "Darwin", "larch64"]: - qt_env = env.Clone() - - if arch == "Darwin": - qt_env['QTDIR'] = "/usr/local/opt/qt" - QT_BASE = "/usr/local/opt/qt/" - qt_dirs = [ - QT_BASE + "include/", - QT_BASE + "include/QtWidgets", - QT_BASE + "include/QtGui", - QT_BASE + "include/QtCore", - QT_BASE + "include/QtDBus", - QT_BASE + "include/QtMultimedia", - ] - qt_env["LINKFLAGS"] += ["-F" + QT_BASE + "lib"] - else: - qt_env['QTDIR'] = "/usr" - qt_dirs = [ - f"/usr/include/{real_arch}-linux-gnu/qt5", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtWidgets", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtCore", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtDBus", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtMultimedia", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.5.1/QtGui", - f"/usr/include/{real_arch}-linux-gnu/qt5/QtGui/5.12.8/QtGui", - ] - - qt_env.Tool('qt') - qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"] - qt_flags = [ - "-D_REENTRANT", - "-DQT_NO_DEBUG", - "-DQT_WIDGETS_LIB", - "-DQT_GUI_LIB", - "-DQT_CORE_LIB" - ] - qt_env['CXXFLAGS'] += qt_flags - qt_env['LIBPATH'] += ['#selfdrive/ui'] +Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', + 'cereal', 'transformations') src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [common, 'zmq', 'capnp', 'kj', 'm', cereal, messaging, gpucommon, visionipc] - +libs = [gpucommon, common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', cereal, messaging, visionipc, transformations] if qt_env is None: libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', @@ -57,45 +16,44 @@ if qt_env is None: LINKFLAGS=linkflags, LIBS=libs) else: - qt_libs = ["pthread"] + qt_libs = qt_env["LIBS"] + libs + ["pthread", "ssl", "crypto"] - qt_modules = ["Widgets", "Gui", "Core", "DBus", "Multimedia"] + widgets = qt_env.Library("qt_widgets", + ["qt/qt_sound.cc", "qt/widgets/keyboard.cc", "qt/widgets/input_field.cc", "qt/widgets/drive_stats.cc", "qt/widgets/ssh_keys.cc", + "qt/offroad/networking.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc"], + LIBS=qt_libs) + qt_libs.append(widgets) - if arch == "larch64": - qt_libs += ["GLESv2", "wayland-client"] - elif arch != "Darwin": - qt_libs += ["GL"] if arch == "Darwin": - qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"] - else: - qt_libs += [f"Qt5{m}" for m in qt_modules] + # fix OpenCL + del qt_libs[qt_libs.index('OpenCL')] + qt_env['FRAMEWORKS'] += ['OpenCL'] - - qt_env.Library("qt_widgets", - ["qt/qt_window.cc", "qt/qt_sound.cc", "qt/widgets/keyboard.cc", "qt/widgets/input_field.cc", - "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc"], - LIBS=qt_libs) - qt_libs.append("qt_widgets") - - qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "qt/widgets/offroad_alerts.cc"] + src - qt_env.Program("_ui", qt_src, LIBS=qt_libs + libs) + qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/api.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc", "#phonelibs/qrcode/QrCode.cc"] + src + qt_env.Program("_ui", qt_src, LIBS=qt_libs) # spinner and text window - qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs + libs) - qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs + libs) + qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs) + qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs) # build setup, factory resetter, and installer if "BUILD_SETUP" in os.environ: qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs) - qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + ['curl', 'common']) + qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc"], LIBS=qt_libs + ['curl', 'common', 'json11']) installers = [ ("openpilot", "master"), - ("openpilot_test", "master"), - #("dashcam", "dashcam"), - #("dashcam_test", "dashcam"), + #("openpilot_test", "release3-staging"), + #("openpilot_internal", "master"), + #("dashcam", "dashcam3-staging"), + #("dashcam_test", "dashcam3-staging"), ] for name, branch in installers: - flags = qt_env["CXXFLAGS"] + [f"-D{branch}"] - qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CXXFLAGS=flags) + d = {'BRANCH': f"'\"{branch}\"'"} + if "internal" in name: + import requests + r = requests.get("https://github.com/commaci2.keys") + r.raise_for_status() + d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"' + qt_env.Program(f"qt/setup/installer_{name}", ["qt/setup/installer.cc"], LIBS=qt_libs, CPPDEFINES=d) diff --git a/selfdrive/ui/android/text/Makefile b/selfdrive/ui/android/text/Makefile index 3401cae0f..c83a7a051 100644 --- a/selfdrive/ui/android/text/Makefile +++ b/selfdrive/ui/android/text/Makefile @@ -44,17 +44,6 @@ opensans_regular.o: ../../../assets/fonts/opensans_regular.ttf @echo "[ bin2o ] $@" cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' -%.o: %.c - mkdir -p $(@D) - @echo "[ CC ] $@" - $(CC) $(CPPFLAGS) $(CFLAGS) \ - -I../../.. \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include \ - $(NANOVG_FLAGS) \ - -c -o '$@' '$<' - %.o: %.cc mkdir -p $(@D) @echo "[ CXX ] $@" diff --git a/selfdrive/ui/android/text/text.c b/selfdrive/ui/android/text/text.cc similarity index 82% rename from selfdrive/ui/android/text/text.c rename to selfdrive/ui/android/text/text.cc index 43c43e468..a532ed21a 100644 --- a/selfdrive/ui/android/text/text.c +++ b/selfdrive/ui/android/text/text.cc @@ -4,7 +4,7 @@ #include #include #include - +#include #include #include #include @@ -21,26 +21,27 @@ #define COLOR_WHITE nvgRGBA(255, 255, 255, 255) #define MAX_TEXT_SIZE 2048 -extern const unsigned char _binary_opensans_regular_ttf_start[]; -extern const unsigned char _binary_opensans_regular_ttf_end[]; +extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start"); +extern const uint8_t *bin_opensans_regular_end asm("_binary_opensans_regular_ttf_end"); + int main(int argc, char** argv) { int err; // spinner int fb_w, fb_h; - FramebufferState *fb = framebuffer_init("text", 0x00001000, false, + std::unique_ptr fb = std::make_unique("text", 0x00001000, false, &fb_w, &fb_h); assert(fb); NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES); assert(vg); - int font = nvgCreateFontMem(vg, "regular", (unsigned char*)_binary_opensans_regular_ttf_start, _binary_opensans_regular_ttf_end-_binary_opensans_regular_ttf_start, 0); -assert(font >= 0); + int font = nvgCreateFontMem(vg, "regular", (unsigned char*)bin_opensans_regular, (bin_opensans_regular_end - bin_opensans_regular), 0); + assert(font >= 0); // Awake - framebuffer_set_power(fb, HWC_POWER_MODE_NORMAL); + fb->set_power(HWC_POWER_MODE_NORMAL); set_brightness(255); glClearColor(0.1, 0.1, 0.1, 1.0); @@ -67,7 +68,7 @@ assert(font >= 0); float y = 150; // Copy text - char * text = malloc(MAX_TEXT_SIZE); + char * text = (char *)malloc(MAX_TEXT_SIZE); strncpy(text, argv[1], MAX_TEXT_SIZE); float lineh; @@ -105,7 +106,7 @@ assert(font >= 0); // Draw to screen nvgEndFrame(vg); - framebuffer_swap(fb); + fb->swap(); assert(glGetError() == GL_NO_ERROR); diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index 7017bd283..1c965dedf 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -6,7 +6,6 @@ #include #include "common/util.h" -#include "common/utilpp.h" #include "common/params.h" #include "common/touch.h" #include "common/swaglog.h" @@ -15,11 +14,7 @@ #include "paint.hpp" #include "android/sl_sound.hpp" -volatile sig_atomic_t do_exit = 0; -static void set_do_exit(int sig) { - do_exit = 1; -} - +ExitHandler do_exit; static void ui_set_brightness(UIState *s, int brightness) { static int last_brightness = -1; if (last_brightness != brightness && (s->awake || brightness == 0)) { @@ -30,27 +25,27 @@ static void ui_set_brightness(UIState *s, int brightness) { } static void handle_display_state(UIState *s, bool user_input) { - static int awake_timeout = 0; - awake_timeout = std::max(awake_timeout-1, 0); - // tap detection while display is off - const float accel_samples = 5*UI_FREQ; - static float accel_prev, gyro_prev = 0; + constexpr float accel_samples = 5*UI_FREQ; + static float accel_prev = 0., gyro_prev = 0.; - bool accel_trigger = abs(s->accel_sensor - accel_prev) > 0.2; - bool gyro_trigger = abs(s->gyro_sensor - gyro_prev) > 0.15; - user_input = user_input || (accel_trigger && gyro_trigger); - gyro_prev = s->gyro_sensor; - accel_prev = (accel_prev*(accel_samples - 1) + s->accel_sensor) / accel_samples; + bool should_wake = s->started || s->ignition || user_input; + if (!should_wake) { + // tap detection while display is off + bool accel_trigger = abs(s->accel_sensor - accel_prev) > 0.2; + bool gyro_trigger = abs(s->gyro_sensor - gyro_prev) > 0.15; + should_wake = accel_trigger && gyro_trigger; + gyro_prev = s->gyro_sensor; + accel_prev = (accel_prev * (accel_samples - 1) + s->accel_sensor) / accel_samples; + } // determine desired state - bool should_wake = s->awake; - if (user_input || s->ignition || s->started) { - should_wake = true; + if (should_wake) { awake_timeout = 30*UI_FREQ; - } else if (awake_timeout == 0){ - should_wake = false; + } else if (awake_timeout > 0){ + --awake_timeout; + should_wake = true; } // handle state transition @@ -58,7 +53,7 @@ static void handle_display_state(UIState *s, bool user_input) { s->awake = should_wake; int display_mode = s->awake ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF; LOGW("setting display mode %d", display_mode); - framebuffer_set_power(s->fb, display_mode); + s->fb->set_power(display_mode); if (s->awake) { system("service call window 18 i32 1"); @@ -67,10 +62,10 @@ static void handle_display_state(UIState *s, bool user_input) { } static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { - if (s->started && (touch_x >= s->scene.viz_rect.x - bdr_s) + if (s->started && (touch_x >= s->viz_rect.x - bdr_s) && (s->active_app != cereal::UiLayoutState::App::SETTINGS)) { if (!s->scene.frontview) { - s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + s->sidebar_collapsed = !s->sidebar_collapsed; } else { Params().write_db_value("IsDriverViewEnabled", "0", 1); } @@ -78,13 +73,13 @@ static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { } static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) { - if (!s->scene.uilayout_sidebarcollapsed && touch_x <= sbr_w) { + if (!s->sidebar_collapsed && touch_x <= sbr_w) { if (settings_btn.ptInRect(touch_x, touch_y)) { s->active_app = cereal::UiLayoutState::App::SETTINGS; } else if (home_btn.ptInRect(touch_x, touch_y)) { if (s->started) { s->active_app = cereal::UiLayoutState::App::NONE; - s->scene.uilayout_sidebarcollapsed = true; + s->sidebar_collapsed = true; } else { s->active_app = cereal::UiLayoutState::App::HOME; } @@ -99,14 +94,14 @@ static void update_offroad_layout_state(UIState *s, PubMaster *pm) { if (timeout > 0) { timeout--; } - if (prev_collapsed != s->scene.uilayout_sidebarcollapsed || prev_app != s->active_app || timeout == 0) { + if (prev_collapsed != s->sidebar_collapsed || prev_app != s->active_app || timeout == 0) { MessageBuilder msg; auto layout = msg.initEvent().initUiLayoutState(); layout.setActiveApp(s->active_app); - layout.setSidebarCollapsed(s->scene.uilayout_sidebarcollapsed); + layout.setSidebarCollapsed(s->sidebar_collapsed); pm->send("offroadLayout", msg); - LOGD("setting active app to %d with sidebar %d", (int)s->active_app, s->scene.uilayout_sidebarcollapsed); - prev_collapsed = s->scene.uilayout_sidebarcollapsed; + LOGD("setting active app to %d with sidebar %d", (int)s->active_app, s->sidebar_collapsed); + prev_collapsed = s->sidebar_collapsed; prev_app = s->active_app; timeout = 2 * UI_FREQ; } @@ -114,7 +109,6 @@ static void update_offroad_layout_state(UIState *s, PubMaster *pm) { int main(int argc, char* argv[]) { setpriority(PRIO_PROCESS, 0, -14); - signal(SIGINT, (sighandler_t)set_do_exit); SLSound sound; UIState uistate = {}; @@ -148,7 +142,7 @@ int main(int argc, char* argv[]) { while (!do_exit) { if (!s->started) { - usleep(50 * 1000); + util::sleep_for(50); } double u1 = millis_since_boot(); @@ -169,7 +163,7 @@ int main(int argc, char* argv[]) { } // up one notch every 5 m/s - s->sound->setVolume(fmin(MAX_VOLUME, MIN_VOLUME + s->scene.controls_state.getVEgo() / 5)); + s->sound->setVolume(fmin(MAX_VOLUME, MIN_VOLUME + s->scene.car_state.getVEgo() / 5)); // set brightness float clipped_brightness = fmin(512, (s->light_sensor*brightness_m) + brightness_b); @@ -184,7 +178,7 @@ int main(int argc, char* argv[]) { // warn on sub 15fps LOGW("slow frame(%llu) time: %.2f", (s->sm)->frame, u2-u1); } - framebuffer_swap(s->fb); + s->fb->swap(); } handle_display_state(s, true); diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 5e7008b88..678aad7f5 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1,20 +1,16 @@ #include "ui.hpp" + #include #include #include #include #include "common/util.h" +#include "common/timing.h" #include - #define NANOVG_GLES3_IMPLEMENTATION #include "nanovg_gl.h" #include "nanovg_gl_utils.h" - -extern "C"{ -#include "common/glutil.h" -} - #include "paint.hpp" #include "sidebar.hpp" @@ -41,43 +37,29 @@ const mat3 intrinsic_matrix = (mat3){{ // Projects a point in car to space to the corresponding point in full frame // image space. -bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, float *out_x, float *out_y, float margin=0.0) { - const vec4 car_space_projective = (vec4){{in_x, in_y, in_z, 1.}}; - // We'll call the car space point p. - // First project into normalized image coordinates with the extrinsics matrix. - const vec4 Ep4 = matvecmul(s->scene.extrinsic_matrix, car_space_projective); - - // The last entry is zero because of how we store E (to use matvecmul). - const vec3 Ep = {{Ep4.v[0], Ep4.v[1], Ep4.v[2]}}; +bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out) { + const float margin = 500.0f; + const vec3 pt = (vec3){{in_x, in_y, in_z}}; + const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); const vec3 KEp = matvecmul3(intrinsic_matrix, Ep); // Project. - *out_x = KEp.v[0] / KEp.v[2]; - *out_y = KEp.v[1] / KEp.v[2]; + float x = KEp.v[0] / KEp.v[2]; + float y = KEp.v[1] / KEp.v[2]; - return *out_x >= -margin && *out_x <= s->fb_w + margin && *out_y >= -margin && *out_y <= s->fb_h + margin; + nvgTransformPoint(&out->x, &out->y, s->car_space_transform, x, y); + return out->x >= -margin && out->x <= s->fb_w + margin && out->y >= -margin && out->y <= s->fb_h + margin; } -static void ui_draw_text(NVGcontext *vg, float x, float y, const char* string, float size, NVGcolor color, int font){ - nvgFontFaceId(vg, font); - nvgFontSize(vg, size); - nvgFillColor(vg, color); - nvgText(vg, x, y, string, NULL); +static void ui_draw_text(const UIState *s, float x, float y, const char* string, float size, NVGcolor color, const char *font_name){ + nvgFontFace(s->vg, font_name); + nvgFontSize(s->vg, size); + nvgFillColor(s->vg, color); + nvgText(s->vg, x, y, string, NULL); } -static void draw_chevron(UIState *s, float x_in, float y_in, float sz, - NVGcolor fillColor, NVGcolor glowColor) { - float x, y; - if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &x, &y)) { - return; - } - - sz *= 30; - sz /= (x_in / 3 + 30); - if (sz > 30) sz = 30; - if (sz < 15) sz = 15; - +static void draw_chevron(UIState *s, float x, float y, float sz, NVGcolor fillColor, NVGcolor glowColor) { // glow float g_xo = sz/5; float g_yo = sz/10; @@ -99,23 +81,26 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgFill(s->vg); } -static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int image, NVGcolor color, float img_alpha, int img_y = 0) { +static void ui_draw_circle_image(const UIState *s, int x, int y, int size, const char *image, NVGcolor color, float img_alpha, int img_y = 0) { const int img_size = size * 1.5; - nvgBeginPath(vg); - nvgCircle(vg, x, y + (bdr_s * 1.5), size); - nvgFillColor(vg, color); - nvgFill(vg); - ui_draw_image(vg, x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size, image, img_alpha); + nvgBeginPath(s->vg); + nvgCircle(s->vg, x, y + (bdr_s * 1.5), size); + nvgFillColor(s->vg, color); + nvgFill(s->vg); + ui_draw_image(s, {x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size}, image, img_alpha); } -static void ui_draw_circle_image(NVGcontext *vg, float x, float y, int size, int image, bool active) { +static void ui_draw_circle_image(const UIState *s, int x, int y, int size, const char *image, bool active) { float bg_alpha = active ? 0.3f : 0.1f; float img_alpha = active ? 1.0f : 0.15f; - ui_draw_circle_image(vg, x, y, size, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha); + ui_draw_circle_image(s, x, y, size, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha); } -static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead){ +static void draw_lead(UIState *s, int idx){ // Draw lead car indicator + const auto &lead = s->scene.lead_data[idx]; + auto [x, y] = s->scene.lead_vertices[idx]; + float fillAlpha = 0; float speedBuff = 10.; float leadBuff = 40.; @@ -128,15 +113,20 @@ static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &le } fillAlpha = (int)(fmin(fillAlpha, 255)); } - draw_chevron(s, d_rel, lead.getYRel(), 25, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); + + float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * zoom; + x = std::clamp(x, 0.f, s->viz_rect.right() - sz / 2); + y = std::fmin(s->viz_rect.bottom() - sz * .6, y); + draw_chevron(s, x, y, sz, nvgRGBA(201, 34, 49, fillAlpha), COLOR_YELLOW); } -static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcolor *color, NVGpaint *paint) { - if (cnt == 0) return; +static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *color, NVGpaint *paint) { + if (vd.cnt == 0) return; + const vertex_data *v = &vd.v[0]; nvgBeginPath(s->vg); nvgMoveTo(s->vg, v[0].x, v[0].y); - for (int i = 1; i < cnt; i++) { + for (int i = 1; i < vd.cnt; i++) { nvgLineTo(s->vg, v[i].x, v[i].y); } nvgClosePath(s->vg); @@ -148,38 +138,6 @@ static void ui_draw_line(UIState *s, const vertex_data *v, const int cnt, NVGcol nvgFill(s->vg); } -static void update_track_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, track_vertices_data *pvd) { - const UIScene *scene = &s->scene; - const float off = 0.5; - int max_idx = 0; - float lead_d; - if (s->sm->updated("radarState")) { - lead_d = scene->lead_data[0].getDRel()*2.; - } else { - lead_d = MAX_DRAW_DISTANCE; - } - float path_length = (lead_d>0.)?lead_d-fmin(lead_d*0.35, 10.):MAX_DRAW_DISTANCE; - path_length = fmin(path_length, scene->max_distance); - - - vertex_data *v = &pvd->v[0]; - const float margin = 500.0f; - for (int i = 0; line.getX()[i] <= path_length and i < TRAJECTORY_SIZE; i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i], &v->x, &v->y, margin); - max_idx = i; - } - for (int i = max_idx; i >= 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i], &v->x, &v->y, margin); - } - pvd->cnt = v - pvd->v; -} - -static void ui_draw_track(UIState *s, track_vertices_data *pvd) { - NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, - COLOR_WHITE, COLOR_WHITE_ALPHA(0)); - ui_draw_line(s, &pvd->v[0], pvd->cnt, nullptr, &track_bg); -} - static void draw_frame(UIState *s) { mat4 *out_mat; if (s->scene.frontview) { @@ -191,18 +149,18 @@ static void draw_frame(UIState *s) { } glActiveTexture(GL_TEXTURE0); - if (s->stream.last_idx >= 0) { - glBindTexture(GL_TEXTURE_2D, s->frame_texs[s->stream.last_idx]); + if (s->last_frame) { + glBindTexture(GL_TEXTURE_2D, s->texture[s->last_frame->idx]->frame_tex); #ifndef QCOM // this is handled in ion on QCOM - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->stream.bufs_info.width, s->stream.bufs_info.height, - 0, GL_RGB, GL_UNSIGNED_BYTE, s->priv_hnds[s->stream.last_idx]); + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->last_frame->width, s->last_frame->height, + 0, GL_RGB, GL_UNSIGNED_BYTE, s->last_frame->addr); #endif } - glUseProgram(s->frame_program); - glUniform1i(s->frame_texture_loc, 0); - glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat->v); + glUseProgram(s->gl_shader->prog); + glUniform1i(s->gl_shader->getUniformLocation("uTexture"), 0); + glUniformMatrix4fv(s->gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, out_mat->v); assert(glGetError() == GL_NO_ERROR); glEnableVertexAttribArray(0); @@ -211,70 +169,31 @@ static void draw_frame(UIState *s) { glBindVertexArray(0); } -static void update_line_data(UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, float off, line_vertices_data *pvd, float max_distance) { - // TODO check that this doesn't overflow max vertex buffer - int max_idx; - vertex_data *v = &pvd->v[0]; - const float margin = 500.0f; - for (int i = 0; ((i < TRAJECTORY_SIZE) and (line.getX()[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] - off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); - max_idx = i; - } - for (int i = max_idx - 1; i > 0; i--) { - v += car_space_to_full_frame(s, line.getX()[i], -line.getY()[i] + off, -line.getZ()[i] + 1.22, &v->x, &v->y, margin); - } - pvd->cnt = v - pvd->v; -} - - static void ui_draw_vision_lane_lines(UIState *s) { - const UIScene *scene = &s->scene; - + const UIScene &scene = s->scene; // paint lanelines - line_vertices_data *pvd_ll = &s->lane_line_vertices[0]; - for (int ll_idx = 0; ll_idx < 4; ll_idx++) { - if (s->sm->updated("modelV2")) { - update_line_data(s, scene->model.getLaneLines()[ll_idx], 0.025*scene->model.getLaneLineProbs()[ll_idx], pvd_ll + ll_idx, scene->max_distance); - } - NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene->lane_line_probs[ll_idx]); - ui_draw_line(s, (pvd_ll + ll_idx)->v, (pvd_ll + ll_idx)->cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 1.0, 1.0, scene.lane_line_probs[i]); + ui_draw_line(s, scene.lane_line_vertices[i], &color, nullptr); } // paint road edges - line_vertices_data *pvd_re = &s->road_edge_vertices[0]; - for (int re_idx = 0; re_idx < 2; re_idx++) { - if (s->sm->updated("modelV2")) { - update_line_data(s, scene->model.getRoadEdges()[re_idx], 0.025, pvd_re + re_idx, scene->max_distance); - } - NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0-scene->road_edge_stds[re_idx], 0.0, 1.0)); - ui_draw_line(s, (pvd_re + re_idx)->v, (pvd_re + re_idx)->cnt, &color, nullptr); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + NVGcolor color = nvgRGBAf(1.0, 0.0, 0.0, std::clamp(1.0 - scene.road_edge_stds[i], 0.0, 1.0)); + ui_draw_line(s, scene.road_edge_vertices[i], &color, nullptr); } // paint path - if (s->sm->updated("modelV2")) { - update_track_data(s, scene->model.getPosition(), &s->track_vertices); - } - ui_draw_track(s, &s->track_vertices); + NVGpaint track_bg = nvgLinearGradient(s->vg, s->fb_w, s->fb_h, s->fb_w, s->fb_h * .4, + COLOR_WHITE, COLOR_WHITE_ALPHA(0)); + ui_draw_line(s, scene.track_vertices, nullptr, &track_bg); } // Draw all world space objects. static void ui_draw_world(UIState *s) { const UIScene *scene = &s->scene; - - nvgSave(s->vg); - // Don't draw on top of sidebar - nvgScissor(s->vg, scene->viz_rect.x, scene->viz_rect.y, scene->viz_rect.w, scene->viz_rect.h); - - // Apply transformation such that video pixel coordinates match video - // 1) Put (0, 0) in the middle of the video - nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset); - - // 2) Apply same scaling as video - nvgScale(s->vg, zoom, zoom); - - // 3) Put (0, 0) in top left corner of video - nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); + nvgScissor(s->vg, s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, s->viz_rect.h); // Draw lane edges and vision/mpc tracks ui_draw_vision_lane_lines(s); @@ -282,159 +201,112 @@ static void ui_draw_world(UIState *s) { // Draw lead indicators if openpilot is handling longitudinal if (s->longitudinal_control) { if (scene->lead_data[0].getStatus()) { - draw_lead(s, scene->lead_data[0]); + draw_lead(s, 0); } if (scene->lead_data[1].getStatus() && (std::abs(scene->lead_data[0].getDRel() - scene->lead_data[1].getDRel()) > 3.0)) { - draw_lead(s, scene->lead_data[1]); + draw_lead(s, 1); } } - nvgRestore(s->vg); + nvgResetScissor(s->vg); } static void ui_draw_vision_maxspeed(UIState *s) { - char maxspeed_str[32]; + const int SET_SPEED_NA = 255; float maxspeed = s->scene.controls_state.getVCruise(); - int maxspeed_calc = maxspeed * 0.6225 + 0.5; - if (s->is_metric) { - maxspeed_calc = maxspeed + 0.5; - } + const bool is_cruise_set = maxspeed != 0 && maxspeed != SET_SPEED_NA; + if (is_cruise_set && !s->is_metric) { maxspeed *= 0.6225; } - bool is_cruise_set = (maxspeed != 0 && maxspeed != SET_SPEED_NA); - - int viz_maxspeed_w = 184; - int viz_maxspeed_h = 202; - int viz_maxspeed_x = s->scene.viz_rect.x + (bdr_s*2); - int viz_maxspeed_y = s->scene.viz_rect.y + (bdr_s*1.5); - int viz_maxspeed_xo = 180; - - viz_maxspeed_xo = 0; - - // Draw Background - ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, COLOR_BLACK_ALPHA(100), 30); - - // Draw Border - NVGcolor color = COLOR_WHITE_ALPHA(100); - ui_draw_rect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, color, 20, 10); + const Rect rect = {s->viz_rect.x + (bdr_s * 2), int(s->viz_rect.y + (bdr_s * 1.5)), 184, 202}; + ui_fill_rect(s->vg, rect, COLOR_BLACK_ALPHA(100), 30.); + ui_draw_rect(s->vg, rect, COLOR_WHITE_ALPHA(100), 10, 20.); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - const int text_x = viz_maxspeed_x + (viz_maxspeed_xo / 2) + (viz_maxspeed_w / 2); - ui_draw_text(s->vg, text_x, 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), s->font_sans_regular); - + ui_draw_text(s, rect.centerX(), 148, "MAX", 26 * 2.5, COLOR_WHITE_ALPHA(is_cruise_set ? 200 : 100), "sans-regular"); if (is_cruise_set) { - snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc); - ui_draw_text(s->vg, text_x, 242, maxspeed_str, 48 * 2.5, COLOR_WHITE, s->font_sans_bold); + const std::string maxspeed_str = std::to_string((int)std::nearbyint(maxspeed)); + ui_draw_text(s, rect.centerX(), 242, maxspeed_str.c_str(), 48 * 2.5, COLOR_WHITE, "sans-bold"); } else { - ui_draw_text(s->vg, text_x, 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), s->font_sans_semibold); + ui_draw_text(s, rect.centerX(), 242, "N/A", 42 * 2.5, COLOR_WHITE_ALPHA(100), "sans-semibold"); } } static void ui_draw_vision_speed(UIState *s) { - const Rect &viz_rect = s->scene.viz_rect; - float v_ego = s->scene.controls_state.getVEgo(); - float speed = v_ego * 2.2369363 + 0.5; - if (s->is_metric){ - speed = v_ego * 3.6 + 0.5; - } - const int viz_speed_w = 280; - const int viz_speed_x = viz_rect.centerX() - viz_speed_w/2; - char speed_str[32]; - - nvgBeginPath(s->vg); - nvgRect(s->vg, viz_speed_x, viz_rect.y, viz_speed_w, header_h); + const float speed = std::max(0.0, s->scene.car_state.getVEgo() * (s->is_metric ? 3.6 : 2.2369363)); + const std::string speed_str = std::to_string((int)std::nearbyint(speed)); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); - - snprintf(speed_str, sizeof(speed_str), "%d", (int)speed); - ui_draw_text(s->vg, viz_rect.centerX(), 240, speed_str, 96*2.5, COLOR_WHITE, s->font_sans_bold); - ui_draw_text(s->vg, viz_rect.centerX(), 320, s->is_metric?"km/h":"mph", 36*2.5, COLOR_WHITE_ALPHA(200), s->font_sans_regular); + ui_draw_text(s, s->viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold"); + ui_draw_text(s, s->viz_rect.centerX(), 320, s->is_metric ? "km/h" : "mph", 36 * 2.5, COLOR_WHITE_ALPHA(200), "sans-regular"); } static void ui_draw_vision_event(UIState *s) { const int viz_event_w = 220; - const int viz_event_x = s->scene.viz_rect.right() - (viz_event_w + bdr_s*2); - const int viz_event_y = s->scene.viz_rect.y + (bdr_s*1.5); - if (s->scene.controls_state.getDecelForModel() && s->scene.controls_state.getEnabled()) { - // draw winding road sign - const int img_turn_size = 160*1.5; - ui_draw_image(s->vg, viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size, s->img_turn, 1.0f); - } else if (s->scene.controls_state.getEngageable()) { + const int viz_event_x = s->viz_rect.right() - (viz_event_w + bdr_s*2); + const int viz_event_y = s->viz_rect.y + (bdr_s*1.5); + if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; const int bg_wheel_x = viz_event_x + (viz_event_w-bg_wheel_size); const int bg_wheel_y = viz_event_y + (bg_wheel_size/2); const NVGcolor color = bg_colors[s->status]; - ui_draw_circle_image(s->vg, bg_wheel_x, bg_wheel_y, bg_wheel_size, s->img_wheel, color, 1.0f, bg_wheel_y - 25); + ui_draw_circle_image(s, bg_wheel_x, bg_wheel_y, bg_wheel_size, "wheel", color, 1.0f, bg_wheel_y - 25); } } static void ui_draw_vision_face(UIState *s) { const int face_size = 96; - const int face_x = (s->scene.viz_rect.x + face_size + (bdr_s * 2)); - const int face_y = (s->scene.viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); - ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected()); + const int face_x = (s->viz_rect.x + face_size + (bdr_s * 2)); + const int face_y = (s->viz_rect.bottom() - footer_h + ((footer_h - face_size) / 2)); + ui_draw_circle_image(s, face_x, face_y, face_size, "driver_face", s->scene.dmonitoring_state.getIsActiveMode()); } static void ui_draw_driver_view(UIState *s) { - const UIScene *scene = &s->scene; - s->scene.uilayout_sidebarcollapsed = true; - const Rect &viz_rect = s->scene.viz_rect; - const int ff_xoffset = 32; - const int frame_x = viz_rect.x; - const int frame_w = viz_rect.w; - const int valid_frame_w = 4 * viz_rect.h / 3; - const int box_y = viz_rect.y; - const int box_h = viz_rect.h; - const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset; + s->sidebar_collapsed = true; + const bool is_rhd = s->scene.is_rhd; + const int width = 3 * s->viz_rect.w / 4; + const Rect rect = {s->viz_rect.centerX() - width / 2, s->viz_rect.y, width, s->viz_rect.h}; // x, y, w, h + const Rect valid_rect = {is_rhd ? rect.right() - rect.h / 2 : rect.x, rect.y, rect.h / 2, rect.h}; // blackout - NVGpaint gradient = nvgLinearGradient(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + valid_frame_w), - box_y, - scene->is_rhd ? (valid_frame_w - box_h / 2) : (valid_frame_x + box_h / 2), box_y, - COLOR_BLACK, COLOR_BLACK_ALPHA(0)); - ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + box_h / 2), box_y, valid_frame_w - box_h / 2, box_h, gradient); - ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, COLOR_BLACK_ALPHA(144)); + const int blackout_x = is_rhd ? rect.x : valid_rect.right(); + const int blackout_w = rect.w - valid_rect.w; + NVGpaint gradient = nvgLinearGradient(s->vg, blackout_x, rect.y, blackout_x + blackout_w, rect.y, + COLOR_BLACK_ALPHA(is_rhd ? 255 : 0), COLOR_BLACK_ALPHA(is_rhd ? 0 : 255)); + ui_fill_rect(s->vg, {blackout_x, rect.y, blackout_w, rect.h}, gradient); + ui_fill_rect(s->vg, {blackout_x, rect.y, blackout_w, rect.h}, COLOR_BLACK_ALPHA(144)); + // border + ui_draw_rect(s->vg, rect, bg_colors[STATUS_OFFROAD], 1); - // borders - ui_draw_rect(s->vg, frame_x, box_y, valid_frame_x - frame_x, box_h, nvgRGBA(23, 51, 73, 255)); - ui_draw_rect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h, nvgRGBA(23, 51, 73, 255)); + const bool face_detected = s->scene.driver_state.getFaceProb() > 0.4; + if (face_detected) { + auto fxy_list = s->scene.driver_state.getFacePosition(); + float face_x = fxy_list[0]; + float face_y = fxy_list[1]; + int fbox_x = valid_rect.centerX() + (is_rhd ? face_x : -face_x) * valid_rect.w; + int fbox_y = valid_rect.centerY() + face_y * valid_rect.h; - // draw face box - if (scene->dmonitoring_state.getFaceDetected()) { - auto fxy_list = scene->driver_state.getFacePosition(); - const float face_x = fxy_list[0]; - const float face_y = fxy_list[1]; - float fbox_x; - float fbox_y = box_y + (face_y + 0.5) * box_h - 0.5 * 0.6 * box_h / 2;; - if (!scene->is_rhd) { - fbox_x = valid_frame_x + (1 - (face_x + 0.5)) * (box_h / 2) - 0.5 * 0.6 * box_h / 2; - } else { - fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2; - } + float alpha = 0.2; + if (face_x = std::abs(face_x), face_y = std::abs(face_y); face_x <= 0.35 && face_y <= 0.4) + alpha = 0.8 - (face_x > face_y ? face_x : face_y) * 0.6 / 0.375; - if (std::abs(face_x) <= 0.35 && std::abs(face_y) <= 0.4) { - ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, - nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((std::abs(face_x) > std::abs(face_y) ? std::abs(face_x) : std::abs(face_y))) * 0.6 / 0.375), - 35, 10); - } else { - ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, nvgRGBAf(1.0, 1.0, 1.0, 0.2), 35, 10); - } + const int box_size = 0.6 * rect.h / 2; + ui_draw_rect(s->vg, {fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size}, nvgRGBAf(1.0, 1.0, 1.0, alpha), 10, 35.); } // draw face icon const int face_size = 85; - const int x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0); - const int y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5)); - ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected()); + const int icon_x = is_rhd ? rect.right() - face_size - bdr_s * 2 : rect.x + face_size + bdr_s * 2; + const int icon_y = rect.bottom() - face_size - bdr_s * 2.5; + ui_draw_circle_image(s, icon_x, icon_y, face_size, "driver_face", face_detected); } static void ui_draw_vision_header(UIState *s) { - const Rect &viz_rect = s->scene.viz_rect; - NVGpaint gradient = nvgLinearGradient(s->vg, viz_rect.x, - viz_rect.y+(header_h-(header_h/2.5)), - viz_rect.x, viz_rect.y+header_h, + NVGpaint gradient = nvgLinearGradient(s->vg, s->viz_rect.x, + s->viz_rect.y+(header_h-(header_h/2.5)), + s->viz_rect.x, s->viz_rect.y+header_h, nvgRGBAf(0,0,0,0.45), nvgRGBAf(0,0,0,0)); - ui_draw_rect(s->vg, viz_rect.x, viz_rect.y, viz_rect.w, header_h, gradient); + ui_fill_rect(s->vg, {s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, header_h}, gradient); ui_draw_vision_maxspeed(s); ui_draw_vision_speed(s); @@ -445,9 +317,12 @@ static void ui_draw_vision_footer(UIState *s) { ui_draw_vision_face(s); } +static float get_alert_alpha(float blink_rate) { + return 0.375 * cos((millis_since_boot() / 1000) * 2 * M_PI * blink_rate) + 0.625; +} + static void ui_draw_vision_alert(UIState *s) { static std::map alert_size_map = { - {cereal::ControlsState::AlertSize::NONE, 0}, {cereal::ControlsState::AlertSize::SMALL, 241}, {cereal::ControlsState::AlertSize::MID, 390}, {cereal::ControlsState::AlertSize::FULL, s->fb_h}}; @@ -455,48 +330,42 @@ static void ui_draw_vision_alert(UIState *s) { bool longAlert1 = scene->alert_text1.length() > 15; NVGcolor color = bg_colors[s->status]; - color.a *= s->alert_blinking_alpha; - int alr_s = alert_size_map[scene->alert_size]; + color.a *= get_alert_alpha(scene->alert_blinking_rate); + const int alr_h = alert_size_map[scene->alert_size] + bdr_s; + const Rect rect = {.x = s->viz_rect.x - bdr_s, + .y = s->fb_h - alr_h, + .w = s->viz_rect.w + (bdr_s * 2), + .h = alr_h}; - const int alr_x = scene->viz_rect.x - bdr_s; - const int alr_w = scene->viz_rect.w + (bdr_s*2); - const int alr_h = alr_s+(scene->alert_size==cereal::ControlsState::AlertSize::NONE?0:bdr_s); - const int alr_y = s->fb_h-alr_h; - - ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, color); - - NVGpaint gradient = nvgLinearGradient(s->vg, alr_x, alr_y, alr_x, alr_y+alr_h, - nvgRGBAf(0.0,0.0,0.0,0.05), nvgRGBAf(0.0,0.0,0.0,0.35)); - ui_draw_rect(s->vg, alr_x, alr_y, alr_w, alr_h, gradient); + ui_fill_rect(s->vg, rect, color); + ui_fill_rect(s->vg, rect, nvgLinearGradient(s->vg, rect.x, rect.y, rect.x, rect.bottom(), + nvgRGBAf(0.0, 0.0, 0.0, 0.05), nvgRGBAf(0.0, 0.0, 0.0, 0.35))); nvgFillColor(s->vg, COLOR_WHITE); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); if (scene->alert_size == cereal::ControlsState::AlertSize::SMALL) { - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+15, scene->alert_text1.c_str(), 40*2.5, COLOR_WHITE, s->font_sans_semibold); + ui_draw_text(s, rect.centerX(), rect.centerY() + 15, scene->alert_text1.c_str(), 40*2.5, COLOR_WHITE, "sans-semibold"); } else if (scene->alert_size == cereal::ControlsState::AlertSize::MID) { - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2-45, scene->alert_text1.c_str(), 48*2.5, COLOR_WHITE, s->font_sans_bold); - ui_draw_text(s->vg, alr_x+alr_w/2, alr_y+alr_h/2+75, scene->alert_text2.c_str(), 36*2.5, COLOR_WHITE, s->font_sans_regular); + ui_draw_text(s, rect.centerX(), rect.centerY() - 45, scene->alert_text1.c_str(), 48*2.5, COLOR_WHITE, "sans-bold"); + ui_draw_text(s, rect.centerX(), rect.centerY() + 75, scene->alert_text2.c_str(), 36*2.5, COLOR_WHITE, "sans-regular"); } else if (scene->alert_size == cereal::ControlsState::AlertSize::FULL) { nvgFontSize(s->vg, (longAlert1?72:96)*2.5); - nvgFontFaceId(s->vg, s->font_sans_bold); + nvgFontFace(s->vg, "sans-bold"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, alr_x, alr_y+(longAlert1?360:420), alr_w-60, scene->alert_text1.c_str(), NULL); + nvgTextBox(s->vg, rect.x, rect.y+(longAlert1?360:420), rect.w-60, scene->alert_text1.c_str(), NULL); nvgFontSize(s->vg, 48*2.5); - nvgFontFaceId(s->vg, s->font_sans_regular); + nvgFontFace(s->vg, "sans-regular"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); - nvgTextBox(s->vg, alr_x, alr_h-(longAlert1?300:360), alr_w-60, scene->alert_text2.c_str(), NULL); + nvgTextBox(s->vg, rect.x, rect.h-(longAlert1?300:360), rect.w-60, scene->alert_text2.c_str(), NULL); } } static void ui_draw_vision_frame(UIState *s) { - const UIScene *scene = &s->scene; - const Rect &viz_rect = scene->viz_rect; - // Draw video frames glEnable(GL_SCISSOR_TEST); glViewport(s->video_rect.x, s->video_rect.y, s->video_rect.w, s->video_rect.h); - glScissor(viz_rect.x, viz_rect.y, viz_rect.w, viz_rect.h); + glScissor(s->viz_rect.x, s->viz_rect.y, s->viz_rect.w, s->viz_rect.h); draw_frame(s); glDisable(GL_SCISSOR_TEST); @@ -527,18 +396,18 @@ static void ui_draw_background(UIState *s) { } void ui_draw(UIState *s) { - s->scene.viz_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; - if (!s->scene.uilayout_sidebarcollapsed) { - s->scene.viz_rect.x += sbr_w; - s->scene.viz_rect.w -= sbr_w; + s->viz_rect = Rect{bdr_s, bdr_s, s->fb_w - 2 * bdr_s, s->fb_h - 2 * bdr_s}; + if (!s->sidebar_collapsed) { + s->viz_rect.x += sbr_w; + s->viz_rect.w -= sbr_w; } - const bool draw_vision = s->started && s->status != STATUS_OFFROAD && - s->active_app == cereal::UiLayoutState::App::NONE; + const bool draw_alerts = s->started && s->active_app == cereal::UiLayoutState::App::NONE; + const bool draw_vision = draw_alerts && s->vipc_client->connected; // GL drawing functions ui_draw_background(s); - if (draw_vision && s->vision_connected) { + if (draw_vision) { ui_draw_vision_frame(s); } glEnable(GL_BLEND); @@ -548,44 +417,46 @@ void ui_draw(UIState *s) { // NVG drawing functions - should be no GL inside NVG frame nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); ui_draw_sidebar(s); - if (draw_vision && s->vision_connected) { + if (draw_vision) { ui_draw_vision(s); } - if (draw_vision && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { + if (draw_alerts && s->scene.alert_size != cereal::ControlsState::AlertSize::NONE) { ui_draw_vision_alert(s); } nvgEndFrame(s->vg); glDisable(GL_BLEND); } -void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha){ - nvgBeginPath(vg); - NVGpaint imgPaint = nvgImagePattern(vg, x, y, w, h, 0, image, alpha); - nvgRect(vg, x, y, w, h); - nvgFillPaint(vg, imgPaint); - nvgFill(vg); +void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha){ + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, r.x, r.y, r.w, r.h, 0, s->images.at(name), alpha); + nvgRect(s->vg, r.x, r.y, r.w, r.h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); } -void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r, int width) { +void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius) { nvgBeginPath(vg); - r > 0 ? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); - if (width) { - nvgStrokeColor(vg, color); - nvgStrokeWidth(vg, width); - nvgStroke(vg); - } else { - nvgFillColor(vg, color); - nvgFill(vg); - } + radius > 0 ? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h); + nvgStrokeColor(vg, color); + nvgStrokeWidth(vg, width); + nvgStroke(vg); } -void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGpaint &paint, float r){ +static inline void fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor *color, const NVGpaint *paint, float radius) { nvgBeginPath(vg); - r > 0? nvgRoundedRect(vg, x, y, w, h, r) : nvgRect(vg, x, y, w, h); - nvgFillPaint(vg, paint); + radius > 0? nvgRoundedRect(vg, r.x, r.y, r.w, r.h, radius) : nvgRect(vg, r.x, r.y, r.w, r.h); + if (color) nvgFillColor(vg, *color); + if (paint) nvgFillPaint(vg, *paint); nvgFill(vg); } +void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor &color, float radius) { + fill_rect(vg, r, &color, nullptr, radius); +} +void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius){ + fill_rect(vg, r, nullptr, &paint, radius); +} static const char frame_vertex_shader[] = #ifdef NANOVG_GL3_IMPLEMENTATION @@ -639,47 +510,44 @@ void ui_nvg_init(UIState *s) { #else s->vg = nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); #endif - assert(s->vg); - s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/fonts/opensans_regular.ttf"); - assert(s->font_sans_regular >= 0); - s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/fonts/opensans_semibold.ttf"); - assert(s->font_sans_semibold >= 0); - s->font_sans_bold = nvgCreateFont(s->vg, "sans-bold", "../assets/fonts/opensans_bold.ttf"); - assert(s->font_sans_bold >= 0); + // init fonts + std::pair fonts[] = { + {"sans-regular", "../assets/fonts/opensans_regular.ttf"}, + {"sans-semibold", "../assets/fonts/opensans_semibold.ttf"}, + {"sans-bold", "../assets/fonts/opensans_bold.ttf"}, + }; + for (auto [name, file] : fonts) { + int font_id = nvgCreateFont(s->vg, name, file); + assert(font_id >= 0); + } - s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1); - assert(s->img_wheel != 0); - s->img_turn = nvgCreateImage(s->vg, "../assets/img_trafficSign_turn.png", 1); - assert(s->img_turn != 0); - s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1); - assert(s->img_face != 0); - s->img_button_settings = nvgCreateImage(s->vg, "../assets/images/button_settings.png", 1); - assert(s->img_button_settings != 0); - s->img_button_home = nvgCreateImage(s->vg, "../assets/images/button_home.png", 1); - assert(s->img_button_home != 0); - s->img_battery = nvgCreateImage(s->vg, "../assets/images/battery.png", 1); - assert(s->img_battery != 0); - s->img_battery_charging = nvgCreateImage(s->vg, "../assets/images/battery_charging.png", 1); - assert(s->img_battery_charging != 0); - - for(int i=0;i<=5;++i) { - char network_asset[32]; - snprintf(network_asset, sizeof(network_asset), "../assets/images/network_%d.png", i); - s->img_network[i] = nvgCreateImage(s->vg, network_asset, 1); - assert(s->img_network[i] != 0); + // init images + std::vector> images = { + {"wheel", "../assets/img_chffr_wheel.png"}, + {"trafficSign_turn", "../assets/img_trafficSign_turn.png"}, + {"driver_face", "../assets/img_driver_face.png"}, + {"button_settings", "../assets/images/button_settings.png"}, + {"button_home", "../assets/images/button_home.png"}, + {"battery", "../assets/images/battery.png"}, + {"battery_charging", "../assets/images/battery_charging.png"}, + {"network_0", "../assets/images/network_0.png"}, + {"network_1", "../assets/images/network_1.png"}, + {"network_2", "../assets/images/network_2.png"}, + {"network_3", "../assets/images/network_3.png"}, + {"network_4", "../assets/images/network_4.png"}, + {"network_5", "../assets/images/network_5.png"}, + }; + for (auto [name, file] : images) { + s->images[name] = nvgCreateImage(s->vg, file, 1); + assert(s->images[name] != 0); } // init gl - s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); - assert(s->frame_program); - - s->frame_pos_loc = glGetAttribLocation(s->frame_program, "aPosition"); - s->frame_texcoord_loc = glGetAttribLocation(s->frame_program, "aTexCoord"); - - s->frame_texture_loc = glGetUniformLocation(s->frame_program, "uTexture"); - s->frame_transform_loc = glGetUniformLocation(s->frame_program, "uTransform"); + s->gl_shader = std::make_unique(frame_vertex_shader, frame_fragment_shader); + GLint frame_pos_loc = glGetAttribLocation(s->gl_shader->prog, "aPosition"); + GLint frame_texcoord_loc = glGetAttribLocation(s->gl_shader->prog, "aTexCoord"); glViewport(0, 0, s->fb_w, s->fb_h); @@ -714,11 +582,11 @@ void ui_nvg_init(UIState *s) { 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, + glEnableVertexAttribArray(frame_pos_loc); + glVertexAttribPointer(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, + glEnableVertexAttribArray(frame_texcoord_loc); + glVertexAttribPointer(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]); @@ -741,8 +609,16 @@ void ui_nvg_init(UIState *s) { 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] = 0; - s->priv_hnds[i] = NULL; - } + // Apply transformation such that video pixel coordinates match video + // 1) Put (0, 0) in the middle of the video + nvgTranslate(s->vg, s->video_rect.x + s->video_rect.w / 2, s->video_rect.y + s->video_rect.h / 2 + y_offset); + + // 2) Apply same scaling as video + nvgScale(s->vg, zoom, zoom); + + // 3) Put (0, 0) in top left corner of video + nvgTranslate(s->vg, -intrinsic_matrix.v[2], -intrinsic_matrix.v[5]); + + nvgCurrentTransform(s->vg, s->car_space_transform); + nvgResetTransform(s->vg); } diff --git a/selfdrive/ui/paint.hpp b/selfdrive/ui/paint.hpp index 203beef17..4f63d3ef4 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.hpp @@ -1,8 +1,10 @@ #pragma once #include "ui.hpp" +bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out); void ui_draw(UIState *s); -void ui_draw_image(NVGcontext *vg, float x, float y, float w, float h, int image, float alpha); -void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGcolor color, float r = 0, int width = 0); -void ui_draw_rect(NVGcontext *vg, float x, float y, float w, float h, NVGpaint &paint, float r = 0); +void ui_draw_image(const UIState *s, const Rect &r, const char *name, float alpha); +void ui_draw_rect(NVGcontext *vg, const Rect &r, NVGcolor color, int width, float radius = 0); +void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float radius = 0); +void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGcolor &color, float radius = 0); void ui_nvg_init(UIState *s); diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc new file mode 100644 index 000000000..06f59c407 --- /dev/null +++ b/selfdrive/ui/qt/api.cc @@ -0,0 +1,135 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "api.hpp" +#include "home.hpp" +#include "common/params.h" +#include "common/util.h" +#if defined(QCOM) || defined(QCOM2) +const std::string private_key_path = "/persist/comma/id_rsa"; +#else +const std::string private_key_path = util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa"); +#endif + +QByteArray CommaApi::rsa_sign(QByteArray data) { + auto file = QFile(private_key_path.c_str()); + if (!file.open(QIODevice::ReadOnly)) { + qDebug() << "No RSA private key found, please run manager.py or registration.py"; + return QByteArray(); + } + auto key = file.readAll(); + file.close(); + file.deleteLater(); + BIO* mem = BIO_new_mem_buf(key.data(), key.size()); + assert(mem); + RSA* rsa_private = PEM_read_bio_RSAPrivateKey(mem, NULL, NULL, NULL); + assert(rsa_private); + auto sig = QByteArray(); + sig.resize(RSA_size(rsa_private)); + unsigned int sig_len; + int ret = RSA_sign(NID_sha256, (unsigned char*)data.data(), data.size(), (unsigned char*)sig.data(), &sig_len, rsa_private); + assert(ret == 1); + assert(sig_len == sig.size()); + BIO_free(mem); + RSA_free(rsa_private); + return sig; +} + +QString CommaApi::create_jwt(QVector> payloads, int expiry) { + QString dongle_id = QString::fromStdString(Params().get("DongleId")); + + QJsonObject header; + header.insert("alg", "RS256"); + + QJsonObject payload; + payload.insert("identity", dongle_id); + + auto t = QDateTime::currentSecsSinceEpoch(); + payload.insert("nbf", t); + payload.insert("iat", t); + payload.insert("exp", t + expiry); + for (auto load : payloads) { + payload.insert(load.first, load.second); + } + + QString jwt = + QJsonDocument(header).toJson(QJsonDocument::Compact).toBase64(QByteArray::Base64UrlEncoding | QByteArray::OmitTrailingEquals) + + '.' + + QJsonDocument(payload).toJson(QJsonDocument::Compact).toBase64(QByteArray::Base64UrlEncoding | QByteArray::OmitTrailingEquals); + + auto hash = QCryptographicHash::hash(jwt.toUtf8(), QCryptographicHash::Sha256); + auto sig = rsa_sign(hash); + jwt += '.' + sig.toBase64(QByteArray::Base64UrlEncoding | QByteArray::OmitTrailingEquals); + return jwt; +} + +QString CommaApi::create_jwt() { + return create_jwt(*(new QVector>())); +} + +RequestRepeater::RequestRepeater(QWidget* parent, QString requestURL, int period_seconds, QVector> payloads, bool disableWithScreen) + : disableWithScreen(disableWithScreen), QObject(parent) { + networkAccessManager = new QNetworkAccessManager(this); + + QTimer* timer = new QTimer(this); + QObject::connect(timer, &QTimer::timeout, [=](){sendRequest(requestURL, payloads);}); + timer->start(period_seconds * 1000); + + networkTimer = new QTimer(this); + networkTimer->setSingleShot(true); + networkTimer->setInterval(20000); // 20s before aborting + connect(networkTimer, SIGNAL(timeout()), this, SLOT(requestTimeout())); +} + +void RequestRepeater::sendRequest(QString requestURL, QVector> payloads){ + // No network calls onroad + if(GLWindow::ui_state.started){ + return; + } + if (!active || (!GLWindow::ui_state.awake && disableWithScreen)) { + return; + } + if(reply != NULL){ + return; + } + aborted = false; + QString token = CommaApi::create_jwt(payloads); + QNetworkRequest request; + request.setUrl(QUrl(requestURL)); + request.setRawHeader("Authorization", ("JWT " + token).toUtf8()); + + reply = networkAccessManager->get(request); + + networkTimer->start(); + connect(reply, SIGNAL(finished()), this, SLOT(requestFinished())); +} + +void RequestRepeater::requestTimeout(){ + aborted = true; + reply->abort(); +} + +// This function should always emit something +void RequestRepeater::requestFinished(){ + if(!aborted){ + networkTimer->stop(); + QString response = reply->readAll(); + if (reply->error() == QNetworkReply::NoError) { + emit receivedResponse(response); + } else { + emit failedResponse(reply->errorString()); + } + }else{ + emit failedResponse("Custom Openpilot network timeout"); + } + reply->deleteLater(); + reply = NULL; +} diff --git a/selfdrive/ui/qt/api.hpp b/selfdrive/ui/qt/api.hpp new file mode 100644 index 000000000..7f7892fe0 --- /dev/null +++ b/selfdrive/ui/qt/api.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +class CommaApi : public QObject { + Q_OBJECT +public: + static QByteArray rsa_sign(QByteArray data); + static QString create_jwt(QVector> payloads, int expiry=3600); + static QString create_jwt(); +private: + QNetworkAccessManager* networkAccessManager; +}; + +/** + * Makes repeated requests to the request endpoint. + */ +class RequestRepeater : public QObject { + Q_OBJECT +public: + explicit RequestRepeater(QWidget* parent, QString requestURL, int period = 10, QVector> payloads = *(new QVector>()), bool disableWithScreen = true); + bool active = true; +private: + bool disableWithScreen; + QNetworkReply* reply; + QNetworkAccessManager* networkAccessManager; + QTimer* networkTimer; + std::atomic aborted = false; // Not 100% sure we need atomic + void sendRequest(QString requestURL, QVector> payloads); +private slots: + void requestTimeout(); + void requestFinished(); +signals: + void receivedResponse(QString response); + void failedResponse(QString errorString); +}; diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index bc402eec8..740003b3e 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -1,40 +1,41 @@ #include -#include #include +#include #include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include #include "common/params.h" +#include "common/timing.h" +#include "common/swaglog.h" #include "home.hpp" #include "paint.hpp" #include "qt_window.hpp" +#include "widgets/drive_stats.hpp" +#include "widgets/setup.hpp" #define BACKLIGHT_DT 0.25 #define BACKLIGHT_TS 2.00 - -OffroadHome::OffroadHome(QWidget *parent) : QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(); +OffroadHome::OffroadHome(QWidget* parent) : QWidget(parent) { + QVBoxLayout* main_layout = new QVBoxLayout(); main_layout->setContentsMargins(sbr_w + 50, 50, 50, 50); - center_layout = new QStackedLayout(); - - // header - QHBoxLayout *header_layout = new QHBoxLayout(); + // top header + QHBoxLayout* header_layout = new QHBoxLayout(); date = new QLabel(); date->setStyleSheet(R"(font-size: 55px;)"); header_layout->addWidget(date, 0, Qt::AlignTop | Qt::AlignLeft); - QLabel *version = new QLabel(QString::fromStdString("openpilot v" + Params().get("Version"))); + QLabel* version = new QLabel(QString::fromStdString("openpilot v" + Params().get("Version"))); version->setStyleSheet(R"(font-size: 45px;)"); header_layout->addWidget(version, 0, Qt::AlignTop | Qt::AlignRight); @@ -44,10 +45,23 @@ OffroadHome::OffroadHome(QWidget *parent) : QWidget(parent) { QObject::connect(alert_notification, SIGNAL(released()), this, SLOT(openAlerts())); main_layout->addWidget(alert_notification, 0, Qt::AlignTop | Qt::AlignRight); - // center - QLabel *drive = new QLabel("Drive me"); - drive->setStyleSheet(R"(font-size: 175px;)"); - center_layout->addWidget(drive); + // main content + main_layout->addSpacing(25); + center_layout = new QStackedLayout(); + + QHBoxLayout* statsAndSetup = new QHBoxLayout(); + + DriveStats* drive = new DriveStats; + drive->setFixedSize(1000, 800); + statsAndSetup->addWidget(drive); + + SetupWidget* setup = new SetupWidget; + statsAndSetup->addWidget(setup); + + QWidget* statsAndSetupWidget = new QWidget(); + statsAndSetupWidget->setLayout(statsAndSetup); + + center_layout->addWidget(statsAndSetupWidget); alerts_widget = new OffroadAlert(); QObject::connect(alerts_widget, SIGNAL(closeAlerts()), this, SLOT(closeAlerts())); @@ -108,17 +122,16 @@ void OffroadHome::refresh() { border: 1px solid; border-radius: 5px; font-size: 40px; - font-weight: bold; - background-color: red; + font-weight: 500; + background-color: #E22C2C; )"); - if (alerts_widget->updateAvailable){ - style.replace("red", "blue"); + if (alerts_widget->updateAvailable) { + style.replace("#E22C2C", "#364DEF"); } alert_notification->setStyleSheet(style); } - -HomeWindow::HomeWindow(QWidget *parent) : QWidget(parent) { +HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { layout = new QGridLayout; layout->setMargin(0); @@ -130,6 +143,8 @@ HomeWindow::HomeWindow(QWidget *parent) : QWidget(parent) { home = new OffroadHome(); layout->addWidget(home, 0, 0); QObject::connect(glWindow, SIGNAL(offroadTransition(bool)), this, SLOT(setVisibility(bool))); + QObject::connect(glWindow, SIGNAL(offroadTransition(bool)), this, SIGNAL(offroadTransition(bool))); + QObject::connect(glWindow, SIGNAL(screen_shutoff()), this, SIGNAL(closeSettings())); QObject::connect(this, SIGNAL(openSettings()), home, SLOT(refresh())); setLayout(layout); setStyleSheet(R"( @@ -143,30 +158,29 @@ void HomeWindow::setVisibility(bool offroad) { home->setVisible(offroad); } -void HomeWindow::mousePressEvent(QMouseEvent *e) { - UIState *ui_state = glWindow->ui_state; +void HomeWindow::mousePressEvent(QMouseEvent* e) { + UIState* ui_state = &glWindow->ui_state; glWindow->wake(); // Settings button click - if (!ui_state->scene.uilayout_sidebarcollapsed && settings_btn.ptInRect(e->x(), e->y())) { + if (!ui_state->sidebar_collapsed && settings_btn.ptInRect(e->x(), e->y())) { emit openSettings(); } // Vision click - if (ui_state->started && (e->x() >= ui_state->scene.viz_rect.x - bdr_s)) { - ui_state->scene.uilayout_sidebarcollapsed = !ui_state->scene.uilayout_sidebarcollapsed; + if (ui_state->started && (e->x() >= ui_state->viz_rect.x - bdr_s)) { + ui_state->sidebar_collapsed = !ui_state->sidebar_collapsed; } } - -static void handle_display_state(UIState *s, int dt, bool user_input) { - static int awake_timeout = 0; - awake_timeout = std::max(awake_timeout-dt, 0); +static void handle_display_state(UIState* s, bool user_input) { + static int awake_timeout = 0; // Somehow this only gets called on program start + awake_timeout = std::max(awake_timeout - 1, 0); if (user_input || s->ignition || s->started) { s->awake = true; - awake_timeout = 30*UI_FREQ; + awake_timeout = 30 * UI_FREQ; } else if (awake_timeout == 0) { s->awake = false; } @@ -180,8 +194,7 @@ static void set_backlight(int brightness) { } } - -GLWindow::GLWindow(QWidget *parent) : QOpenGLWidget(parent) { +GLWindow::GLWindow(QWidget* parent) : QOpenGLWidget(parent) { timer = new QTimer(this); QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate())); @@ -209,47 +222,49 @@ void GLWindow::initializeGL() { std::cout << "OpenGL renderer: " << glGetString(GL_RENDERER) << std::endl; std::cout << "OpenGL language version: " << glGetString(GL_SHADING_LANGUAGE_VERSION) << std::endl; - ui_state = new UIState(); - ui_state->sound = &sound; - ui_state->fb_w = vwp_w; - ui_state->fb_h = vwp_h; - ui_init(ui_state); + ui_state.sound = &sound; + ui_init(&ui_state); wake(); - timer->start(0); - backlight_timer->start(BACKLIGHT_DT * 100); + prev_draw_t = millis_since_boot(); + timer->start(1000 / UI_FREQ); + backlight_timer->start(BACKLIGHT_DT * 1000); } void GLWindow::backlightUpdate() { // Update brightness float k = (BACKLIGHT_DT / BACKLIGHT_TS) / (1.0f + BACKLIGHT_DT / BACKLIGHT_TS); - float clipped_brightness = std::min(1023.0f, (ui_state->light_sensor*brightness_m) + brightness_b); + float clipped_brightness = std::min(1023.0f, (ui_state.light_sensor * brightness_m) + brightness_b); smooth_brightness = clipped_brightness * k + smooth_brightness * (1.0f - k); int brightness = smooth_brightness; - if (!ui_state->awake) { + if (!ui_state.awake) { brightness = 0; + emit screen_shutoff(); } - std::thread{set_backlight, brightness}.detach(); } void GLWindow::timerUpdate() { - if (ui_state->started != onroad) { - onroad = ui_state->started; - emit offroadTransition(!onroad); -#ifdef QCOM2 - timer->setInterval(onroad ? 0 : 1000); -#endif + // Connecting to visionIPC requires opengl to be current + if (!ui_state.vipc_client->connected){ + makeCurrent(); } - // Fix awake timeout if running 1 Hz when offroad - int dt = timer->interval() == 0 ? 1 : 20; - handle_display_state(ui_state, dt, false); + if (ui_state.started != onroad) { + onroad = ui_state.started; + emit offroadTransition(!onroad); - ui_update(ui_state); + // Change timeout to 0 when onroad, this will call timerUpdate continously. + // This puts visionIPC in charge of update frequency, reducing video latency + timer->start(onroad ? 0 : 1000 / UI_FREQ); + } + + handle_display_state(&ui_state, false); + + ui_update(&ui_state); repaint(); } @@ -258,31 +273,27 @@ void GLWindow::resizeGL(int w, int h) { } void GLWindow::paintGL() { - ui_draw(ui_state); -} + if(GLWindow::ui_state.awake){ + ui_draw(&ui_state); -void GLWindow::wake() { - // UI state might not be initialized yet - if (ui_state != nullptr) { - handle_display_state(ui_state, 1, true); + double cur_draw_t = millis_since_boot(); + double dt = cur_draw_t - prev_draw_t; + if (dt > 66 && onroad){ + // warn on sub 15fps +#ifdef QCOM2 + LOGW("slow frame(%llu) time: %.2f", ui_state.sm->frame, dt); +#endif + } + prev_draw_t = cur_draw_t; } } -GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) { - unsigned int texture; - glGenTextures(1, &texture); - glBindTexture(GL_TEXTURE_2D, texture); - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph); - glGenerateMipmap(GL_TEXTURE_2D); - *pkhr = (EGLImageKHR)1; // not NULL - return texture; +void GLWindow::wake() { + handle_display_state(&ui_state, true); } -void visionimg_destroy_gl(EGLImageKHR khr, void *ph) { - // empty -} - -FramebufferState* framebuffer_init(const char* name, int32_t layer, int alpha, - int *out_w, int *out_h) { - return (FramebufferState*)1; // not null +FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h) { + *out_w = vwp_w; + *out_h = vwp_h; } +FrameBuffer::~FrameBuffer() {} diff --git a/selfdrive/ui/qt/home.hpp b/selfdrive/ui/qt/home.hpp index 5f71e0e4f..98ebc77f4 100644 --- a/selfdrive/ui/qt/home.hpp +++ b/selfdrive/ui/qt/home.hpp @@ -1,19 +1,18 @@ #pragma once +#include #include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include #include "qt_sound.hpp" -#include "widgets/offroad_alerts.hpp" #include "ui/ui.hpp" - +#include "widgets/offroad_alerts.hpp" // container window for onroad NVG UI class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { @@ -21,14 +20,15 @@ class GLWindow : public QOpenGLWidget, protected QOpenGLFunctions { public: using QOpenGLWidget::QOpenGLWidget; - explicit GLWindow(QWidget *parent = 0); + explicit GLWindow(QWidget* parent = 0); void wake(); ~GLWindow(); - UIState *ui_state = nullptr; + inline static UIState ui_state = {0}; signals: void offroadTransition(bool offroad); + void screen_shutoff(); protected: void initializeGL() override; @@ -36,12 +36,13 @@ protected: void paintGL() override; private: - QTimer *timer; - QTimer *backlight_timer; + QTimer* timer; + QTimer* backlight_timer; QtSound sound; bool onroad = true; + double prev_draw_t = 0; // TODO: this shouldn't be here float brightness_b = 0; @@ -58,42 +59,42 @@ class OffroadHome : public QWidget { Q_OBJECT public: - explicit OffroadHome(QWidget *parent = 0); + explicit OffroadHome(QWidget* parent = 0); private: - QTimer *timer; + QTimer* timer; // offroad home screen widgets - QLabel *date; - QStackedLayout *center_layout; - OffroadAlert *alerts_widget; - QPushButton *alert_notification; + QLabel* date; + QStackedLayout* center_layout; + OffroadAlert* alerts_widget; + QPushButton* alert_notification; public slots: - void closeAlerts(); + void closeAlerts(); void openAlerts(); void refresh(); }; - class HomeWindow : public QWidget { Q_OBJECT public: - explicit HomeWindow(QWidget *parent = 0); - GLWindow *glWindow; + explicit HomeWindow(QWidget* parent = 0); + GLWindow* glWindow; signals: + void offroadTransition(bool offroad); void openSettings(); + void closeSettings(); protected: - void mousePressEvent(QMouseEvent *e) override; + void mousePressEvent(QMouseEvent* e) override; private: - QGridLayout *layout; - OffroadHome *home; + QGridLayout* layout; + OffroadHome* home; private slots: void setVisibility(bool offroad); }; - diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc new file mode 100644 index 000000000..39b1667d1 --- /dev/null +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -0,0 +1,404 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "networking.hpp" +#include "util.h" + +void clearLayout(QLayout* layout) { + while (QLayoutItem* item = layout->takeAt(0)) { + if (QWidget* widget = item->widget()) { + widget->deleteLater(); + } + if (QLayout* childLayout = item->layout()) { + clearLayout(childLayout); + } + delete item; + } +} + +QWidget* layoutToWidget(QLayout* l, QWidget* parent){ + QWidget* q = new QWidget(parent); + q->setLayout(l); + return q; +} + +// https://stackoverflow.com/questions/478898/how-do-i-execute-a-command-and-get-the-output-of-the-command-within-c-using-po +std::string exec(const char* cmd) { + std::array buffer; + std::string result; + std::unique_ptr pipe(popen(cmd, "r"), pclose); + if (!pipe) { + throw std::runtime_error("popen() failed!"); + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { + result += buffer.data(); + } + return result; +} + +// Networking functions + +Networking::Networking(QWidget* parent, bool show_advanced) : QWidget(parent), show_advanced(show_advanced){ + s = new QStackedLayout; + + QLabel* warning = new QLabel("Network manager is inactive!"); + warning->setStyleSheet(R"(font-size: 65px;)"); + + s->addWidget(warning); + setLayout(s); + + QTimer* timer = new QTimer(this); + QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); + timer->start(5000); + attemptInitialization(); +} + +void Networking::attemptInitialization(){ + // Checks if network manager is active + try { + wifi = new WifiManager(this); + } catch (std::exception &e) { + return; + } + + connect(wifi, SIGNAL(wrongPassword(QString)), this, SLOT(wrongPassword(QString))); + + QVBoxLayout* vlayout = new QVBoxLayout; + + if (show_advanced) { + QPushButton* advancedSettings = new QPushButton("Advanced"); + advancedSettings->setStyleSheet(R"(margin-right: 30px)"); + advancedSettings->setFixedSize(350, 100); + connect(advancedSettings, &QPushButton::released, [=](){s->setCurrentWidget(an);}); + vlayout->addSpacing(10); + vlayout->addWidget(advancedSettings, 0, Qt::AlignRight); + vlayout->addSpacing(10); + } + + wifiWidget = new WifiUI(0, 5, wifi); + connect(wifiWidget, SIGNAL(connectToNetwork(Network)), this, SLOT(connectToNetwork(Network))); + vlayout->addWidget(wifiWidget, 1); + + wifiScreen = layoutToWidget(vlayout, this); + s->addWidget(wifiScreen); + + an = new AdvancedNetworking(this, wifi); + connect(an, &AdvancedNetworking::backPress, [=](){s->setCurrentWidget(wifiScreen);}); + s->addWidget(an); + + setStyleSheet(R"( + QPushButton { + font-size: 50px; + margin: 0px; + padding: 15px; + border-width: 0; + border-radius: 30px; + color: #dddddd; + background-color: #444444; + } + QPushButton:disabled { + color: #777777; + background-color: #222222; + } + )"); + s->setCurrentWidget(wifiScreen); + ui_setup_complete = true; +} + +void Networking::refresh(){ + if (!this->isVisible()) { + return; + } + if (!ui_setup_complete) { + attemptInitialization(); + if (!ui_setup_complete) { + return; + } + } + wifiWidget->refresh(); + an->refresh(); +} + +void Networking::connectToNetwork(Network n) { + if (n.security_type == SecurityType::OPEN) { + wifi->connect(n); + } else if (n.security_type == SecurityType::WPA) { + QString pass = InputDialog::getText("Enter password for \"" + n.ssid + "\""); + wifi->connect(n, pass); + } +} + +void Networking::wrongPassword(QString ssid) { + return; // TODO: add this back + /* + for (Network n : wifi->seen_networks) { + if (n.ssid == ssid) { + inputField->setPromptText("Wrong password for \"" + n.ssid +"\""); + s->setCurrentIndex(0); + emit openKeyboard(); + return; + } + } + */ +} + +QFrame* hline(QWidget* parent = 0){ + QFrame* line = new QFrame(parent); + line->setFrameShape(QFrame::StyledPanel); + line->setStyleSheet("margin-left: 40px; margin-right: 40px; border-width: 1px; border-bottom-style: solid; border-color: gray;"); + line->setFixedHeight(2); + return line; +} + +// AdvancedNetworking functions + +AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWidget(parent), wifi(wifi){ + s = new QStackedLayout; // mainPage, SSH settings + + QVBoxLayout* vlayout = new QVBoxLayout; + + // Back button + QHBoxLayout* backLayout = new QHBoxLayout; + QPushButton* back = new QPushButton("Back"); + back->setFixedSize(500, 100); + connect(back, &QPushButton::released, [=](){emit backPress();}); + backLayout->addWidget(back, 0, Qt::AlignLeft); + vlayout->addWidget(layoutToWidget(backLayout, this), 0, Qt::AlignLeft); + + // Enable tethering layout + QHBoxLayout* tetheringToggleLayout = new QHBoxLayout; + tetheringToggleLayout->addWidget(new QLabel("Enable tethering")); + Toggle* toggle_switch = new Toggle; + toggle_switch->setFixedSize(150, 100); + tetheringToggleLayout->addWidget(toggle_switch); + tetheringToggleLayout->addSpacing(40); + if (wifi->tetheringEnabled()) { + toggle_switch->togglePosition(); + } + QObject::connect(toggle_switch, SIGNAL(stateChanged(int)), this, SLOT(toggleTethering(int))); + vlayout->addWidget(layoutToWidget(tetheringToggleLayout, this), 0); + vlayout->addWidget(hline(), 0); + + // Change tethering password + QHBoxLayout *tetheringPassword = new QHBoxLayout; + tetheringPassword->addWidget(new QLabel("Edit tethering password"), 1); + editPasswordButton = new QPushButton("EDIT"); + editPasswordButton->setFixedWidth(500); + connect(editPasswordButton, &QPushButton::released, [=](){ + QString pass = InputDialog::getText("Enter new tethering password"); + if (pass.size()) { + wifi->changeTetheringPassword(pass); + } + }); + tetheringPassword->addWidget(editPasswordButton, 1, Qt::AlignRight); + vlayout->addWidget(layoutToWidget(tetheringPassword, this), 0); + vlayout->addWidget(hline(), 0); + + // IP adress + QHBoxLayout* IPlayout = new QHBoxLayout; + IPlayout->addWidget(new QLabel("IP address"), 0); + ipLabel = new QLabel(wifi->ipv4_address); + ipLabel->setStyleSheet("color: #aaaaaa"); + IPlayout->addWidget(ipLabel, 0, Qt::AlignRight); + vlayout->addWidget(layoutToWidget(IPlayout, this), 0); + vlayout->addWidget(hline(), 0); + + // Enable SSH + QHBoxLayout* enableSSHLayout = new QHBoxLayout(this); + enableSSHLayout->addWidget(new QLabel("Enable SSH", this)); + toggle_switch_SSH = new Toggle(this); + toggle_switch_SSH->setFixedSize(150, 100); + if (isSSHEnabled()) { + toggle_switch_SSH->togglePosition(); + } + QObject::connect(toggle_switch_SSH, SIGNAL(stateChanged(int)), this, SLOT(toggleSSH(int))); + enableSSHLayout->addWidget(toggle_switch_SSH); + vlayout->addWidget(layoutToWidget(enableSSHLayout, this)); + vlayout->addWidget(hline(), 0); + + // SSH keys + QHBoxLayout* authSSHLayout = new QHBoxLayout(this); + authSSHLayout->addWidget(new QLabel("SSH keys", this)); + QPushButton* editAuthSSHButton = new QPushButton("EDIT", this); + editAuthSSHButton->setFixedWidth(500); + connect(editAuthSSHButton, &QPushButton::released, [=](){s->setCurrentWidget(ssh);}); + authSSHLayout->addWidget(editAuthSSHButton); + vlayout->addWidget(layoutToWidget(authSSHLayout, this)); + vlayout->addSpacing(50); + + // //Disconnect or delete connections + // QHBoxLayout* dangerZone = new QHBoxLayout(this); + // QPushButton* disconnect = new QPushButton("Disconnect from WiFi", this); + // dangerZone->addWidget(disconnect); + // QPushButton* deleteAll = new QPushButton("DELETE ALL NETWORKS", this); + // dangerZone->addWidget(deleteAll); + // vlayout->addWidget(layoutToWidget(dangerZone, this)); + + // vlayout to widget + QWidget* settingsWidget = layoutToWidget(vlayout, this); + settingsWidget->setStyleSheet("margin-left: 40px; margin-right: 40px;"); + s->addWidget(settingsWidget); + + ssh = new SSH; + connect(ssh, &SSH::closeSSHSettings, [=](){s->setCurrentWidget(settingsWidget);}); + s->addWidget(ssh); + + setLayout(s); +} + +bool AdvancedNetworking::isSSHEnabled(){ + QString response = QString::fromStdString(exec("systemctl is-active ssh")); + return response.startsWith("active"); +} + +void AdvancedNetworking::refresh(){ + ipLabel->setText(wifi->ipv4_address); + // Don't refresh while changing SSH state + if(!toggle_switch_SSH->getEnabled()){ + return; + } + if (toggle_switch_SSH->on != isSSHEnabled()) { + toggle_switch_SSH->togglePosition(); + } + // Qt can be lazy + repaint(); +} + +void AdvancedNetworking::toggleTethering(int enable) { + if (enable) { + wifi->enableTethering(); + } else { + wifi->disableTethering(); + } + editPasswordButton->setEnabled(!enable); +} + +void enableSSH(Toggle* toggle_switch_SSH){ + Params().write_db_value("SshEnabled", "1"); + toggle_switch_SSH->setEnabled(true); +} + +void disableSSH(Toggle* toggle_switch_SSH){ + Params().write_db_value("SshEnabled", "0"); + toggle_switch_SSH->setEnabled(true); +} + +void AdvancedNetworking::toggleSSH(int enable) { + toggle_switch_SSH->setEnabled(false); + if (enable) { + QtConcurrent::run(enableSSH, toggle_switch_SSH); + } else { + QtConcurrent::run(disableSSH, toggle_switch_SSH); + } +} + + +// WifiUI functions + +WifiUI::WifiUI(QWidget *parent, int page_length, WifiManager* wifi) : QWidget(parent), networks_per_page(page_length), wifi(wifi) { + vlayout = new QVBoxLayout; + + // Scan on startup + QLabel *scanning = new QLabel("Scanning for networks"); + scanning->setStyleSheet(R"(font-size: 65px;)"); + vlayout->addWidget(scanning, 0, Qt::AlignCenter); + vlayout->setSpacing(25); + + setLayout(vlayout); + page = 0; +} + +void WifiUI::refresh() { + wifi->request_scan(); + wifi->refreshNetworks(); + clearLayout(vlayout); + + connectButtons = new QButtonGroup(this); // TODO check if this is a leak + QObject::connect(connectButtons, SIGNAL(buttonClicked(QAbstractButton*)), this, SLOT(handleButton(QAbstractButton*))); + + int i = 0; + int countWidgets = 0; + for (Network &network : wifi->seen_networks) { + QHBoxLayout *hlayout = new QHBoxLayout; + if (page * networks_per_page <= i && i < (page + 1) * networks_per_page) { + // SSID + hlayout->addSpacing(50); + QString ssid = QString::fromUtf8(network.ssid); + if(ssid.length() > 20){ + ssid = ssid.left(20 - 3) + "…"; + } + + QLabel *ssid_label = new QLabel(ssid); + ssid_label->setStyleSheet(R"( + font-size: 55px; + )"); + ssid_label->setFixedWidth(this->width()*0.5); + hlayout->addWidget(ssid_label, 0, Qt::AlignLeft); + + // TODO: don't use images for this + // strength indicator + unsigned int strength_scale = network.strength / 17; + QPixmap pix("../assets/images/network_" + QString::number(strength_scale) + ".png"); + QLabel *icon = new QLabel(); + icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); + icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + hlayout->addWidget(icon, 0, Qt::AlignRight); + + // connect button + QPushButton* btn = new QPushButton(network.security_type == SecurityType::UNSUPPORTED ? "Unsupported" : (network.connected == ConnectedType::CONNECTED ? "Connected" : (network.connected == ConnectedType::CONNECTING ? "Connecting" : "Connect"))); + btn->setDisabled(network.connected == ConnectedType::CONNECTED || network.connected == ConnectedType::CONNECTING || network.security_type == SecurityType::UNSUPPORTED); + btn->setFixedWidth(350); + hlayout->addWidget(btn, 0, Qt::AlignRight); + + connectButtons->addButton(btn, i); + + vlayout->addLayout(hlayout, 1); + // Don't add the last horizontal line + if (page * networks_per_page <= i+1 && i+1 < (page + 1) * networks_per_page && i+1 < wifi->seen_networks.size()) { + vlayout->addWidget(hline(), 0); + } + countWidgets++; + } + i++; + } + vlayout->addStretch(3); + + + // Setup buttons for pagination + QHBoxLayout *prev_next_buttons = new QHBoxLayout; + + QPushButton* prev = new QPushButton("Previous"); + prev->setEnabled(page); + QObject::connect(prev, SIGNAL(released()), this, SLOT(prevPage())); + prev_next_buttons->addWidget(prev); + + QPushButton* next = new QPushButton("Next"); + next->setEnabled(wifi->seen_networks.size() > (page + 1) * networks_per_page); + QObject::connect(next, SIGNAL(released()), this, SLOT(nextPage())); + prev_next_buttons->addWidget(next); + + vlayout->addLayout(prev_next_buttons, 2); +} + +void WifiUI::handleButton(QAbstractButton* button) { + QPushButton* btn = static_cast(button); + Network n = wifi->seen_networks[connectButtons->id(btn)]; + emit connectToNetwork(n); +} + +void WifiUI::prevPage() { + page--; + refresh(); +} + +void WifiUI::nextPage() { + page++; + refresh(); +} diff --git a/selfdrive/ui/qt/offroad/networking.hpp b/selfdrive/ui/qt/offroad/networking.hpp new file mode 100644 index 000000000..64f010121 --- /dev/null +++ b/selfdrive/ui/qt/offroad/networking.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "wifiManager.hpp" +#include "widgets/input_field.hpp" +#include "widgets/ssh_keys.hpp" +#include "widgets/toggle.hpp" + +class WifiUI : public QWidget { + Q_OBJECT + +public: + int page; + explicit WifiUI(QWidget *parent = 0, int page_length = 5, WifiManager* wifi = 0); + +private: + WifiManager *wifi = nullptr; + int networks_per_page; + QVBoxLayout *vlayout; + + QButtonGroup *connectButtons; + bool tetheringEnabled; + +signals: + void openKeyboard(); + void closeKeyboard(); + void connectToNetwork(Network n); + +public slots: + void handleButton(QAbstractButton* m_button); + void refresh(); + + void prevPage(); + void nextPage(); +}; + +class AdvancedNetworking : public QWidget { + Q_OBJECT +public: + explicit AdvancedNetworking(QWidget* parent = 0, WifiManager* wifi = 0); + QStackedLayout* s; + +private: + QLabel* ipLabel; + QPushButton* editPasswordButton; + SSH* ssh; + Toggle* toggle_switch_SSH; + + WifiManager* wifi = nullptr; + + bool isSSHEnabled(); + +signals: + void backPress(); + +public slots: + void toggleTethering(int enable); + void toggleSSH(int enable); + void refresh(); +}; + +class Networking : public QWidget { + Q_OBJECT + +public: + explicit Networking(QWidget* parent = 0, bool show_advanced = true); + +private: + QStackedLayout* s = nullptr; // nm_warning, keyboard, wifiScreen, advanced + QWidget* wifiScreen = nullptr; + AdvancedNetworking* an = nullptr; + bool ui_setup_complete = false; + bool show_advanced; + + Network selectedNetwork; + + WifiUI* wifiWidget; + WifiManager* wifi = nullptr; + void attemptInitialization(); + +private slots: + void connectToNetwork(Network n); + void refresh(); + void wrongPassword(QString ssid); +}; + diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc index 697dea427..00d48520d 100644 --- a/selfdrive/ui/qt/offroad/onboarding.cc +++ b/selfdrive/ui/qt/offroad/onboarding.cc @@ -3,43 +3,101 @@ #include #include #include +#include +#include +#include -#include "onboarding.hpp" #include "common/params.h" +#include "onboarding.hpp" +#include "home.hpp" +#include "util.h" QLabel * title_label(QString text) { QLabel *l = new QLabel(text); - l->setStyleSheet(R"(font-size: 100px;)"); + l->setStyleSheet(R"( + font-size: 100px; + font-weight: 400; + )"); return l; } -QWidget * OnboardingWindow::terms_screen() { +QWidget * layout2Widget(QLayout* l){ + QWidget *q = new QWidget; + q->setLayout(l); + return q; +} - QGridLayout *main_layout = new QGridLayout(); - main_layout->setMargin(30); - main_layout->setSpacing(30); - main_layout->addWidget(title_label("Review Terms"), 0, 0, 1, -1); +void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) { + int leftOffset = (geometry().width()-1620)/2; + int mousex = e->x()-leftOffset; + int mousey = e->y(); - QLabel *terms = new QLabel("See terms at https://my.comma.ai/terms"); - terms->setAlignment(Qt::AlignCenter); - terms->setStyleSheet(R"( - font-size: 75px; - border-radius: 10px; - background-color: #292929; + // Check for restart + if (currentIndex == boundingBox.size()-1) { + if (1050 <= mousex && mousex <= 1500 && 773 <= mousey && mousey <= 954){ + slayout->setCurrentIndex(0); + currentIndex = 0; + return; + } + } + + if (boundingBox[currentIndex][0] <= mousex && mousex <= boundingBox[currentIndex][1] && boundingBox[currentIndex][2] <= mousey && mousey <= boundingBox[currentIndex][3]) { + slayout->setCurrentIndex(++currentIndex); + } + if (currentIndex >= boundingBox.size()) { + emit completedTraining(); + return; + } +} + +TrainingGuide::TrainingGuide(QWidget* parent) { + QHBoxLayout* hlayout = new QHBoxLayout; + + slayout = new QStackedLayout(this); + for (int i = 0; i < boundingBox.size(); i++) { + QWidget* w = new QWidget; + w->setStyleSheet(".QWidget {background-image: url(../assets/training/step" + QString::number(i) + ".jpg);}"); + w->setFixedSize(1620, 1080); + slayout->addWidget(w); + } + + QWidget* sw = layout2Widget(slayout); + hlayout->addWidget(sw, 1, Qt::AlignCenter); + setLayout(hlayout); + setStyleSheet(R"( + background-color: #072339; )"); - main_layout->addWidget(terms, 1, 0, 1, -1); - main_layout->setRowStretch(1, 1); +} - QPushButton *accept_btn = new QPushButton("Accept"); - main_layout->addWidget(accept_btn, 2, 1); + +QWidget* OnboardingWindow::terms_screen() { + QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->setContentsMargins(40, 0, 40, 0); + + view = new QWebEngineView(this); + view->settings()->setAttribute(QWebEngineSettings::ShowScrollBars, false); + QString html = QString::fromStdString(util::read_file("../assets/offroad/tc.html")); + view->setHtml(html); + main_layout->addWidget(view); + + QHBoxLayout* buttons = new QHBoxLayout; + buttons->addWidget(new QPushButton("Decline")); + buttons->addSpacing(50); + accept_btn = new QPushButton("Scroll to accept"); + accept_btn->setEnabled(false); + buttons->addWidget(accept_btn); QObject::connect(accept_btn, &QPushButton::released, [=]() { - Params().write_db_value("HasAcceptedTerms", LATEST_TERMS_VERSION); + Params().write_db_value("HasAcceptedTerms", current_terms_version); updateActiveScreen(); }); - main_layout->addWidget(new QPushButton("Decline"), 2, 0); + QObject::connect(view->page(), SIGNAL(scrollPositionChanged(QPointF)), this, SLOT(scrollPositionChanged(QPointF))); + + QWidget* w = layout2Widget(buttons); + w->setFixedHeight(200); + main_layout->addWidget(w); QWidget *widget = new QWidget; widget->setLayout(main_layout); @@ -55,34 +113,11 @@ QWidget * OnboardingWindow::terms_screen() { return widget; } -QWidget * OnboardingWindow::training_screen() { - - QVBoxLayout *main_layout = new QVBoxLayout(); - main_layout->setMargin(30); - main_layout->setSpacing(30); - - main_layout->addWidget(title_label("Training Guide")); - - main_layout->addWidget(new QLabel(), 1); // just a spacer - - QPushButton *btn = new QPushButton("Continue"); - main_layout->addWidget(btn); - QObject::connect(btn, &QPushButton::released, [=]() { - Params().write_db_value("CompletedTrainingVersion", LATEST_TRAINING_VERSION); - updateActiveScreen(); - }); - - QWidget *widget = new QWidget; - widget->setLayout(main_layout); - return widget; -} - void OnboardingWindow::updateActiveScreen() { - Params params = Params(); - bool accepted_terms = params.get("HasAcceptedTerms", false).compare(LATEST_TERMS_VERSION) == 0; - bool training_done = params.get("CompletedTrainingVersion", false).compare(LATEST_TRAINING_VERSION) == 0; + bool accepted_terms = params.get("HasAcceptedTerms", false).compare(current_terms_version) == 0; + bool training_done = params.get("CompletedTrainingVersion", false).compare(current_training_version) == 0; if (!accepted_terms) { setCurrentIndex(0); } else if (!training_done) { @@ -92,21 +127,48 @@ void OnboardingWindow::updateActiveScreen() { } } -OnboardingWindow::OnboardingWindow(QWidget *parent) { +OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) { + Params params = Params(); + current_terms_version = params.get("TermsVersion", false); + current_training_version = params.get("TrainingVersion", false); + bool accepted_terms = params.get("HasAcceptedTerms", false).compare(current_terms_version) == 0; + bool training_done = params.get("CompletedTrainingVersion", false).compare(current_training_version) == 0; + + //Don't initialize widgets unless neccesary. + if (accepted_terms && training_done) { + return; + } addWidget(terms_screen()); - addWidget(training_screen()); + + TrainingGuide* tr = new TrainingGuide(this); + connect(tr, &TrainingGuide::completedTraining, [=](){ + Params().write_db_value("CompletedTrainingVersion", current_training_version); + updateActiveScreen(); + }); + addWidget(tr); setStyleSheet(R"( * { + color: white; background-color: black; } QPushButton { - font-size: 50px; padding: 50px; - border-radius: 10px; + border-radius: 30px; background-color: #292929; } + QPushButton:disabled { + color: #777777; + background-color: #222222; + } )"); updateActiveScreen(); } + +void OnboardingWindow::scrollPositionChanged(QPointF position){ + if (position.y() > view->page()->contentsSize().height() - 1000){ + accept_btn->setEnabled(true); + accept_btn->setText("Accept"); + } +} diff --git a/selfdrive/ui/qt/offroad/onboarding.hpp b/selfdrive/ui/qt/offroad/onboarding.hpp index 9e4aabeb7..f427cf79c 100644 --- a/selfdrive/ui/qt/offroad/onboarding.hpp +++ b/selfdrive/ui/qt/offroad/onboarding.hpp @@ -2,10 +2,32 @@ #include #include +#include +#include +#include +#include +#include -// TODO: this is defined in python too -#define LATEST_TERMS_VERSION "2" -#define LATEST_TRAINING_VERSION "0.2.0" + +class TrainingGuide : public QFrame { + Q_OBJECT + +public: + explicit TrainingGuide(QWidget *parent = 0); + +protected: + void mouseReleaseEvent(QMouseEvent* e) override; + +private: + int currentIndex = 0; + QStackedLayout* slayout; + //Vector of bounding boxes for the next step. (minx, maxx, miny, maxy) + QVector> boundingBox {{250, 930, 750, 900}, {280, 1280, 650, 950}, {330, 1130, 590, 900}, {910, 1580, 500, 1000}, {1180, 1300, 630, 720}, {290, 1050, 590, 960}, + {1090, 1240, 550, 660}, {1050, 1580, 250, 900}, {320, 1130, 670, 1020}, {1010, 1580, 410, 750}, {1040, 1500, 230, 1030}, {300, 1190, 590, 920}, {1050, 1310, 170, 870}, {950, 1530, 460, 770}, {190, 970, 750, 970}}; + +signals: + void completedTraining(); +}; class OnboardingWindow : public QStackedWidget { Q_OBJECT @@ -14,12 +36,19 @@ public: explicit OnboardingWindow(QWidget *parent = 0); private: + std::string current_terms_version; + std::string current_training_version; QWidget * terms_screen(); QWidget * training_screen(); + QWebEngineView* view; + QPushButton* accept_btn; signals: void onboardingDone(); public slots: void updateActiveScreen(); + +private slots: + void scrollPositionChanged(QPointF position); }; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4e650a0c4..49b22e1c9 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -10,60 +10,64 @@ #include #include -#include "wifi.hpp" +#include "networking.hpp" #include "settings.hpp" #include "widgets/toggle.hpp" #include "widgets/offroad_alerts.hpp" #include "common/params.h" -#include "common/utilpp.h" +#include "common/util.h" +QFrame* horizontal_line(QWidget* parent = 0){ + QFrame* line = new QFrame(parent); + line->setFrameShape(QFrame::StyledPanel); + line->setStyleSheet("margin-left: 40px; margin-right: 40px; border-width: 1px; border-bottom-style: solid; border-color: gray;"); + line->setFixedHeight(2); + return line; +} +QWidget* labelWidget(QString labelName, QString labelContent){ + QHBoxLayout* labelLayout = new QHBoxLayout; + labelLayout->addWidget(new QLabel(labelName), 0, Qt::AlignLeft); + QLabel* paramContent = new QLabel(labelContent); + paramContent->setStyleSheet("color: #aaaaaa"); + labelLayout->addWidget(paramContent, 0, Qt::AlignRight); + QWidget* labelWidget = new QWidget; + labelWidget->setLayout(labelLayout); + return labelWidget; +} ParamsToggle::ParamsToggle(QString param, QString title, QString description, QString icon_path, QWidget *parent): QFrame(parent) , param(param) { - QHBoxLayout *hlayout = new QHBoxLayout; + QHBoxLayout *layout = new QHBoxLayout; + layout->setSpacing(50); // Parameter image - hlayout->addSpacing(25); if (icon_path.length()) { QPixmap pix(icon_path); QLabel *icon = new QLabel(); - icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); + icon->setPixmap(pix.scaledToWidth(80, Qt::SmoothTransformation)); icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - hlayout->addWidget(icon); + layout->addWidget(icon); } else { - hlayout->addSpacing(100); + layout->addSpacing(80); } - hlayout->addSpacing(25); // Name of the parameter QLabel *label = new QLabel(title); - label->setWordWrap(true); + label->setStyleSheet(R"(font-size: 50px;)"); + layout->addWidget(label); // toggle switch - Toggle* toggle_switch = new Toggle(this); - toggle_switch->setFixedSize(150, 100); + Toggle *toggle = new Toggle(this); + toggle->setFixedSize(150, 100); + layout->addWidget(toggle); + QObject::connect(toggle, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); - // TODO: show descriptions on tap - hlayout->addWidget(label); - hlayout->addSpacing(50); - hlayout->addWidget(toggle_switch); - hlayout->addSpacing(20); - - setLayout(hlayout); + // set initial state from param if (Params().read_db_bool(param.toStdString().c_str())) { - toggle_switch->togglePosition(); + toggle->togglePosition(); } - setStyleSheet(R"( - QLabel { - font-size: 50px; - } - * { - background-color: #114265; - } - )"); - - QObject::connect(toggle_switch, SIGNAL(stateChanged(int)), this, SLOT(checkboxClicked(int))); + setLayout(layout); } void ParamsToggle::checkboxClicked(int state) { @@ -72,40 +76,45 @@ void ParamsToggle::checkboxClicked(int state) { } QWidget * toggles_panel() { - QVBoxLayout *toggles_list = new QVBoxLayout(); - toggles_list->setSpacing(25); + toggles_list->setMargin(50); toggles_list->addWidget(new ParamsToggle("OpenpilotEnabledToggle", - "Enable Openpilot", + "Enable openpilot", "Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.", "../assets/offroad/icon_openpilot.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("LaneChangeEnabled", "Enable Lane Change Assist", "Perform assisted lane changes with openpilot by checking your surroundings for safety, activating the turn signal and gently nudging the steering wheel towards your desired lane. openpilot is not capable of checking if a lane change is safe. You must continuously observe your surroundings to use this feature.", "../assets/offroad/icon_road.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("IsLdwEnabled", "Enable Lane Departure Warnings", "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31mph (50kph).", "../assets/offroad/icon_warning.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("RecordFront", "Record and Upload Driver Camera", "Upload data from the driver facing camera and help improve the driver monitoring algorithm.", "../assets/offroad/icon_network.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("IsRHD", "Enable Right-Hand Drive", "Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.", "../assets/offroad/icon_openpilot_mirrored.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("IsMetric", "Use Metric System", "Display speed in km/h instead of mp/h.", "../assets/offroad/icon_metric.png" )); + toggles_list->addWidget(horizontal_line()); toggles_list->addWidget(new ParamsToggle("CommunityFeaturesToggle", "Enable Community Features", "Use features from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. These features include community supported cars and community supported hardware. Be extra cautious when using these features", @@ -120,44 +129,56 @@ QWidget * toggles_panel() { QWidget * device_panel() { QVBoxLayout *device_layout = new QVBoxLayout; - device_layout->setSpacing(50); + device_layout->setMargin(100); + device_layout->setSpacing(30); Params params = Params(); std::vector> labels = { {"Dongle ID", params.get("DongleId", false)}, - //{"Serial Number", "abcdefghijk"}, }; - for (auto l : labels) { - QString text = QString::fromStdString(l.first + ": " + l.second); - device_layout->addWidget(new QLabel(text)); + // get serial number + //std::string cmdline = util::read_file("/proc/cmdline"); + //auto delim = cmdline.find("serialno="); + //if (delim != std::string::npos) { + // labels.push_back({"Serial", cmdline.substr(delim, cmdline.find(" ", delim))}); + //} + + for (auto &l : labels) { + device_layout->addWidget(labelWidget(QString::fromStdString(l.first), QString::fromStdString(l.second)), 0, Qt::AlignTop); } + // TODO: show current calibration values QPushButton *clear_cal_btn = new QPushButton("Reset Calibration"); - device_layout->addWidget(clear_cal_btn); + device_layout->addWidget(clear_cal_btn, 0, Qt::AlignBottom); + device_layout->addWidget(horizontal_line(), Qt::AlignBottom); QObject::connect(clear_cal_btn, &QPushButton::released, [=]() { Params().delete_db_value("CalibrationParams"); }); - std::map power_btns = { - {"Power Off", "sudo poweroff"}, - {"Reboot", "sudo reboot"}, - }; - - for (auto b : power_btns) { - QPushButton *btn = new QPushButton(QString::fromStdString(b.first)); - device_layout->addWidget(btn); + QPushButton *poweroff_btn = new QPushButton("Power Off"); + device_layout->addWidget(poweroff_btn, Qt::AlignBottom); + QPushButton *reboot_btn = new QPushButton("Reboot"); + device_layout->addWidget(reboot_btn, Qt::AlignBottom); + device_layout->addWidget(horizontal_line(), Qt::AlignBottom); #ifdef __aarch64__ - QObject::connect(btn, &QPushButton::released, - [=]() {std::system(b.second);}); + QObject::connect(poweroff_btn, &QPushButton::released, [=]() { std::system("sudo poweroff"); }); + QObject::connect(reboot_btn, &QPushButton::released, [=]() { std::system("sudo reboot"); }); #endif - } + + // TODO: add confirmation dialog + QPushButton *uninstall_btn = new QPushButton("Uninstall openpilot"); + device_layout->addWidget(uninstall_btn); + QObject::connect(uninstall_btn, &QPushButton::released, [=]() { Params().write_db_value("DoUninstall", "1"); }); QWidget *widget = new QWidget; widget->setLayout(device_layout); widget->setStyleSheet(R"( QPushButton { - padding: 60px; + padding: 0; + height: 120px; + border-radius: 15px; + background-color: #393939; } )"); return widget; @@ -165,6 +186,7 @@ QWidget * device_panel() { QWidget * developer_panel() { QVBoxLayout *main_layout = new QVBoxLayout; + main_layout->setMargin(100); // TODO: enable SSH toggle and github keys @@ -182,9 +204,13 @@ QWidget * developer_panel() { labels.push_back({"OS Version", "AGNOS " + os_version}); } - for (auto l : labels) { - QString text = QString::fromStdString(l.first + ": " + l.second); - main_layout->addWidget(new QLabel(text)); + for (int i = 0; iaddWidget(labelWidget(QString::fromStdString(l.first), QString::fromStdString(l.second))); + + if(i+1addWidget(horizontal_line()); + } } QWidget *widget = new QWidget; @@ -198,72 +224,94 @@ QWidget * developer_panel() { } QWidget * network_panel(QWidget * parent) { - WifiUI *w = new WifiUI(); - QObject::connect(w, SIGNAL(openKeyboard()), parent, SLOT(closeSidebar())); - QObject::connect(w, SIGNAL(closeKeyboard()), parent, SLOT(openSidebar())); + Networking *w = new Networking(parent); return w; } void SettingsWindow::setActivePanel() { - QPushButton *btn = qobject_cast(sender()); + auto *btn = qobject_cast(nav_btns->checkedButton()); panel_layout->setCurrentWidget(panels[btn->text()]); } -SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { - // sidebar +SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) { + // setup two main layouts QVBoxLayout *sidebar_layout = new QVBoxLayout(); + sidebar_layout->setMargin(0); panel_layout = new QStackedLayout(); // close button - QPushButton *close_button = new QPushButton("X"); - close_button->setStyleSheet(R"( - QPushButton { - padding: 50px; - font-weight: bold; - font-size: 100px; - } + QPushButton *close_btn = new QPushButton("X"); + close_btn->setStyleSheet(R"( + font-size: 90px; + font-weight: bold; + border 1px grey solid; + border-radius: 100px; + background-color: #292929; )"); - sidebar_layout->addWidget(close_button); - QObject::connect(close_button, SIGNAL(released()), this, SIGNAL(closeSettings())); + close_btn->setFixedSize(200, 200); + sidebar_layout->addSpacing(45); + sidebar_layout->addWidget(close_btn, 0, Qt::AlignCenter); + QObject::connect(close_btn, SIGNAL(released()), this, SIGNAL(closeSettings())); // setup panels panels = { - {"developer", developer_panel()}, - {"device", device_panel()}, - {"network", network_panel(this)}, - {"toggles", toggles_panel()}, + {"Developer", developer_panel()}, + {"Device", device_panel()}, + {"Network", network_panel(this)}, + {"Toggles", toggles_panel()}, }; + sidebar_layout->addSpacing(45); + nav_btns = new QButtonGroup(); for (auto &panel : panels) { QPushButton *btn = new QPushButton(panel.first); + btn->setCheckable(true); btn->setStyleSheet(R"( - QPushButton { - padding-top: 35px; - padding-bottom: 35px; - font-size: 60px; - text-align: right; + * { + color: grey; border: none; background: none; - font-weight: bold; + font-size: 65px; + font-weight: 500; + padding-top: 35px; + padding-bottom: 35px; + } + QPushButton:checked { + color: white; } )"); - sidebar_layout->addWidget(btn); + nav_btns->addButton(btn); + sidebar_layout->addWidget(btn, 0, Qt::AlignRight); panel_layout->addWidget(panel.second); QObject::connect(btn, SIGNAL(released()), this, SLOT(setActivePanel())); + QObject::connect(btn, &QPushButton::released, [=](){emit sidebarPressed();}); } + qobject_cast(nav_btns->buttons()[0])->setChecked(true); + sidebar_layout->setContentsMargins(50, 50, 100, 50); + // main settings layout, sidebar + main panel QHBoxLayout *settings_layout = new QHBoxLayout(); - settings_layout->addSpacing(45); sidebar_widget = new QWidget; sidebar_widget->setLayout(sidebar_layout); + sidebar_widget->setFixedWidth(500); settings_layout->addWidget(sidebar_widget); - settings_layout->addSpacing(45); - settings_layout->addLayout(panel_layout); - settings_layout->addSpacing(45); + + panel_frame = new QFrame; + panel_frame->setLayout(panel_layout); + panel_frame->setStyleSheet(R"( + QFrame { + border-radius: 30px; + background-color: #292929; + } + * { + background-color: none; + } + )"); + settings_layout->addWidget(panel_frame); setLayout(settings_layout); setStyleSheet(R"( @@ -271,13 +319,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QWidget(parent) { color: white; font-size: 50px; } + SettingsWindow { + background-color: black; + } )"); } - -void SettingsWindow::closeSidebar() { - sidebar_widget->setVisible(false); -} - -void SettingsWindow::openSidebar() { - sidebar_widget->setVisible(true); -} diff --git a/selfdrive/ui/qt/offroad/settings.hpp b/selfdrive/ui/qt/offroad/settings.hpp index b2c0a911d..e10a2652c 100644 --- a/selfdrive/ui/qt/offroad/settings.hpp +++ b/selfdrive/ui/qt/offroad/settings.hpp @@ -4,9 +4,10 @@ #include #include #include +#include #include -#include "wifi.hpp" +#include "networking.hpp" // *** settings widgets *** @@ -27,7 +28,7 @@ public slots: // *** settings window *** -class SettingsWindow : public QWidget { +class SettingsWindow : public QFrame { Q_OBJECT public: @@ -35,15 +36,16 @@ public: signals: void closeSettings(); + void sidebarPressed(); private: QPushButton *sidebar_alert_widget; QWidget *sidebar_widget; std::map panels; + QButtonGroup *nav_btns; QStackedLayout *panel_layout; + QFrame* panel_frame; public slots: void setActivePanel(); - void closeSidebar(); - void openSidebar(); }; diff --git a/selfdrive/ui/qt/offroad/wifi.cc b/selfdrive/ui/qt/offroad/wifi.cc deleted file mode 100644 index ae641f509..000000000 --- a/selfdrive/ui/qt/offroad/wifi.cc +++ /dev/null @@ -1,267 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "wifi.hpp" -#include "widgets/toggle.hpp" - -void clearLayout(QLayout* layout) { - while (QLayoutItem* item = layout->takeAt(0)) { - if (QWidget* widget = item->widget()) { - widget->deleteLater(); - } - if (QLayout* childLayout = item->layout()) { - clearLayout(childLayout); - } - delete item; - } -} - -WifiUI::WifiUI(QWidget *parent, int page_length) : QWidget(parent), networks_per_page(page_length) { - try { - wifi = new WifiManager; - } catch (std::exception &e) { - QLabel* warning = new QLabel("Network manager is inactive!"); - warning->setStyleSheet(R"(font-size: 65px;)"); - - QVBoxLayout* warning_layout = new QVBoxLayout; - warning_layout->addWidget(warning, 0, Qt::AlignCenter); - setLayout(warning_layout); - return; - } - - QObject::connect(wifi, SIGNAL(wrongPassword(QString)), this, SLOT(wrongPassword(QString))); - - QVBoxLayout * top_layout = new QVBoxLayout; - top_layout->setSpacing(0); - swidget = new QStackedWidget; - - // Networks page - wifi_widget = new QWidget; - QVBoxLayout* networkLayout = new QVBoxLayout; - QHBoxLayout *tethering_field = new QHBoxLayout; - tethering_field->addSpacing(50); - - ipv4 = new QLabel(""); - tethering_field->addWidget(ipv4); - tethering_field->addWidget(new QLabel("Enable Tethering")); - - Toggle* toggle_switch = new Toggle(this); - toggle_switch->setFixedSize(150, 100); - tethering_field->addWidget(toggle_switch); - if (wifi->tetheringEnabled()) { - toggle_switch->togglePosition(); - } - QObject::connect(toggle_switch, SIGNAL(stateChanged(int)), this, SLOT(toggleTethering(int))); - - QWidget* tetheringWidget = new QWidget; - tetheringWidget->setLayout(tethering_field); - tetheringWidget->setFixedHeight(150); - networkLayout->addWidget(tetheringWidget); - - vlayout = new QVBoxLayout; - wifi_widget->setLayout(vlayout); - networkLayout->addWidget(wifi_widget); - - QWidget* networkWidget = new QWidget; - networkWidget->setLayout(networkLayout); - swidget->addWidget(networkWidget); - - // Keyboard page - input_field = new InputField(); - QObject::connect(input_field, SIGNAL(emitText(QString)), this, SLOT(receiveText(QString))); - swidget->addWidget(input_field); - swidget->setCurrentIndex(0); - - top_layout->addWidget(swidget); - setLayout(top_layout); - - // Update network list - timer = new QTimer(this); - QObject::connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); - timer->start(2000); - - // Scan on startup - QLabel *scanning = new QLabel("Scanning for networks"); - scanning->setStyleSheet(R"(font-size: 65px;)"); - vlayout->addWidget(scanning, 0, Qt::AlignCenter); - vlayout->setSpacing(25); - - wifi->request_scan(); - refresh(); - page = 0; -} - -void WifiUI::refresh() { - if (!this->isVisible()) { - return; - } - - wifi->request_scan(); - wifi->refreshNetworks(); - ipv4->setText(wifi->ipv4_address); - clearLayout(vlayout); - - connectButtons = new QButtonGroup(this); - QObject::connect(connectButtons, SIGNAL(buttonClicked(QAbstractButton*)), this, SLOT(handleButton(QAbstractButton*))); - - int i = 0; - int countWidgets = 0; - int button_height = static_cast(this->height() / (networks_per_page + 1) * 0.6); - for (Network &network : wifi->seen_networks) { - QHBoxLayout *hlayout = new QHBoxLayout; - if (page * networks_per_page <= i && i < (page + 1) * networks_per_page) { - // SSID - hlayout->addSpacing(50); - hlayout->addWidget(new QLabel(QString::fromUtf8(network.ssid))); - - // strength indicator - unsigned int strength_scale = network.strength / 17; - QPixmap pix("../assets/images/network_" + QString::number(strength_scale) + ".png"); - QLabel *icon = new QLabel(); - icon->setPixmap(pix.scaledToWidth(100, Qt::SmoothTransformation)); - icon->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - hlayout->addWidget(icon); - hlayout->addSpacing(20); - - // connect button - QPushButton* btn = new QPushButton(network.connected == ConnectedType::CONNECTED ? "Connected" : (network.connected == ConnectedType::CONNECTING ? "Connecting" : "Connect")); - btn->setFixedWidth(300); - btn->setFixedHeight(button_height); - btn->setDisabled(network.connected == ConnectedType::CONNECTED || network.connected == ConnectedType::CONNECTING || network.security_type == SecurityType::UNSUPPORTED); - hlayout->addWidget(btn); - hlayout->addSpacing(20); - - connectButtons->addButton(btn, i); - - QWidget * w = new QWidget; - w->setLayout(hlayout); - vlayout->addWidget(w); - w->setStyleSheet(R"( - QLabel { - font-size: 50px; - } - QPushButton { - padding: 0; - font-size: 50px; - background-color: #114265; - } - QPushButton:disabled { - background-color: #323C43; - } - * { - background-color: #114265; - } - )"); - countWidgets++; - } - i++; - } - - // Pad vlayout to prevert oversized network widgets in case of low visible network count - for (int i = countWidgets; i < networks_per_page; i++) { - QWidget *w = new QWidget; - vlayout->addWidget(w); - } - - QHBoxLayout *prev_next_buttons = new QHBoxLayout; - QPushButton* prev = new QPushButton("Previous"); - prev->setEnabled(page); - prev->setFixedHeight(button_height); - QPushButton* next = new QPushButton("Next"); - next->setFixedHeight(button_height); - - // If there are more visible networks then we can show, enable going to next page - next->setEnabled(wifi->seen_networks.size() > (page + 1) * networks_per_page); - - QObject::connect(prev, SIGNAL(released()), this, SLOT(prevPage())); - QObject::connect(next, SIGNAL(released()), this, SLOT(nextPage())); - prev_next_buttons->addWidget(prev); - prev_next_buttons->addWidget(next); - - QWidget *w = new QWidget; - w->setLayout(prev_next_buttons); - w->setStyleSheet(R"( - QPushButton { - padding: 0; - background-color: #114265; - } - QPushButton:disabled { - background-color: #323C43; - } - * { - background-color: #114265; - } - )"); - vlayout->addWidget(w); -} - - - -void WifiUI::toggleTethering(int enable) { - if (enable) { - wifi->enableTethering(); - } else { - wifi->disableTethering(); - } -} - -void WifiUI::handleButton(QAbstractButton* button) { - QPushButton* btn = static_cast(button); - Network n = wifi->seen_networks[connectButtons->id(btn)]; - connectToNetwork(n); -} - -void WifiUI::connectToNetwork(Network n) { - timer->stop(); - if (n.security_type == SecurityType::OPEN) { - wifi->connect(n); - } else if (n.security_type == SecurityType::WPA) { - input_field->setPromptText("Enter password for \"" + n.ssid + "\""); - QString password = getStringFromUser(); - if (password.size()) { - wifi->connect(n, password); - } - } - refresh(); - timer->start(); -} - -QString WifiUI::getStringFromUser() { - emit openKeyboard(); - swidget->setCurrentIndex(1); - loop.exec(); - emit closeKeyboard(); - swidget->setCurrentIndex(0); - return text; -} - -void WifiUI::receiveText(QString t) { - loop.quit(); - text = t; -} - - -void WifiUI::wrongPassword(QString ssid) { - if (loop.isRunning()) { - return; - } - for (Network n : wifi->seen_networks) { - if (n.ssid == ssid) { - input_field->setPromptText("Wrong password for \"" + n.ssid +"\""); - connectToNetwork(n); - } - } -} - -void WifiUI::prevPage() { - page--; - refresh(); -} -void WifiUI::nextPage() { - page++; - refresh(); -} diff --git a/selfdrive/ui/qt/offroad/wifi.hpp b/selfdrive/ui/qt/offroad/wifi.hpp deleted file mode 100644 index bad8d9356..000000000 --- a/selfdrive/ui/qt/offroad/wifi.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include "wifiManager.hpp" -#include "widgets/input_field.hpp" - - -class WifiUI : public QWidget { - Q_OBJECT - -public: - int page; - explicit WifiUI(QWidget *parent = 0, int page_length = 5); - -private: - WifiManager *wifi = nullptr; - const int networks_per_page; - - QStackedWidget *swidget; - QVBoxLayout *vlayout; - QWidget *wifi_widget; - - InputField *input_field; - QEventLoop loop; - QTimer *timer; - QString text; - QButtonGroup *connectButtons; - bool tetheringEnabled; - QLabel *ipv4; - - void connectToNetwork(Network n); - QString getStringFromUser(); - -private slots: - void handleButton(QAbstractButton* m_button); - void toggleTethering(int enable); - void refresh(); - void receiveText(QString text); - void wrongPassword(QString ssid); - - void prevPage(); - void nextPage(); - -signals: - void openKeyboard(); - void closeKeyboard(); -}; diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index ff8977c9a..057e52e82 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -2,8 +2,10 @@ #include #include #include -#include "wifiManager.hpp" + #include "common/params.h" +#include "common/swaglog.h" +#include "wifiManager.hpp" /** * We are using a NetworkManager DBUS API : https://developer.gnome.org/NetworkManager/1.26/spec.html @@ -20,32 +22,37 @@ const int NM_802_11_AP_SEC_GROUP_WEP104 = 0x00000020; const int NM_802_11_AP_SEC_KEY_MGMT_PSK = 0x00000100; const int NM_802_11_AP_SEC_KEY_MGMT_802_1X = 0x00000200; +const QString nm_path = "/org/freedesktop/NetworkManager"; +const QString nm_settings_path = "/org/freedesktop/NetworkManager/Settings"; -QString nm_path = "/org/freedesktop/NetworkManager"; -QString nm_settings_path = "/org/freedesktop/NetworkManager/Settings"; +const QString nm_iface = "org.freedesktop.NetworkManager"; +const QString props_iface = "org.freedesktop.DBus.Properties"; +const QString nm_settings_iface = "org.freedesktop.NetworkManager.Settings"; +const QString nm_settings_conn_iface = "org.freedesktop.NetworkManager.Settings.Connection"; +const QString device_iface = "org.freedesktop.NetworkManager.Device"; +const QString wireless_device_iface = "org.freedesktop.NetworkManager.Device.Wireless"; +const QString ap_iface = "org.freedesktop.NetworkManager.AccessPoint"; +const QString connection_iface = "org.freedesktop.NetworkManager.Connection.Active"; +const QString ipv4config_iface = "org.freedesktop.NetworkManager.IP4Config"; -QString nm_iface = "org.freedesktop.NetworkManager"; -QString props_iface = "org.freedesktop.DBus.Properties"; -QString nm_settings_iface = "org.freedesktop.NetworkManager.Settings"; -QString nm_settings_conn_iface = "org.freedesktop.NetworkManager.Settings.Connection"; -QString device_iface = "org.freedesktop.NetworkManager.Device"; -QString wireless_device_iface = "org.freedesktop.NetworkManager.Device.Wireless"; -QString ap_iface = "org.freedesktop.NetworkManager.AccessPoint"; -QString connection_iface = "org.freedesktop.NetworkManager.Connection.Active"; -QString ipv4config_iface = "org.freedesktop.NetworkManager.IP4Config"; - -QString nm_service = "org.freedesktop.NetworkManager"; +const QString nm_service = "org.freedesktop.NetworkManager"; const int state_connected = 100; const int state_need_auth = 60; const int reason_wrong_password = 8; +const int dbus_timeout = 100; template T get_response(QDBusMessage response) { QVariant first = response.arguments().at(0); QDBusVariant dbvFirst = first.value(); QVariant vFirst = dbvFirst.variant(); - return vFirst.value(); + if (vFirst.canConvert()) { + return vFirst.value(); + } else { + LOGE("Variant unpacking failure"); + return T(); + } } bool compare_by_strength(const Network &a, const Network &b) { @@ -56,8 +63,7 @@ bool compare_by_strength(const Network &a, const Network &b) { return a.strength > b.strength; } - -WifiManager::WifiManager() { +WifiManager::WifiManager(QWidget* parent) { qDBusRegisterMetaType(); qDBusRegisterMetaType(); connecting_to_network = ""; @@ -65,27 +71,32 @@ WifiManager::WifiManager() { bool has_adapter = adapter != ""; if (!has_adapter){ - throw std::runtime_error("Error connecting to panda"); + throw std::runtime_error("Error connecting to NetworkManager"); } QDBusInterface nm(nm_service, adapter, device_iface, bus); bus.connect(nm_service, adapter, device_iface, "StateChanged", this, SLOT(change(unsigned int, unsigned int, unsigned int))); QDBusInterface device_props(nm_service, adapter, props_iface, bus); + device_props.setTimeout(dbus_timeout); QDBusMessage response = device_props.call("Get", device_iface, "State"); raw_adapter_state = get_response(response); change(raw_adapter_state, 0, 0); - // Compute tethering ssid as "Weedle" + first 4 characters of a dongle id + // Set tethering ssid as "weedle" + first 4 characters of a dongle id tethering_ssid = "weedle"; std::string bytes = Params().get("DongleId"); if (bytes.length() >= 4) { tethering_ssid+="-"+QString::fromStdString(bytes.substr(0,4)); } + + // Create dbus interface for tethering button. This populates the introspection cache, + // making sure all future creations are non-blocking + // https://bugreports.qt.io/browse/QTBUG-14485 + QDBusInterface(nm_service, nm_settings_path, nm_settings_iface, bus); } void WifiManager::refreshNetworks() { - bus = QDBusConnection::systemBus(); seen_networks.clear(); seen_ssids.clear(); ipv4_address = get_ipv4_address(); @@ -96,6 +107,7 @@ void WifiManager::refreshNetworks() { seen_ssids.push_back(network.ssid); seen_networks.push_back(network); } + } QString WifiManager::get_ipv4_address(){ @@ -103,23 +115,31 @@ QString WifiManager::get_ipv4_address(){ return ""; } QVector conns = get_active_connections(); - for (auto p : conns){ + for (auto &p : conns){ QString active_connection = p.path(); QDBusInterface nm(nm_service, active_connection, props_iface, bus); + nm.setTimeout(dbus_timeout); + QDBusObjectPath pth = get_response(nm.call("Get", connection_iface, "Ip4Config")); QString ip4config = pth.path(); - QDBusInterface nm2(nm_service, ip4config, props_iface, bus); - const QDBusArgument &arr = get_response(nm2.call("Get", ipv4config_iface, "AddressData")); - QMap pth2; - arr.beginArray(); - while (!arr.atEnd()){ - arr >> pth2; - QString ipv4 = pth2.value("address").value(); + QString type = get_response(nm.call("Get", connection_iface, "Type")); + + if (type == "802-11-wireless") { + QDBusInterface nm2(nm_service, ip4config, props_iface, bus); + nm2.setTimeout(dbus_timeout); + + const QDBusArgument &arr = get_response(nm2.call("Get", ipv4config_iface, "AddressData")); + QMap pth2; + arr.beginArray(); + while (!arr.atEnd()){ + arr >> pth2; + QString ipv4 = pth2.value("address").value(); + arr.endArray(); + return ipv4; + } arr.endArray(); - return ipv4; } - arr.endArray(); } return ""; } @@ -127,6 +147,8 @@ QString WifiManager::get_ipv4_address(){ QList WifiManager::get_networks() { QList r; QDBusInterface nm(nm_service, adapter, wireless_device_iface, bus); + nm.setTimeout(dbus_timeout); + QDBusMessage response = nm.call("GetAllAccessPoints"); QVariant first = response.arguments().at(0); @@ -190,7 +212,7 @@ void WifiManager::connect(Network n, QString password) { void WifiManager::connect(Network n, QString username, QString password) { connecting_to_network = n.ssid; - disconnect(); + // disconnect(); clear_connections(n.ssid); //Clear all connections that may already exist to the network we are connecting connect(n.ssid, username, password, n.security_type); } @@ -215,24 +237,34 @@ void WifiManager::connect(QByteArray ssid, QString username, QString password, S connection["ipv6"]["method"] = "ignore"; QDBusInterface nm_settings(nm_service, nm_settings_path, nm_settings_iface, bus); + nm_settings.setTimeout(dbus_timeout); + nm_settings.call("AddConnection", QVariant::fromValue(connection)); + activate_wifi_connection(QString(ssid)); } void WifiManager::deactivate_connections(QString ssid) { for (QDBusObjectPath active_connection_raw : get_active_connections()) { QString active_connection = active_connection_raw.path(); QDBusInterface nm(nm_service, active_connection, props_iface, bus); + nm.setTimeout(dbus_timeout); + QDBusObjectPath pth = get_response(nm.call("Get", connection_iface, "SpecificObject")); - QString Ssid = get_property(pth.path(), "Ssid"); - if (Ssid == ssid) { - QDBusInterface nm2(nm_service, nm_path, nm_iface, bus); - nm2.call("DeactivateConnection", QVariant::fromValue(active_connection_raw)); + if (pth.path() != "" && pth.path() != "/") { + QString Ssid = get_property(pth.path(), "Ssid"); + if (Ssid == ssid) { + QDBusInterface nm2(nm_service, nm_path, nm_iface, bus); + nm2.setTimeout(dbus_timeout); + nm2.call("DeactivateConnection", QVariant::fromValue(active_connection_raw));// TODO change to disconnect + } } } } QVector WifiManager::get_active_connections() { QDBusInterface nm(nm_service, nm_path, props_iface, bus); + nm.setTimeout(dbus_timeout); + QDBusMessage response = nm.call("Get", nm_iface, "ActiveConnections"); const QDBusArgument &arr = get_response(response); QVector conns; @@ -248,27 +280,21 @@ QVector WifiManager::get_active_connections() { } void WifiManager::clear_connections(QString ssid) { - QDBusInterface nm(nm_service, nm_settings_path, nm_settings_iface, bus); - QDBusMessage response = nm.call("ListConnections"); - QVariant first = response.arguments().at(0); - const QDBusArgument &args = first.value(); - args.beginArray(); - while (!args.atEnd()) { - QDBusObjectPath path; - args >> path; + for(QDBusObjectPath path : list_connections()){ QDBusInterface nm2(nm_service, path.path(), nm_settings_conn_iface, bus); + nm2.setTimeout(dbus_timeout); + QDBusMessage response = nm2.call("GetSettings"); const QDBusArgument &dbusArg = response.arguments().at(0).value(); - QMap > map; + QMap> map; dbusArg >> map; - for (QString outer_key : map.keys()) { - QMap innerMap = map.value(outer_key); - for (QString inner_key : innerMap.keys()) { - if (inner_key == "ssid") { - QString value = innerMap.value(inner_key).value(); - if (value == ssid) { + for (auto &inner : map) { + for (auto &val : inner) { + QString key = inner.key(val); + if (key == "ssid") { + if (val == ssid) { nm2.call("Delete"); } } @@ -279,11 +305,14 @@ void WifiManager::clear_connections(QString ssid) { void WifiManager::request_scan() { QDBusInterface nm(nm_service, adapter, wireless_device_iface, bus); + nm.setTimeout(dbus_timeout); nm.call("RequestScan", QVariantMap()); } uint WifiManager::get_wifi_device_state() { QDBusInterface device_props(nm_service, adapter, props_iface, bus); + device_props.setTimeout(dbus_timeout); + QDBusMessage response = device_props.call("Get", device_iface, "State"); uint resp = get_response(response); return resp; @@ -291,6 +320,8 @@ uint WifiManager::get_wifi_device_state() { QString WifiManager::get_active_ap() { QDBusInterface device_props(nm_service, adapter, props_iface, bus); + device_props.setTimeout(dbus_timeout); + QDBusMessage response = device_props.call("Get", wireless_device_iface, "ActiveAccessPoint"); QDBusObjectPath r = get_response(response); return r.path(); @@ -298,19 +329,24 @@ QString WifiManager::get_active_ap() { QByteArray WifiManager::get_property(QString network_path ,QString property) { QDBusInterface device_props(nm_service, network_path, props_iface, bus); + device_props.setTimeout(dbus_timeout); + QDBusMessage response = device_props.call("Get", ap_iface, property); return get_response(response); } unsigned int WifiManager::get_ap_strength(QString network_path) { QDBusInterface device_props(nm_service, network_path, props_iface, bus); + device_props.setTimeout(dbus_timeout); + QDBusMessage response = device_props.call("Get", ap_iface, "Strength"); return get_response(response); } QString WifiManager::get_adapter() { - QDBusInterface nm(nm_service, nm_path, nm_iface, bus); + nm.setTimeout(dbus_timeout); + QDBusMessage response = nm.call("GetDevices"); QVariant first = response.arguments().at(0); @@ -324,6 +360,8 @@ QString WifiManager::get_adapter() { // Get device type QDBusInterface device_props(nm_service, path.path(), props_iface, bus); + device_props.setTimeout(dbus_timeout); + QDBusMessage response = device_props.call("Get", device_iface, "DeviceType"); uint device_type = get_response(response); @@ -337,31 +375,106 @@ QString WifiManager::get_adapter() { return adapter_path; } -void WifiManager::change(unsigned int new_state,unsigned int previous_state,unsigned int change_reason) { +void WifiManager::change(unsigned int new_state, unsigned int previous_state, unsigned int change_reason) { raw_adapter_state = new_state; if (new_state == state_need_auth && change_reason == reason_wrong_password) { emit wrongPassword(connecting_to_network); } else if (new_state == state_connected) { + emit successfulConnection(connecting_to_network); connecting_to_network = ""; } } void WifiManager::disconnect() { QString active_ap = get_active_ap(); - if (active_ap!="" && active_ap!="/") { + if (active_ap != "" && active_ap != "/") { deactivate_connections(get_property(active_ap, "Ssid")); } } -//Functions for tethering +QVector WifiManager::list_connections(){ + qDebug() << "list connections"; + QVector connections; + QDBusInterface nm(nm_service, nm_settings_path, nm_settings_iface, bus); + qDebug() << "here"; + nm.setTimeout(dbus_timeout); -void WifiManager::enableTethering() { - disconnect(); + QDBusMessage response = nm.call("ListConnections"); + QVariant first = response.arguments().at(0); + const QDBusArgument &args = first.value(); + args.beginArray(); + while (!args.atEnd()) { + QDBusObjectPath path; + args >> path; + connections.push_back(path); + } + return connections; +} + +bool WifiManager::activate_wifi_connection(QString ssid){ + QString devicePath = get_adapter(); + + for(QDBusObjectPath path : list_connections()){ + QDBusInterface nm2(nm_service, path.path(), nm_settings_conn_iface, bus); + nm2.setTimeout(dbus_timeout); + + QDBusMessage response = nm2.call("GetSettings"); + const QDBusArgument &dbusArg = response.arguments().at(0).value(); + + QMap> map; + dbusArg >> map; + for (auto &inner : map) { + for (auto &val : inner) { + QString key = inner.key(val); + if (key == "ssid") { + if (val == ssid) { + QDBusInterface nm3(nm_service, nm_path, nm_iface, bus); + nm3.setTimeout(dbus_timeout); + nm3.call("ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(devicePath)), QVariant::fromValue(QDBusObjectPath("/"))); + return true; + } + } + } + } + } + return false; +} +//Functions for tethering +bool WifiManager::activate_tethering_connection(){ + QString devicePath = get_adapter(); + + for(QDBusObjectPath path : list_connections()){ + QDBusInterface nm2(nm_service, path.path(), nm_settings_conn_iface, bus); + nm2.setTimeout(dbus_timeout); + + QDBusMessage response = nm2.call("GetSettings"); + const QDBusArgument &dbusArg = response.arguments().at(0).value(); + + QMap> map; + dbusArg >> map; + for (auto &inner : map) { + for (auto &val : inner) { + QString key = inner.key(val); + if (key == "ssid") { + if (val == tethering_ssid.toUtf8()) { + QDBusInterface nm3(nm_service, nm_path, nm_iface, bus); + nm3.setTimeout(dbus_timeout); + nm3.call("ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(devicePath)), QVariant::fromValue(QDBusObjectPath("/"))); + return true; + } + } + } + } + } + return false; +} +void WifiManager::addTetheringConnection(){ Connection connection; connection["connection"]["id"] = "Hotspot"; connection["connection"]["uuid"] = QUuid::createUuid().toString().remove('{').remove('}'); connection["connection"]["type"] = "802-11-wireless"; connection["connection"]["interface-name"] = "wlan0"; + connection["connection"]["autoconnect"] = false; connection["802-11-wireless"]["band"] = "bg"; connection["802-11-wireless"]["mode"] = "ap"; @@ -371,7 +484,7 @@ void WifiManager::enableTethering() { connection["802-11-wireless-security"]["key-mgmt"] = "wpa-psk"; connection["802-11-wireless-security"]["pairwise"] = QStringList("ccmp"); connection["802-11-wireless-security"]["proto"] = QStringList("rsn"); - connection["802-11-wireless-security"]["psk"] = "swagswagcomma"; + connection["802-11-wireless-security"]["psk"] = tetheringPassword; connection["ipv4"]["method"] = "shared"; QMap address; @@ -382,15 +495,29 @@ void WifiManager::enableTethering() { connection["ipv6"]["method"] = "ignore"; QDBusInterface nm_settings(nm_service, nm_settings_path, nm_settings_iface, bus); + nm_settings.setTimeout(dbus_timeout); nm_settings.call("AddConnection", QVariant::fromValue(connection)); +} +void WifiManager::enableTethering() { + if(activate_tethering_connection()){ + return; + } + addTetheringConnection(); + activate_tethering_connection(); } void WifiManager::disableTethering() { - clear_connections(tethering_ssid); + deactivate_connections(tethering_ssid.toUtf8()); } bool WifiManager::tetheringEnabled() { QString active_ap = get_active_ap(); return get_property(active_ap, "Ssid") == tethering_ssid; } + +void WifiManager::changeTetheringPassword(QString newPassword){ + tetheringPassword = newPassword; + clear_connections(tethering_ssid.toUtf8()); + addTetheringConnection(); +} diff --git a/selfdrive/ui/qt/offroad/wifiManager.hpp b/selfdrive/ui/qt/offroad/wifiManager.hpp index 13eb90f3f..bbbe2e3ed 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.hpp +++ b/selfdrive/ui/qt/offroad/wifiManager.hpp @@ -28,7 +28,7 @@ struct Network { class WifiManager : public QWidget { Q_OBJECT public: - explicit WifiManager(); + explicit WifiManager(QWidget* parent); void request_scan(); QVector seen_networks; @@ -38,12 +38,18 @@ public: void connect(Network ssid); void connect(Network ssid, QString password); void connect(Network ssid, QString username, QString password); - // Tethering functions + void disconnect(); + // Tethering functions void enableTethering(); void disableTethering(); bool tetheringEnabled(); + bool activate_tethering_connection(); + void addTetheringConnection(); + bool activate_wifi_connection(QString ssid); + void changeTetheringPassword(QString newPassword); + private: QVector seen_ssids; QString adapter;//Path to network manager wifi-device @@ -51,6 +57,7 @@ private: unsigned int raw_adapter_state;//Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState QString connecting_to_network; QString tethering_ssid; + QString tetheringPassword = "swagswagcommma"; QString get_adapter(); QString get_ipv4_address(); @@ -64,11 +71,12 @@ private: QByteArray get_property(QString network_path, QString property); unsigned int get_ap_strength(QString network_path); SecurityType getSecurityType(QString ssid); - void disconnect(); + QVector list_connections(); private slots: void change(unsigned int new_state, unsigned int previous_state, unsigned int change_reason); signals: void wrongPassword(QString ssid); + void successfulConnection(QString ssid); void refresh(); }; diff --git a/selfdrive/ui/qt/qt_window.cc b/selfdrive/ui/qt/qt_window.cc deleted file mode 100644 index 7f6b383bd..000000000 --- a/selfdrive/ui/qt/qt_window.cc +++ /dev/null @@ -1,26 +0,0 @@ -#include -#include - -#ifdef QCOM2 -#include -#include -#include -#endif - -#include "qt_window.hpp" - -void setMainWindow(QWidget *w) { - float scale = getenv("SCALE") != NULL ? std::stof(getenv("SCALE")) : 1.0; - w->setFixedSize(vwp_w*scale, vwp_h*scale); - w->show(); - -#ifdef QCOM2 - QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); - wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w->windowHandle())); - wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); - wl_surface_commit(s); - w->showFullScreen(); -#endif -} - - diff --git a/selfdrive/ui/qt/qt_window.hpp b/selfdrive/ui/qt/qt_window.hpp index a4485959b..f7d36af8d 100644 --- a/selfdrive/ui/qt/qt_window.hpp +++ b/selfdrive/ui/qt/qt_window.hpp @@ -1,4 +1,15 @@ +#include + #include +#include +#include + +#ifdef QCOM2 +#include +#include +#include +#endif + #ifdef QCOM2 const int vwp_w = 2160, vwp_h = 1080; @@ -6,4 +17,16 @@ const int vwp_w = 1920, vwp_h = 1080; #endif -void setMainWindow(QWidget *w); +inline void setMainWindow(QWidget *w) { + const float scale = getenv("SCALE") != NULL ? std::stof(getenv("SCALE")) : 1.0; + w->setFixedSize(vwp_w*scale, vwp_h*scale); + w->show(); + +#ifdef QCOM2 + QPlatformNativeInterface *native = QGuiApplication::platformNativeInterface(); + wl_surface *s = reinterpret_cast(native->nativeResourceForWindow("surface", w->windowHandle())); + wl_surface_set_buffer_transform(s, WL_OUTPUT_TRANSFORM_270); + wl_surface_commit(s); + w->showFullScreen(); +#endif +} diff --git a/selfdrive/ui/qt/spinner.cc b/selfdrive/ui/qt/spinner.cc index 77151f3bb..1a261995c 100644 --- a/selfdrive/ui/qt/spinner.cc +++ b/selfdrive/ui/qt/spinner.cc @@ -3,9 +3,9 @@ #include #include +#include #include #include -#include #include "spinner.hpp" #include "qt_window.hpp" @@ -15,19 +15,26 @@ Spinner::Spinner(QWidget *parent) { main_layout->setSpacing(0); main_layout->setContentsMargins(200, 200, 200, 200); - const int img_size = 360; - comma = new QLabel(); - comma->setPixmap(QPixmap("../assets/img_spinner_comma.png").scaled(img_size, img_size, Qt::KeepAspectRatio, Qt::SmoothTransformation)); - comma->setFixedSize(img_size, img_size); + comma->setPixmap(QPixmap("../assets/img_spinner_comma.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation)); + comma->setFixedSize(spinner_size); main_layout->addWidget(comma, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); - track_img = QPixmap("../assets/img_spinner_track.png").scaled(img_size, img_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); track = new QLabel(); - track->setPixmap(track_img); - track->setFixedSize(img_size, img_size); + track->setFixedSize(spinner_size); main_layout->addWidget(track, 0, 0, Qt::AlignHCenter | Qt::AlignVCenter); + // pre-compute all the track imgs. make this a gif instead? + track_idx = 0; + QTransform transform; + QPixmap track_img = QPixmap("../assets/img_spinner_track.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation); + for (auto &img : track_imgs) { + QPixmap r = track_img.transformed(transform.rotate(360/spinner_fps), Qt::SmoothTransformation); + int x = (r.width() - track->width()) / 2; + int y = (r.height() - track->height()) / 2; + img = r.copy(x, y, track->width(), track->height()); + } + text = new QLabel(); text->setVisible(false); main_layout->addWidget(text, 1, 0, Qt::AlignHCenter); @@ -61,7 +68,7 @@ Spinner::Spinner(QWidget *parent) { )"); rotate_timer = new QTimer(this); - rotate_timer->start(1000/30.); + rotate_timer->start(1000./spinner_fps); QObject::connect(rotate_timer, SIGNAL(timeout()), this, SLOT(rotate())); notifier = new QSocketNotifier(fileno(stdin), QSocketNotifier::Read); @@ -69,12 +76,8 @@ Spinner::Spinner(QWidget *parent) { }; void Spinner::rotate() { - transform.rotate(5); - - QPixmap r = track_img.transformed(transform.rotate(5), Qt::SmoothTransformation); - int x = (r.width() - track->width()) / 2; - int y = (r.height() - track->height()) / 2; - track->setPixmap(r.copy(x, y, track->width(), track->height())); + track_idx = (track_idx+1) % track_imgs.size(); + track->setPixmap(track_imgs[track_idx]); }; void Spinner::update(int n) { diff --git a/selfdrive/ui/qt/spinner.hpp b/selfdrive/ui/qt/spinner.hpp index ea0f8afdc..8bdd13a3d 100644 --- a/selfdrive/ui/qt/spinner.hpp +++ b/selfdrive/ui/qt/spinner.hpp @@ -1,11 +1,15 @@ +#include + #include #include #include #include #include -#include #include +constexpr int spinner_fps = 30; +constexpr QSize spinner_size = QSize(360, 360); + class Spinner : public QWidget { Q_OBJECT @@ -13,12 +17,13 @@ public: explicit Spinner(QWidget *parent = 0); private: - QPixmap track_img; - QTimer *rotate_timer; + int track_idx; QLabel *comma, *track; QLabel *text; QProgressBar *progress_bar; - QTransform transform; + std::array track_imgs; + + QTimer *rotate_timer; QSocketNotifier *notifier; public slots: diff --git a/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc new file mode 100644 index 000000000..cee011995 --- /dev/null +++ b/selfdrive/ui/qt/widgets/drive_stats.cc @@ -0,0 +1,117 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "api.hpp" +#include "common/params.h" +#include "common/util.h" +#include "drive_stats.hpp" +#include "home.hpp" + +const double MILE_TO_KM = 1.60934; + +#if defined(QCOM) || defined(QCOM2) +const std::string private_key_path = "/persist/comma/id_rsa"; +#else +const std::string private_key_path = util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa"); +#endif + +void clearLayouts(QLayout* layout) { + while (QLayoutItem* item = layout->takeAt(0)) { + if (QWidget* widget = item->widget()) { + widget->deleteLater(); + } + if (QLayout* childLayout = item->layout()) { + clearLayouts(childLayout); + } + delete item; + } +} + +QLayout* build_stat(QString name, int stat) { + QVBoxLayout* layout = new QVBoxLayout; + + QLabel* metric = new QLabel(QString("%1").arg(stat)); + metric->setStyleSheet(R"( + font-size: 80px; + font-weight: 600; + )"); + layout->addWidget(metric, 0, Qt::AlignLeft); + + QLabel* label = new QLabel(name); + label->setStyleSheet(R"( + font-size: 45px; + font-weight: 500; + )"); + layout->addWidget(label, 0, Qt::AlignLeft); + + return layout; +} + +void DriveStats::parseError(QString response) { + clearLayouts(vlayout); + vlayout->addWidget(new QLabel("No internet connection")); +} + +void DriveStats::parseResponse(QString response) { + response.chop(1); + clearLayouts(vlayout); + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting past drives statistics"; + return; + } + + QString IsMetric = QString::fromStdString(Params().get("IsMetric")); + bool metric = (IsMetric == "1"); + + QJsonObject json = doc.object(); + auto all = json["all"].toObject(); + auto week = json["week"].toObject(); + + QGridLayout* gl = new QGridLayout(); + + int all_distance = all["distance"].toDouble() * (metric ? MILE_TO_KM : 1); + gl->addWidget(new QLabel("ALL TIME"), 0, 0, 1, 3); + gl->addLayout(build_stat("DRIVES", all["routes"].toDouble()), 1, 0, 3, 1); + gl->addLayout(build_stat(metric ? "KM" : "MILES", all_distance), 1, 1, 3, 1); + gl->addLayout(build_stat("HOURS", all["minutes"].toDouble() / 60), 1, 2, 3, 1); + + int week_distance = week["distance"].toDouble() * (metric ? MILE_TO_KM : 1); + gl->addWidget(new QLabel("PAST WEEK"), 6, 0, 1, 3); + gl->addLayout(build_stat("DRIVES", week["routes"].toDouble()), 7, 0, 3, 1); + gl->addLayout(build_stat(metric ? "KM" : "MILES", week_distance), 7, 1, 3, 1); + gl->addLayout(build_stat("HOURS", week["minutes"].toDouble() / 60), 7, 2, 3, 1); + + QWidget* q = new QWidget; + q->setLayout(gl); + + vlayout->addWidget(q); +} + +DriveStats::DriveStats(QWidget* parent) : QWidget(parent) { + vlayout = new QVBoxLayout(this); + setLayout(vlayout); + setStyleSheet(R"( + QLabel { + font-size: 48px; + font-weight: 500; + } + )"); + + QString dongleId = QString::fromStdString(Params().get("DongleId")); + QString url = "https://api.commadotai.com/v1.1/devices/" + dongleId + "/stats"; + RequestRepeater* repeater = new RequestRepeater(this, url, 13); + QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(parseResponse(QString))); + QObject::connect(repeater, SIGNAL(failedResponse(QString)), this, SLOT(parseError(QString))); + +} diff --git a/selfdrive/ui/qt/widgets/drive_stats.hpp b/selfdrive/ui/qt/widgets/drive_stats.hpp new file mode 100644 index 000000000..bf4cdae00 --- /dev/null +++ b/selfdrive/ui/qt/widgets/drive_stats.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include +#include +#include + +#include "api.hpp" + +class DriveStats : public QWidget { + Q_OBJECT + +public: + explicit DriveStats(QWidget* parent = 0); + +private: + QVBoxLayout* vlayout; + +private slots: + void parseError(QString response); + void parseResponse(QString response); +}; diff --git a/selfdrive/ui/qt/widgets/input_field.cc b/selfdrive/ui/qt/widgets/input_field.cc index 858c1c75a..f1f48a466 100644 --- a/selfdrive/ui/qt/widgets/input_field.cc +++ b/selfdrive/ui/qt/widgets/input_field.cc @@ -1,56 +1,86 @@ #include #include "input_field.hpp" +#include "qt_window.hpp" -InputField::InputField(QWidget *parent): QWidget(parent) { - layout = new QGridLayout(); - layout->setSpacing(30); +InputDialog::InputDialog(QString prompt_text, QWidget *parent): QDialog(parent) { + layout = new QVBoxLayout(); + layout->setContentsMargins(50, 50, 50, 50); + layout->setSpacing(20); - label = new QLabel(this); - label->setStyleSheet(R"(font-size: 55px;)"); - layout->addWidget(label, 0, 0, Qt::AlignVCenter | Qt::AlignLeft); - layout->setColumnStretch(0, 1); + // build header + QHBoxLayout *header_layout = new QHBoxLayout(); - QPushButton* cancel = new QPushButton("Cancel"); - cancel->setFixedSize(300, 150); - cancel->setStyleSheet(R"(padding: 0;)"); - layout->addWidget(cancel, 0, 1, Qt::AlignVCenter | Qt::AlignRight); - QObject::connect(cancel, SIGNAL(released()), this, SLOT(emitEmpty())); + label = new QLabel(prompt_text, this); + label->setStyleSheet(R"(font-size: 75px; font-weight: 500;)"); + header_layout->addWidget(label, 1, Qt::AlignLeft); + + QPushButton* cancel_btn = new QPushButton("Cancel"); + cancel_btn->setStyleSheet(R"( + padding: 30px; + padding-right: 45px; + padding-left: 45px; + border-radius: 7px; + font-size: 45px; + background-color: #444444; + )"); + header_layout->addWidget(cancel_btn, 0, Qt::AlignRight); + QObject::connect(cancel_btn, SIGNAL(released()), this, SLOT(reject())); + + layout->addLayout(header_layout); // text box + layout->addSpacing(20); line = new QLineEdit(); line->setStyleSheet(R"( - color: black; - background-color: white; - font-size: 45px; - padding: 25px; + border: none; + background-color: #444444; + font-size: 80px; + font-weight: 500; + padding: 10px; )"); - layout->addWidget(line, 1, 0, 1, -1); + layout->addWidget(line, 1, Qt::AlignTop); k = new Keyboard(this); - QObject::connect(k, SIGNAL(emitButton(QString)), this, SLOT(getText(QString))); - layout->addWidget(k, 2, 0, 1, -1); + QObject::connect(k, SIGNAL(emitButton(QString)), this, SLOT(handleInput(QString))); + layout->addWidget(k, 2, Qt::AlignBottom); + + setStyleSheet(R"( + * { + color: white; + background-color: black; + } + )"); setLayout(layout); } -void InputField::setPromptText(QString text) { - label->setText(text); +QString InputDialog::getText(const QString prompt) { + InputDialog d = InputDialog(prompt); + const int ret = d.exec(); + if (ret) { + return d.text(); + } else { + return QString(); + } } -void InputField::emitEmpty() { - emitText(""); - line->setText(""); +QString InputDialog::text() { + return line->text(); } -void InputField::getText(QString s) { +int InputDialog::exec() { + setMainWindow(this); + return QDialog::exec(); +} + +void InputDialog::handleInput(QString s) { if (!QString::compare(s,"⌫")) { line->backspace(); } if (!QString::compare(s,"⏎")) { - emitText(line->text()); - line->setText(""); + done(QDialog::Accepted); } QVector control_buttons {"⇧", "↑", "ABC", "⏎", "#+=", "⌫", "123"}; diff --git a/selfdrive/ui/qt/widgets/input_field.hpp b/selfdrive/ui/qt/widgets/input_field.hpp index 8dad0c0dd..5543eff39 100644 --- a/selfdrive/ui/qt/widgets/input_field.hpp +++ b/selfdrive/ui/qt/widgets/input_field.hpp @@ -3,28 +3,29 @@ #include #include #include +#include #include -#include +#include #include "keyboard.hpp" -class InputField : public QWidget { +class InputDialog : public QDialog { Q_OBJECT public: - explicit InputField(QWidget* parent = 0); - void setPromptText(QString text); + explicit InputDialog(QString prompt_text, QWidget* parent = 0); + static QString getText(QString prompt); + QString text(); private: QLineEdit *line; Keyboard *k; QLabel *label; - QGridLayout *layout; + QVBoxLayout *layout; public slots: - void emitEmpty(); - void getText(QString s); + int exec() override; -signals: - void emitText(QString s); +private slots: + void handleInput(QString s); }; diff --git a/selfdrive/ui/qt/widgets/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc index bf192aa5d..8a8006ed9 100644 --- a/selfdrive/ui/qt/widgets/keyboard.cc +++ b/selfdrive/ui/qt/widgets/keyboard.cc @@ -7,49 +7,58 @@ #include "keyboard.hpp" -const int DEFAULT_WIDTH = 1; -const int SPACEBAR_WIDTH = 3; +const int DEFAULT_STRETCH = 1; +const int SPACEBAR_STRETCH = 3; KeyboardLayout::KeyboardLayout(QWidget *parent, std::vector> layout) : QWidget(parent) { QVBoxLayout* vlayout = new QVBoxLayout; - QButtonGroup* btn_group = new QButtonGroup(this); + vlayout->setMargin(0); + vlayout->setSpacing(15); + QButtonGroup* btn_group = new QButtonGroup(this); QObject::connect(btn_group, SIGNAL(buttonClicked(QAbstractButton*)), parent, SLOT(handleButton(QAbstractButton*))); - int i = 0; - for (auto s : layout) { + for (const auto &s : layout) { QHBoxLayout *hlayout = new QHBoxLayout; + hlayout->setSpacing(30); - if (i == 1) { + if (vlayout->count() == 1) { hlayout->addSpacing(90); } - for (QString p : s) { + for (const QString &p : s) { QPushButton* btn = new QPushButton(p); btn->setFixedHeight(120); btn_group->addButton(btn); - hlayout->addSpacing(10); - if (p == QString(" ")) { - hlayout->addWidget(btn, SPACEBAR_WIDTH); - } else { - hlayout->addWidget(btn, DEFAULT_WIDTH); - } - + hlayout->addWidget(btn, p == QString(" ") ? SPACEBAR_STRETCH : DEFAULT_STRETCH); } - if (i == 1) { + if (vlayout->count() == 1) { hlayout->addSpacing(90); } vlayout->addLayout(hlayout); - i++; } + setStyleSheet(R"( + QPushButton { + font-size: 65px; + margin: 0px; + padding: 0px; + border-radius: 30px; + color: #dddddd; + background-color: #444444; + } + QPushButton:pressed { + background-color: #000000; + } + )"); setLayout(vlayout); } -Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { +Keyboard::Keyboard(QWidget *parent) : QFrame(parent) { main_layout = new QStackedLayout; + main_layout->setMargin(0); // lowercase std::vector> lowercase = { @@ -69,7 +78,7 @@ Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { }; main_layout->addWidget(new KeyboardLayout(this, uppercase)); - // 1234567890 + // numbers + specials std::vector> numbers = { {"1","2","3","4","5","6","7","8","9","0"}, {"-","/",":",";","(",")","$","&&","@","\""}, @@ -78,10 +87,10 @@ Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { }; main_layout->addWidget(new KeyboardLayout(this, numbers)); - // Special characters + // extra specials std::vector> specials = { {"[","]","{","}","#","%","^","*","+","="}, - {"_","\\","|","~","<",">","€","£","¥"," "}, + {"_","\\","|","~","<",">","€","£","¥","•"}, {"123",".",",","?","!","`","⌫"}, {"ABC"," ","⏎"}, }; @@ -89,19 +98,8 @@ Keyboard::Keyboard(QWidget *parent) : QWidget(parent) { setLayout(main_layout); main_layout->setCurrentIndex(0); - - setStyleSheet(R"( - QPushButton { - padding: 0; - font-size: 50px; - } - * { - background-color: #99777777; - } - )"); } - void Keyboard::handleButton(QAbstractButton* m_button) { QString id = m_button->text(); if (!QString::compare(m_button->text(), "↑") || !QString::compare(m_button->text(), "ABC")) { diff --git a/selfdrive/ui/qt/widgets/keyboard.hpp b/selfdrive/ui/qt/widgets/keyboard.hpp index f21d48700..2204973f7 100644 --- a/selfdrive/ui/qt/widgets/keyboard.hpp +++ b/selfdrive/ui/qt/widgets/keyboard.hpp @@ -2,6 +2,7 @@ #include +#include #include #include #include @@ -14,7 +15,7 @@ public: explicit KeyboardLayout(QWidget *parent, std::vector> layout); }; -class Keyboard : public QWidget { +class Keyboard : public QFrame { Q_OBJECT public: diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.cc b/selfdrive/ui/qt/widgets/offroad_alerts.cc index 08c8bcde6..cab47d463 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.cc +++ b/selfdrive/ui/qt/widgets/offroad_alerts.cc @@ -1,130 +1,118 @@ -#include #include -#include +#include +#include +#include #include #include #include #include "offroad_alerts.hpp" - #include "common/params.h" -void cleanLayout(QLayout* layout) { - while (QLayoutItem* item = layout->takeAt(0)) { - if (QWidget* widget = item->widget()) { - widget->deleteLater(); - } - if (QLayout* childLayout = item->layout()) { - cleanLayout(childLayout); - } - delete item; +void cleanStackedWidget(QStackedWidget* swidget) { + while(swidget->count() > 0) { + QWidget *w = swidget->widget(0); + swidget->removeWidget(w); + w->deleteLater(); } } -QString vectorToQString(std::vector v) { - return QString::fromStdString(std::string(v.begin(), v.end())); -} +OffroadAlert::OffroadAlert(QWidget* parent) : QFrame(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(); + main_layout->setMargin(25); -OffroadAlert::OffroadAlert(QWidget* parent) { - vlayout = new QVBoxLayout; - refresh(); - setLayout(vlayout); + alerts_stack = new QStackedWidget(); + main_layout->addWidget(alerts_stack, 1); + + // bottom footer + QHBoxLayout *footer_layout = new QHBoxLayout(); + main_layout->addLayout(footer_layout); + + QPushButton *dismiss_btn = new QPushButton("Dismiss"); + dismiss_btn->setFixedSize(400, 125); + footer_layout->addWidget(dismiss_btn, 0, Qt::AlignLeft); + + reboot_btn = new QPushButton("Reboot and Update"); + reboot_btn->setFixedSize(600, 125); + reboot_btn->setVisible(false); + footer_layout->addWidget(reboot_btn, 0, Qt::AlignRight); + + QObject::connect(dismiss_btn, SIGNAL(released()), this, SIGNAL(closeAlerts())); +#ifdef __aarch64__ + QObject::connect(reboot_btn, &QPushButton::released, [=]() {std::system("sudo reboot");}); +#endif + + setLayout(main_layout); + setStyleSheet(R"( + * { + color: white; + } + QFrame { + border-radius: 30px; + background-color: #393939; + } + QPushButton { + color: black; + font-size: 50px; + font-weight: 500; + border-radius: 30px; + background-color: white; + } + )"); + main_layout->setMargin(50); } void OffroadAlert::refresh() { - cleanLayout(vlayout); parse_alerts(); + cleanStackedWidget(alerts_stack); - updateAvailable = false; std::vector bytes = Params().read_db_bytes("UpdateAvailable"); - if (bytes.size() && bytes[0] == '1') { - updateAvailable = true; - } + updateAvailable = bytes.size() && bytes[0] == '1'; + + reboot_btn->setVisible(updateAvailable); + + QVBoxLayout *layout = new QVBoxLayout; if (updateAvailable) { - // If there is an update available, don't show alerts - alerts.clear(); - - QFrame *f = new QFrame(); - - QVBoxLayout *update_layout = new QVBoxLayout; - update_layout->setMargin(10); - update_layout->setSpacing(20); - - QLabel *title = new QLabel("Update available"); + QLabel *title = new QLabel("Update Available"); title->setStyleSheet(R"( - font-size: 55px; - font-weight: bold; + font-size: 72px; )"); - update_layout->addWidget(title, 0, Qt::AlignTop); + layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); QString release_notes = QString::fromStdString(Params().get("ReleaseNotes")); - QLabel *notes_label = new QLabel(release_notes); - notes_label->setStyleSheet(R"(font-size: 40px;)"); - notes_label->setWordWrap(true); - update_layout->addWidget(notes_label, 1, Qt::AlignTop); - - QPushButton *update_button = new QPushButton("Reboot and Update"); - update_layout->addWidget(update_button); -#ifdef __aarch64__ - QObject::connect(update_button, &QPushButton::released, [=]() {std::system("sudo reboot");}); -#endif - - f->setLayout(update_layout); - f->setStyleSheet(R"( - .QFrame{ - border-radius: 20px; - border: 2px solid white; - background-color: #114267; - } - QPushButton { - padding: 20px; - font-size: 35px; - color: white; - background-color: blue; - } + QLabel *body = new QLabel(release_notes); + body->setStyleSheet(R"( + font-size: 48px; )"); - - vlayout->addWidget(f); - vlayout->addSpacing(60); + layout->addWidget(body, 1, Qt::AlignLeft | Qt::AlignTop); } else { - vlayout->addSpacing(60); - - for (auto alert : alerts) { + // TODO: paginate the alerts + for (const auto &alert : alerts) { QLabel *l = new QLabel(alert.text); l->setWordWrap(true); l->setMargin(60); QString style = R"( - font-size: 40px; - font-weight: bold; - border-radius: 30px; - border: 2px solid; - border-color: white; + font-size: 48px; )"; - style.append("background-color: " + QString(alert.severity ? "#971b1c" : "#114267")); - + style.append("background-color: " + QString(alert.severity ? "#E22C2C" : "#292929")); l->setStyleSheet(style); - vlayout->addWidget(l); - vlayout->addSpacing(20); - } - } - QPushButton *hide_btn = new QPushButton(updateAvailable ? "Later" : "Hide alerts"); - hide_btn->setStyleSheet(R"( - padding: 20px; - font-size: 35px; - color: white; - background-color: blue; - )"); - vlayout->addWidget(hide_btn); - QObject::connect(hide_btn, SIGNAL(released()), this, SIGNAL(closeAlerts())); + layout->addWidget(l, 0, Qt::AlignTop); + } + layout->setSpacing(20); + } + QWidget *w = new QWidget(); + w->setLayout(layout); + alerts_stack->addWidget(w); } void OffroadAlert::parse_alerts() { alerts.clear(); - // We launch in selfdrive/ui + + // TODO: only read this once QFile inFile("../controls/lib/alerts_offroad.json"); inFile.open(QIODevice::ReadOnly | QIODevice::Text); QByteArray data = inFile.readAll(); @@ -136,7 +124,7 @@ void OffroadAlert::parse_alerts() { } QJsonObject json = doc.object(); - for (const QString& key : json.keys()) { + for (const QString &key : json.keys()) { std::vector bytes = Params().read_db_bytes(key.toStdString().c_str()); if (bytes.size()) { diff --git a/selfdrive/ui/qt/widgets/offroad_alerts.hpp b/selfdrive/ui/qt/widgets/offroad_alerts.hpp index 1e425158f..edd37dd89 100644 --- a/selfdrive/ui/qt/widgets/offroad_alerts.hpp +++ b/selfdrive/ui/qt/widgets/offroad_alerts.hpp @@ -1,14 +1,15 @@ #pragma once -#include -#include +#include +#include +#include struct Alert { QString text; int severity; }; -class OffroadAlert : public QWidget { +class OffroadAlert : public QFrame { Q_OBJECT public: @@ -17,8 +18,8 @@ public: bool updateAvailable; private: - QVBoxLayout *vlayout; - + QStackedWidget *alerts_stack; + QPushButton *reboot_btn; void parse_alerts(); signals: diff --git a/selfdrive/ui/qt/widgets/setup.cc b/selfdrive/ui/qt/widgets/setup.cc new file mode 100644 index 000000000..193e22519 --- /dev/null +++ b/selfdrive/ui/qt/widgets/setup.cc @@ -0,0 +1,276 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "QrCode.hpp" +#include "api.hpp" +#include "common/params.h" +#include "common/util.h" +#include "home.hpp" +#include "setup.hpp" + +using qrcodegen::QrCode; + +#if defined(QCOM) || defined(QCOM2) +const std::string private_key_path = "/persist/comma/id_rsa"; +#else +const std::string private_key_path = util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa"); +#endif + +PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) { + qrCode = new QLabel; + qrCode->setScaledContents(true); + QVBoxLayout* v = new QVBoxLayout; + v->addWidget(qrCode, 0, Qt::AlignCenter); + setLayout(v); + + QTimer* timer = new QTimer(this); + timer->start(30 * 1000);// HaLf a minute + connect(timer, SIGNAL(timeout()), this, SLOT(refresh())); + refresh(); // Not waiting for the first refresh +} + +void PairingQRWidget::refresh(){ + QString IMEI = QString::fromStdString(Params().get("IMEI")); + QString serial = QString::fromStdString(Params().get("HardwareSerial")); + + if (std::min(IMEI.length(), serial.length()) <= 5) { + qrCode->setText("Error getting serial: contact support"); + qrCode->setWordWrap(true); + qrCode->setStyleSheet(R"( + font-size: 60px; + )"); + return; + } + QVector> payloads; + payloads.push_back(qMakePair(QString("pair"), true)); + QString pairToken = CommaApi::create_jwt(payloads); + + QString qrString = IMEI + "--" + serial + "--" + pairToken; + this->updateQrCode(qrString); +} + +void PairingQRWidget::updateQrCode(QString text) { + QrCode qr = QrCode::encodeText(text.toUtf8().data(), QrCode::Ecc::LOW); + qint32 sz = qr.getSize(); + // We make the image larger so we can have a white border + QImage im(sz + 2, sz + 2, QImage::Format_RGB32); + QRgb black = qRgb(0, 0, 0); + QRgb white = qRgb(255, 255, 255); + + for (int y = 0; y < sz + 2; y++) { + for (int x = 0; x < sz + 2; x++) { + im.setPixel(x, y, white); + } + } + for (int y = 0; y < sz; y++) { + for (int x = 0; x < sz; x++) { + im.setPixel(x + 1, y + 1, qr.getModule(x, y) ? black : white); + } + } + // Integer division to prevent anti-aliasing + int approx500 = (500 / (sz + 2)) * (sz + 2); + qrCode->setPixmap(QPixmap::fromImage(im.scaled(approx500, approx500, Qt::KeepAspectRatio, Qt::FastTransformation), Qt::MonoOnly)); + qrCode->setFixedSize(approx500, approx500); +} + +PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) { + mainLayout = new QVBoxLayout; + QLabel* commaPrime = new QLabel("COMMA PRIME"); + commaPrime->setStyleSheet(R"( + font-size: 60px; + )"); + mainLayout->addWidget(commaPrime); + + username = new QLabel(""); + mainLayout->addWidget(username); + + mainLayout->addSpacing(200); + + QLabel* commaPoints = new QLabel("COMMA POINTS"); + commaPoints->setStyleSheet(R"( + font-size: 60px; + color: #b8b8b8; + )"); + mainLayout->addWidget(commaPoints); + + points = new QLabel(""); + mainLayout->addWidget(points); + + setLayout(mainLayout); + QString dongleId = QString::fromStdString(Params().get("DongleId")); + QString url = "https://api.commadotai.com/v1/devices/" + dongleId + "/owner"; + RequestRepeater* repeater = new RequestRepeater(this, url, 6); + + QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(replyFinished(QString))); +} + +void PrimeUserWidget::replyFinished(QString response) { + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting username and points"; + return; + } + QJsonObject json = doc.object(); + QString username_str = json["username"].toString(); + if (username_str.length()) { + username_str = "@" + username_str; + } + QString points_str = QString::number(json["points"].toInt()); + + username->setText(username_str); + points->setText(points_str); +} + +PrimeAdWidget::PrimeAdWidget(QWidget* parent) : QWidget(parent) { + QVBoxLayout* vlayout = new QVBoxLayout; + + QLabel* upgradeNow = new QLabel("Upgrade now"); + vlayout->addWidget(upgradeNow); + + QLabel* description = new QLabel("Become a comma prime member in the comma app and get premium features!"); + description->setStyleSheet(R"( + font-size: 50px; + color: #b8b8b8; + )"); + description->setWordWrap(true); + vlayout->addWidget(description); + + vlayout->addSpacing(50); + + QVector features = {"✓ REMOTE ACCESS", "✓ 14 DAYS OF STORAGE", "✓ DEVELOPER PERKS"}; + for (auto featureContent : features) { + QLabel* feature = new QLabel(featureContent); + feature->setStyleSheet(R"( + font-size: 40px; + )"); + + vlayout->addWidget(feature); + vlayout->addSpacing(15); + } + + setLayout(vlayout); +} + + + +SetupWidget::SetupWidget(QWidget* parent) : QWidget(parent) { + QVBoxLayout* backgroundLayout = new QVBoxLayout; + + backgroundLayout->addSpacing(100); + + QFrame* background = new QFrame; + + mainLayout = new QStackedLayout; + + QWidget* blankWidget = new QWidget; + mainLayout->addWidget(blankWidget); + + QWidget* finishRegistration = new QWidget; + + QVBoxLayout* finishRegistationLayout = new QVBoxLayout; + finishRegistationLayout->addSpacing(30); + QPushButton* finishButton = new QPushButton("Finish registration"); + finishButton->setFixedHeight(200); + finishButton->setStyleSheet(R"( + border-radius: 30px; + font-size: 55px; + background: #585858; + )"); + QObject::connect(finishButton, SIGNAL(released()), this, SLOT(showQrCode())); + finishRegistationLayout->addWidget(finishButton); + + QLabel* registrationDescription = new QLabel("Pair your Comma device with the Comma Connect app"); + registrationDescription->setStyleSheet(R"( + font-size: 55px; + font-weight: 400; + )"); + + registrationDescription->setWordWrap(true); + finishRegistationLayout->addWidget(registrationDescription); + + finishRegistration->setLayout(finishRegistationLayout); + mainLayout->addWidget(finishRegistration); + + QVBoxLayout* qrLayout = new QVBoxLayout; + + QLabel* qrLabel = new QLabel("Pair with Comma Connect app!"); + qrLabel->setStyleSheet(R"( + font-size: 40px; + )"); + qrLayout->addWidget(qrLabel); + + qrLayout->addWidget(new PairingQRWidget); + + QWidget* q = new QWidget; + q->setLayout(qrLayout); + mainLayout->addWidget(q); + + PrimeAdWidget* primeAd = new PrimeAdWidget; + mainLayout->addWidget(primeAd); + + PrimeUserWidget* primeUserWidget = new PrimeUserWidget; + mainLayout->addWidget(primeUserWidget); + + background->setLayout(mainLayout); + background->setStyleSheet(R"( + .QFrame { + border-radius: 40px; + padding: 40px; + } + )"); + backgroundLayout->addWidget(background); + setLayout(backgroundLayout); + + QString dongleId = QString::fromStdString(Params().get("DongleId")); + QString url = "https://api.commadotai.com/v1.1/devices/" + dongleId + "/"; + RequestRepeater* repeater = new RequestRepeater(this, url, 5); + + QObject::connect(repeater, SIGNAL(receivedResponse(QString)), this, SLOT(replyFinished(QString))); + QObject::connect(repeater, SIGNAL(failedResponse(QString)), this, SLOT(parseError(QString))); + +} + +void SetupWidget::parseError(QString response) { + showQr = false; + mainLayout->setCurrentIndex(0); + setStyleSheet(R"( + font-size: 90px; + background-color: #000000; + )"); +} +void SetupWidget::showQrCode(){ + showQr = true; + mainLayout->setCurrentIndex(2); +} +void SetupWidget::replyFinished(QString response) { + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); + if (doc.isNull()) { + qDebug() << "JSON Parse failed on getting pairing and prime status"; + return; + } + if (mainLayout->currentIndex() == 0) { // If we are still on the blank widget + setStyleSheet(R"( + font-size: 90px; + font-weight: bold; + background-color: #292929; + )"); + } + QJsonObject json = doc.object(); + bool is_paired = json["is_paired"].toBool(); + bool is_prime = json["prime"].toBool(); + + if (!is_paired) { + mainLayout->setCurrentIndex(1 + showQr); + } else if (is_paired && !is_prime) { + showQr = false; + mainLayout->setCurrentIndex(3); + } else if (is_paired && is_prime) { + showQr = false; + mainLayout->setCurrentIndex(4); + } +} diff --git a/selfdrive/ui/qt/widgets/setup.hpp b/selfdrive/ui/qt/widgets/setup.hpp new file mode 100644 index 000000000..0224ace56 --- /dev/null +++ b/selfdrive/ui/qt/widgets/setup.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include + +#include "api.hpp" + +class PairingQRWidget : public QWidget { + Q_OBJECT + +public: + explicit PairingQRWidget(QWidget* parent = 0); + +private: + QLabel* qrCode; + void updateQrCode(QString text); + +private slots: + void refresh(); +}; + +class PrimeUserWidget : public QWidget { + Q_OBJECT +public: + explicit PrimeUserWidget(QWidget* parent = 0); + +private: + QVBoxLayout* mainLayout; + QLabel* username; + QLabel* points; + CommaApi* api; + +private slots: + void replyFinished(QString response); +}; + +class PrimeAdWidget : public QWidget { + Q_OBJECT +public: + explicit PrimeAdWidget(QWidget* parent = 0); +}; + +class SetupWidget : public QWidget { + Q_OBJECT + +public: + explicit SetupWidget(QWidget* parent = 0); + +private: + QStackedLayout* mainLayout; + CommaApi* api; + bool showQr = false; + +private slots: + void parseError(QString response); + void replyFinished(QString response); + void showQrCode(); +}; diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc new file mode 100644 index 000000000..64c1a67b0 --- /dev/null +++ b/selfdrive/ui/qt/widgets/ssh_keys.cc @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include + +#include "common/params.h" +#include "widgets/ssh_keys.hpp" +#include "widgets/input_field.hpp" + +SSH::SSH(QWidget* parent) : QWidget(parent){ + // init variables + manager = new QNetworkAccessManager(this); + networkTimer = new QTimer(this); + networkTimer->setSingleShot(true); + networkTimer->setInterval(5000); + connect(networkTimer, SIGNAL(timeout()), this, SLOT(timeout())); + + // Layout on entering + QVBoxLayout* main_layout = new QVBoxLayout; + main_layout->setMargin(50); + + QPushButton* exitButton = new QPushButton("BACK", this); + exitButton->setFixedSize(500, 100); + main_layout->addWidget(exitButton, 0, Qt::AlignLeft | Qt::AlignTop); + connect(exitButton, SIGNAL(released()), this, SIGNAL(closeSSHSettings())); + + QLabel* wallOfText = new QLabel("Warning: This grants SSH access to all public keys in your GitHub settings. Never enter a GitHub username other than your own."); + wallOfText->setAlignment(Qt::AlignHCenter); + wallOfText->setWordWrap(true); + wallOfText->setStyleSheet(R"(font-size: 60px;)"); + main_layout->addWidget(wallOfText, 0); + + QPushButton* actionButton = new QPushButton; + actionButton->setFixedHeight(100); + main_layout->addWidget(actionButton, 0, Qt::AlignBottom); + + setStyleSheet(R"( + QPushButton { + font-size: 60px; + margin: 0px; + padding: 15px; + border-radius: 25px; + color: #dddddd; + background-color: #444444; + } + )"); + setLayout(main_layout); + + // Initialize the state machine and states + QStateMachine* state = new QStateMachine(this); + QState* initialState = new QState(); //State when entering the widget + QState* initialStateNoGithub = new QState(); //Starting state, key not connected + QState* initialStateConnected = new QState(); //Starting state, ssh connected + QState* removeSSH_State = new QState(); // State when user wants to remove the SSH keys + QState* loadingState = new QState(); // State while waiting for the network response + + // Adding states to the state machine and adding the transitions + state->addState(initialState); + connect(initialState, &QState::entered, [=](){ + checkForSSHKey(); + }); + initialState->addTransition(this, &SSH::NoSSHAdded, initialStateNoGithub); + initialState->addTransition(this, &SSH::SSHAdded, initialStateConnected); + + state->addState(initialStateConnected); + connect(initialStateConnected, &QState::entered, [=](){ + actionButton->setText("Clear SSH keys"); + actionButton->setStyleSheet(R"(background-color: #750c0c;)"); + }); + initialStateConnected->addTransition(actionButton, &QPushButton::released, removeSSH_State); + + state->addState(removeSSH_State); + connect(removeSSH_State, &QState::entered, [=](){ + Params().delete_db_value("GithubSshKeys"); + }); + removeSSH_State->addTransition(removeSSH_State, &QState::entered, initialState); + + state->addState(initialStateNoGithub); + connect(initialStateNoGithub, &QState::entered, [=](){ + actionButton->setText("Link GitHub SSH keys"); + actionButton->setStyleSheet(R"(background-color: #444444;)"); + }); + initialStateNoGithub->addTransition(actionButton, &QPushButton::released, loadingState); + + state->addState(loadingState); + connect(loadingState, &QState::entered, [=](){ + QString user = InputDialog::getText("Enter your GitHub username"); + if (user.size()) { + getSSHKeys(user); + } + }); + connect(this, &SSH::failedResponse, [=](QString message){ + QString user = InputDialog::getText(message); + if (user.size()) { + getSSHKeys(user); + } + }); + loadingState->addTransition(loadingState, &QState::entered, initialState); + loadingState->addTransition(this, &SSH::failedResponse, initialState); + loadingState->addTransition(this, &SSH::gotSSHKeys, initialState); + + state->setInitialState(initialState); + state->start(); +} + +void SSH::checkForSSHKey(){ + QString SSHKey = QString::fromStdString(Params().get("GithubSshKeys")); + if (SSHKey.length()) { + emit SSHAdded(); + } else { + emit NoSSHAdded(); + } +} + +void SSH::getSSHKeys(QString username){ + QString url = "https://github.com/" + username + ".keys"; + aborted = false; + reply = manager->get(QNetworkRequest(QUrl(url))); + connect(reply, SIGNAL(finished()), this, SLOT(parseResponse())); + networkTimer->start(); +} + +void SSH::timeout(){ + aborted = true; + reply->abort(); +} + +void SSH::parseResponse(){ + if (!aborted) { + networkTimer->stop(); + QString response = reply->readAll(); + if (reply->error() == QNetworkReply::NoError && response.length()) { + Params().write_db_value("GithubSshKeys", response.toStdString()); + emit gotSSHKeys(); + } else { + emit failedResponse("Username doesn't exist"); + } + } else { + emit failedResponse("Request timed out"); + } + reply->deleteLater(); + reply = NULL; +} diff --git a/selfdrive/ui/qt/widgets/ssh_keys.hpp b/selfdrive/ui/qt/widgets/ssh_keys.hpp new file mode 100644 index 000000000..3082a43d6 --- /dev/null +++ b/selfdrive/ui/qt/widgets/ssh_keys.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include + +class SSH : public QWidget { + Q_OBJECT + +public: + explicit SSH(QWidget* parent = 0); + +private: + QNetworkAccessManager* manager; + QNetworkReply* reply; + QTimer* networkTimer; + bool aborted; + + void getSSHKeys(QString user); + +signals: + void NoSSHAdded(); + void SSHAdded(); + void failedResponse(QString errorString); + void gotSSHKeys(); + void closeSSHSettings(); + +private slots: + void checkForSSHKey(); + void timeout(); + void parseResponse(); +}; diff --git a/selfdrive/ui/qt/widgets/toggle.cc b/selfdrive/ui/qt/widgets/toggle.cc index bf3030915..551630dd7 100644 --- a/selfdrive/ui/qt/widgets/toggle.cc +++ b/selfdrive/ui/qt/widgets/toggle.cc @@ -3,13 +3,16 @@ Toggle::Toggle(QWidget *parent) : QAbstractButton(parent), _height(80), _height_rect(60), -_on(false), +on(false), _anim(new QPropertyAnimation(this, "offset_circle", this)) { _radius = _height / 2; _x_circle = _radius; _y_circle = _radius; _y_rect = (_height - _height_rect)/2; + circleColor = QColor(0xffffff); // placeholder + green = QColor(0xffffff); // placeholder + setEnabled(true); } void Toggle::paintEvent(QPaintEvent *e) { @@ -19,34 +22,60 @@ void Toggle::paintEvent(QPaintEvent *e) { p.setRenderHint(QPainter::Antialiasing, true); // Draw toggle background left - p.setBrush(QColor("#33ab4c")); + p.setBrush(green); p.drawRoundedRect(QRect(0, _y_rect, _x_circle + _radius, _height_rect), _height_rect/2, _height_rect/2); + // Draw toggle background right - p.setBrush(QColor("#0a1a26")); - p.drawRoundedRect(QRect(_x_circle - _radius, _y_rect, width() -(_x_circle - _radius), _height_rect), _height_rect/2, _height_rect/2); + p.setBrush(QColor(0x393939)); + p.drawRoundedRect(QRect(_x_circle - _radius, _y_rect, width() - (_x_circle - _radius), _height_rect), _height_rect/2, _height_rect/2); // Draw toggle circle - p.setBrush(QColor("#fafafa")); + p.setBrush(circleColor); p.drawEllipse(QRectF(_x_circle - _radius, _y_circle - _radius, 2 * _radius, 2 * _radius)); } void Toggle::mouseReleaseEvent(QMouseEvent *e) { + if(!enabled){ + return; + } + const int left = _radius; + const int right = width() - _radius; + if(_x_circle != left && _x_circle != right){ + //Don't parse touch events, while the animation is running + return; + } if (e->button() & Qt::LeftButton) { togglePosition(); - emit stateChanged(_on); + emit stateChanged(on); } } void Toggle::togglePosition() { - _on = !_on; + on = !on; const int left = _radius; const int right = width() - _radius; - _anim->setStartValue(_on ? left : right); - _anim->setEndValue(_on ? right : left); - _anim->setDuration(120); + _anim->setStartValue(on ? left + immediateOffset : right - immediateOffset); + _anim->setEndValue(on ? right : left); + _anim->setDuration(animation_duration); _anim->start(); + repaint(); } void Toggle::enterEvent(QEvent *e) { QAbstractButton::enterEvent(e); } + +bool Toggle::getEnabled(){ + return enabled; +} + +void Toggle::setEnabled(bool value){ + enabled = value; + if(value){ + circleColor.setRgb(0xfafafa); + green.setRgb(0x33ab4c); + }else{ + circleColor.setRgb(0x888888); + green.setRgb(0x227722); + } +} \ No newline at end of file diff --git a/selfdrive/ui/qt/widgets/toggle.hpp b/selfdrive/ui/qt/widgets/toggle.hpp index 98215c25d..4549245dc 100644 --- a/selfdrive/ui/qt/widgets/toggle.hpp +++ b/selfdrive/ui/qt/widgets/toggle.hpp @@ -3,12 +3,14 @@ class Toggle : public QAbstractButton { Q_OBJECT - Q_PROPERTY(int offset_circle READ offset_circle WRITE set_offset_circle) + Q_PROPERTY(int offset_circle READ offset_circle WRITE set_offset_circle CONSTANT) public: Toggle(QWidget* parent = nullptr); void togglePosition(); - + bool on; + int animation_duration = 250; + int immediateOffset = 0; int offset_circle() const { return _x_circle; } @@ -17,6 +19,8 @@ public: _x_circle = o; update(); } + bool getEnabled(); + void setEnabled(bool value); protected: void paintEvent(QPaintEvent*) override; @@ -24,7 +28,9 @@ protected: void enterEvent(QEvent*) override; private: - bool _on; + QColor circleColor; + QColor green; + bool enabled = true; int _x_circle, _y_circle; int _height, _radius; int _height_rect, _y_rect; diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 8893ca367..e26a50023 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -15,6 +15,8 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { main_layout->setMargin(0); setLayout(main_layout); QObject::connect(homeWindow, SIGNAL(openSettings()), this, SLOT(openSettings())); + QObject::connect(homeWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); + QObject::connect(homeWindow, SIGNAL(offroadTransition(bool)), this, SLOT(offroadTransition(bool))); QObject::connect(settingsWindow, SIGNAL(closeSettings()), this, SLOT(closeSettings())); // start at onboarding @@ -24,12 +26,17 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { setStyleSheet(R"( * { - color: white; - background-color: #072339; + font-family: Inter; } )"); } +void MainWindow::offroadTransition(bool offroad){ + if(!offroad){ + closeSettings(); + } +} + void MainWindow::openSettings() { main_layout->setCurrentWidget(settingsWindow); } diff --git a/selfdrive/ui/qt/window.hpp b/selfdrive/ui/qt/window.hpp index 10822f1fb..2c61ac07c 100644 --- a/selfdrive/ui/qt/window.hpp +++ b/selfdrive/ui/qt/window.hpp @@ -23,6 +23,7 @@ private: OnboardingWindow *onboardingWindow; public slots: + void offroadTransition(bool offroad); void openSettings(); void closeSettings(); }; diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 65b20bdac..0d05dade0 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -2,78 +2,68 @@ #include #include #include - +#include "common/util.h" #include "paint.hpp" #include "sidebar.hpp" -static void ui_draw_sidebar_background(UIState *s) { - ui_draw_rect(s->vg, 0, 0, sbr_w, s->fb_h, COLOR_BLACK_ALPHA(85)); +static void draw_background(UIState *s) { +#ifdef QCOM + const NVGcolor color = COLOR_BLACK_ALPHA(85); +#else + const NVGcolor color = nvgRGBA(0x39, 0x39, 0x39, 0xff); +#endif + ui_fill_rect(s->vg, {0, 0, sbr_w, s->fb_h}, color); } -static void ui_draw_sidebar_settings_button(UIState *s) { +static void draw_settings_button(UIState *s) { const float alpha = s->active_app == cereal::UiLayoutState::App::SETTINGS ? 1.0f : 0.65f; - ui_draw_image(s->vg, settings_btn.x, settings_btn.y, settings_btn.w, settings_btn.h, s->img_button_settings, alpha); + ui_draw_image(s, settings_btn, "button_settings", alpha); } -static void ui_draw_sidebar_home_button(UIState *s) { - const float alpha = s->active_app == cereal::UiLayoutState::App::HOME ? 1.0f : 0.65f;; - ui_draw_image(s->vg, home_btn.x, home_btn.y, home_btn.w, home_btn.h, s->img_button_home, alpha); +static void draw_home_button(UIState *s) { + const float alpha = s->active_app == cereal::UiLayoutState::App::HOME ? 1.0f : 0.65f; + ui_draw_image(s, home_btn, "button_home", alpha); } -static void ui_draw_sidebar_network_strength(UIState *s) { - static std::map network_strength_map = { - {cereal::ThermalData::NetworkStrength::UNKNOWN, 1}, - {cereal::ThermalData::NetworkStrength::POOR, 2}, - {cereal::ThermalData::NetworkStrength::MODERATE, 3}, - {cereal::ThermalData::NetworkStrength::GOOD, 4}, - {cereal::ThermalData::NetworkStrength::GREAT, 5}}; - const int network_img_h = 27; - const int network_img_w = 176; - const int network_img_x = 58; - const int network_img_y = 196; - const int img_idx = s->scene.thermal.getNetworkType() == cereal::ThermalData::NetworkType::NONE ? 0 : network_strength_map[s->scene.thermal.getNetworkStrength()]; - ui_draw_image(s->vg, network_img_x, network_img_y, network_img_w, network_img_h, s->img_network[img_idx], 1.0f); +static void draw_network_strength(UIState *s) { + static std::map network_strength_map = { + {cereal::DeviceState::NetworkStrength::UNKNOWN, 1}, + {cereal::DeviceState::NetworkStrength::POOR, 2}, + {cereal::DeviceState::NetworkStrength::MODERATE, 3}, + {cereal::DeviceState::NetworkStrength::GOOD, 4}, + {cereal::DeviceState::NetworkStrength::GREAT, 5}}; + const int img_idx = s->scene.deviceState.getNetworkType() == cereal::DeviceState::NetworkType::NONE ? 0 : network_strength_map[s->scene.deviceState.getNetworkStrength()]; + ui_draw_image(s, {58, 196, 176, 27}, util::string_format("network_%d", img_idx).c_str(), 1.0f); } -static void ui_draw_sidebar_battery_icon(UIState *s) { - const int battery_img_h = 36; - const int battery_img_w = 76; - const int battery_img_x = 160; - const int battery_img_y = 255; - - int battery_img = s->scene.thermal.getBatteryStatus() == "Charging" ? s->img_battery_charging : s->img_battery; - - ui_draw_rect(s->vg, battery_img_x + 6, battery_img_y + 5, - ((battery_img_w - 19) * (s->scene.thermal.getBatteryPercent() * 0.01)), battery_img_h - 11, COLOR_WHITE); - - ui_draw_image(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_img_h, battery_img, 1.0f); +static void draw_battery_icon(UIState *s) { + const char *battery_img = s->scene.deviceState.getBatteryStatus() == "Charging" ? "battery_charging" : "battery"; + const Rect rect = {160, 255, 76, 36}; + ui_fill_rect(s->vg, {rect.x + 6, rect.y + 5, + int((rect.w - 19) * s->scene.deviceState.getBatteryPercent() * 0.01), rect.h - 11}, COLOR_WHITE); + ui_draw_image(s, rect, battery_img, 1.0f); } -static void ui_draw_sidebar_network_type(UIState *s) { - static std::map network_type_map = { - {cereal::ThermalData::NetworkType::NONE, "--"}, - {cereal::ThermalData::NetworkType::WIFI, "WiFi"}, - {cereal::ThermalData::NetworkType::CELL2_G, "2G"}, - {cereal::ThermalData::NetworkType::CELL3_G, "3G"}, - {cereal::ThermalData::NetworkType::CELL4_G, "4G"}, - {cereal::ThermalData::NetworkType::CELL5_G, "5G"}}; +static void draw_network_type(UIState *s) { + static std::map network_type_map = { + {cereal::DeviceState::NetworkType::NONE, "--"}, + {cereal::DeviceState::NetworkType::WIFI, "WiFi"}, + {cereal::DeviceState::NetworkType::CELL2_G, "2G"}, + {cereal::DeviceState::NetworkType::CELL3_G, "3G"}, + {cereal::DeviceState::NetworkType::CELL4_G, "4G"}, + {cereal::DeviceState::NetworkType::CELL5_G, "5G"}}; const int network_x = 50; const int network_y = 273; const int network_w = 100; - const char *network_type = network_type_map[s->scene.thermal.getNetworkType()]; + const char *network_type = network_type_map[s->scene.deviceState.getNetworkType()]; nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFaceId(s->vg, s->font_sans_regular); + nvgFontFace(s->vg, "sans-regular"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); nvgTextBox(s->vg, network_x, network_y, network_w, network_type ? network_type : "--", NULL); } -static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str) { - const int metric_x = 30; - const int metric_y = 338 + y_offset; - const int metric_w = 240; - const int metric_h = message_str ? strchr(message_str, '\n') ? 124 : 100 : 148; - +static void draw_metric(UIState *s, const char *label_str, const char *value_str, const int severity, const int y_offset, const char *message_str) { NVGcolor status_color; if (severity == 0) { @@ -84,86 +74,85 @@ static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char status_color = COLOR_RED; } - ui_draw_rect(s->vg, metric_x, metric_y, metric_w, metric_h, - severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA(85), 20, 2); + const Rect rect = {30, 338 + y_offset, 240, message_str ? strchr(message_str, '\n') ? 124 : 100 : 148}; + ui_draw_rect(s->vg, rect, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA(85), 2, 20.); nvgBeginPath(s->vg); - nvgRoundedRectVarying(s->vg, metric_x + 6, metric_y + 6, 18, metric_h - 12, 25, 0, 0, 25); + nvgRoundedRectVarying(s->vg, rect.x + 6, rect.y + 6, 18, rect.h - 12, 25, 0, 0, 25); nvgFillColor(s->vg, status_color); nvgFill(s->vg); if (!message_str) { nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 78); - nvgFontFaceId(s->vg, s->font_sans_bold); + nvgFontFace(s->vg, "sans-bold"); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, metric_x + 50, metric_y + 50, metric_w - 60, value_str, NULL); + nvgTextBox(s->vg, rect.x + 50, rect.y + 50, rect.w - 60, value_str, NULL); nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFaceId(s->vg, s->font_sans_regular); + nvgFontFace(s->vg, "sans-regular"); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, metric_x + 50, metric_y + 50 + 66, metric_w - 60, label_str, NULL); + nvgTextBox(s->vg, rect.x + 50, rect.y + 50 + 66, rect.w - 60, label_str, NULL); } else { nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 48); - nvgFontFaceId(s->vg, s->font_sans_bold); + nvgFontFace(s->vg, "sans-bold"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - nvgTextBox(s->vg, metric_x + 35, metric_y + (strchr(message_str, '\n') ? 40 : 50), metric_w - 50, message_str, NULL); + nvgTextBox(s->vg, rect.x + 35, rect.y + (strchr(message_str, '\n') ? 40 : 50), rect.w - 50, message_str, NULL); } } -static void ui_draw_sidebar_temp_metric(UIState *s) { - static std::map temp_severity_map = { - {cereal::ThermalData::ThermalStatus::GREEN, 0}, - {cereal::ThermalData::ThermalStatus::YELLOW, 1}, - {cereal::ThermalData::ThermalStatus::RED, 2}, - {cereal::ThermalData::ThermalStatus::DANGER, 3}}; - std::string temp_val = std::to_string((int)s->scene.thermal.getAmbient()) + "°C"; - ui_draw_sidebar_metric(s, "TEMP", temp_val.c_str(), temp_severity_map[s->scene.thermal.getThermalStatus()], 0, NULL); +static void draw_temp_metric(UIState *s) { + static std::map temp_severity_map = { + {cereal::DeviceState::ThermalStatus::GREEN, 0}, + {cereal::DeviceState::ThermalStatus::YELLOW, 1}, + {cereal::DeviceState::ThermalStatus::RED, 2}, + {cereal::DeviceState::ThermalStatus::DANGER, 3}}; + std::string temp_val = std::to_string((int)s->scene.deviceState.getAmbientTempC()) + "°C"; + draw_metric(s, "TEMP", temp_val.c_str(), temp_severity_map[s->scene.deviceState.getThermalStatus()], 0, NULL); } -static void ui_draw_sidebar_panda_metric(UIState *s) { +static void draw_panda_metric(UIState *s) { const int panda_y_offset = 32 + 148; int panda_severity = 0; std::string panda_message = "VEHICLE\nONLINE"; - if (s->scene.hwType == cereal::HealthData::HwType::UNKNOWN) { + if (s->scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) { panda_severity = 2; - panda_message = "NO\nVEHICLE"; - } else if (s->started) { - if (s->scene.satelliteCount < 6) { - panda_severity = 1; - panda_message = "VEHICLE\nNO GPS"; - } else { - panda_severity = 0; - panda_message = "VEHICLE\nGOOD GPS"; - } + panda_message = "NO\nPANDA"; } - ui_draw_sidebar_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message.c_str()); +#ifdef QCOM2 + else if (s->started) { + panda_severity = s->scene.gpsOK ? 0 : 1; + panda_message = util::string_format("SAT CNT\n%d", s->scene.satelliteCount); + } +#endif + + draw_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message.c_str()); } -static void ui_draw_sidebar_connectivity(UIState *s) { +static void draw_connectivity(UIState *s) { static std::map> connectivity_map = { - {NET_ERROR, {"CONNECT\nERROR", 2}}, - {NET_CONNECTED, {"CONNECT\nONLINE", 0}}, - {NET_DISCONNECTED, {"CONNECT\nOFFLINE", 1}}, + {NET_ERROR, {"CONNECT\nERROR", 2}}, + {NET_CONNECTED, {"CONNECT\nONLINE", 0}}, + {NET_DISCONNECTED, {"CONNECT\nOFFLINE", 1}}, }; auto net_params = connectivity_map[s->scene.athenaStatus]; - ui_draw_sidebar_metric(s, NULL, NULL, net_params.second, 180+158, net_params.first); + draw_metric(s, NULL, NULL, net_params.second, 180 + 158, net_params.first); } void ui_draw_sidebar(UIState *s) { - if (s->scene.uilayout_sidebarcollapsed) { + if (s->sidebar_collapsed) { return; } - ui_draw_sidebar_background(s); - ui_draw_sidebar_settings_button(s); - ui_draw_sidebar_home_button(s); - ui_draw_sidebar_network_strength(s); - ui_draw_sidebar_battery_icon(s); - ui_draw_sidebar_network_type(s); - ui_draw_sidebar_temp_metric(s); - ui_draw_sidebar_panda_metric(s); - ui_draw_sidebar_connectivity(s); + draw_background(s); + draw_settings_button(s); + draw_home_button(s); + draw_network_strength(s); + draw_battery_icon(s); + draw_network_type(s); + draw_temp_metric(s); + draw_panda_metric(s); + draw_connectivity(s); } diff --git a/selfdrive/ui/sound.hpp b/selfdrive/ui/sound.hpp index bdb859080..ab83a0180 100644 --- a/selfdrive/ui/sound.hpp +++ b/selfdrive/ui/sound.hpp @@ -18,6 +18,7 @@ static std::map> sound_map { class Sound { public: + virtual ~Sound() {} virtual bool play(AudibleAlert alert) = 0; virtual void stop() = 0; virtual void setVolume(int volume) = 0; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8240372de..b60146dff 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -1,22 +1,17 @@ +#include #include #include #include #include -#include #include #include -#include -#include -#include #include "common/util.h" #include "common/swaglog.h" #include "common/visionimg.h" -#include "common/utilpp.h" #include "ui.hpp" #include "paint.hpp" -extern volatile sig_atomic_t do_exit; int write_param_float(float param, const char* param_name, bool persistent_param) { char s[16]; @@ -24,46 +19,15 @@ int write_param_float(float param, const char* param_name, bool persistent_param return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s)); } -void ui_init(UIState *s) { - s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame", - "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents"}); - - s->started = false; - s->status = STATUS_OFFROAD; - s->scene.satelliteCount = -1; - read_param(&s->is_metric, "IsMetric"); - - s->fb = framebuffer_init("ui", 0, true, &s->fb_w, &s->fb_h); - assert(s->fb); - - ui_nvg_init(s); -} static void ui_init_vision(UIState *s) { // Invisible until we receive a calibration message. s->scene.world_objects_visible = false; - for (int i = 0; i < UI_BUF_COUNT; i++) { - if (s->khr[i] != 0) { - visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]); - glDeleteTextures(1, &s->frame_texs[i]); - } + for (int i = 0; i < s->vipc_client->num_buffers; i++) { + s->texture[i].reset(new EGLImageTexture(&s->vipc_client->buffers[i])); - VisionImg img = { - .fd = s->stream.bufs[i].fd, - .format = VISIONIMG_FORMAT_RGB24, - .width = s->stream.bufs_info.width, - .height = s->stream.bufs_info.height, - .stride = s->stream.bufs_info.stride, - .bpp = 3, - .size = s->stream.bufs_info.buf_len, - }; -#ifndef QCOM - s->priv_hnds[i] = s->stream.bufs[i].addr; -#endif - s->frame_texs[i] = visionimg_to_gl(&img, &s->khr[i], &s->priv_hnds[i]); - - glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]); + glBindTexture(GL_TEXTURE_2D, s->texture[i]->frame_tex); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); @@ -75,129 +39,137 @@ static void ui_init_vision(UIState *s) { assert(glGetError() == GL_NO_ERROR); } -void ui_update_vision(UIState *s) { - if (!s->vision_connected && s->started) { - const VisionStreamType type = s->scene.frontview ? VISION_STREAM_RGB_FRONT : VISION_STREAM_RGB_BACK; - int err = visionstream_init(&s->stream, type, true, nullptr); - if (err == 0) { - ui_init_vision(s); - s->vision_connected = true; - } - } +void ui_init(UIState *s) { + s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "liveLocationKalman", + "pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss"}); - if (s->vision_connected) { - if (!s->started) goto destroy; + s->started = false; + s->status = STATUS_OFFROAD; - // poll for a new frame - struct pollfd fds[1] = {{ - .fd = s->stream.ipc_fd, - .events = POLLOUT, - }}; - int ret = poll(fds, 1, 100); - if (ret > 0) { - if (!visionstream_get(&s->stream, nullptr)) goto destroy; - } - } + s->fb = std::make_unique("ui", 0, true, &s->fb_w, &s->fb_h); - return; + ui_nvg_init(s); -destroy: - visionstream_destroy(&s->stream); - s->vision_connected = false; + s->last_frame = nullptr; + s->vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true); + s->vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true); + s->vipc_client = s->vipc_client_rear; } -void update_sockets(UIState *s) { +static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) { + const auto line_x = line.getX(); + int max_idx = 0; + for (int i = 0; i < TRAJECTORY_SIZE && line_x[i] < path_height; ++i) { + max_idx = i; + } + return max_idx; +} + +static void update_lead(UIState *s, const cereal::RadarState::Reader &radar_state, + const cereal::ModelDataV2::XYZTData::Reader &line, int idx) { + auto &lead_data = s->scene.lead_data[idx]; + lead_data = (idx == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); + if (lead_data.getStatus()) { + const int path_idx = get_path_length_idx(line, lead_data.getDRel()); + // negative because radarState uses left positive convention + calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), line.getZ()[path_idx] + 1.22, &s->scene.lead_vertices[idx]); + } +} + +static void update_line_data(const UIState *s, const cereal::ModelDataV2::XYZTData::Reader &line, + float y_off, float z_off, line_vertices_data *pvd, float max_distance) { + const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); + int max_idx = -1; + vertex_data *v = &pvd->v[0]; + for (int i = 0; ((i < TRAJECTORY_SIZE) and (line_x[i] < fmax(MIN_DRAW_DISTANCE, max_distance))); i++) { + v += calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, v); + max_idx = i; + } + for (int i = max_idx; i >= 0; i--) { + v += calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, v); + } + pvd->cnt = v - pvd->v; + assert(pvd->cnt < std::size(pvd->v)); +} + +static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { + UIScene &scene = s->scene; + const float max_distance = fmin(model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); + // update lane lines + const auto lane_lines = model.getLaneLines(); + const auto lane_line_probs = model.getLaneLineProbs(); + for (int i = 0; i < std::size(scene.lane_line_vertices); i++) { + scene.lane_line_probs[i] = lane_line_probs[i]; + update_line_data(s, lane_lines[i], 0.025 * scene.lane_line_probs[i], 0, &scene.lane_line_vertices[i], max_distance); + } + + // update road edges + const auto road_edges = model.getRoadEdges(); + const auto road_edge_stds = model.getRoadEdgeStds(); + for (int i = 0; i < std::size(scene.road_edge_vertices); i++) { + scene.road_edge_stds[i] = road_edge_stds[i]; + update_line_data(s, road_edges[i], 0.025, 0, &scene.road_edge_vertices[i], max_distance); + } + + // update path + const float lead_d = scene.lead_data[0].getStatus() ? scene.lead_data[0].getDRel() * 2. : MAX_DRAW_DISTANCE; + float path_length = (lead_d > 0.) ? lead_d - fmin(lead_d * 0.35, 10.) : MAX_DRAW_DISTANCE; + path_length = fmin(path_length, max_distance); + update_line_data(s, model.getPosition(), 0.5, 1.22, &scene.track_vertices, path_length); +} + +static void update_sockets(UIState *s) { + SubMaster &sm = *(s->sm); + if (sm.update(0) == 0) return; UIScene &scene = s->scene; - SubMaster &sm = *(s->sm); - - if (sm.update(0) == 0){ - return; - } - if (s->started && sm.updated("controlsState")) { - auto event = sm["controlsState"]; - scene.controls_state = event.getControlsState(); - - // TODO: the alert stuff shouldn't be handled here - auto alert_sound = scene.controls_state.getAlertSound(); - if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) { - if (alert_sound == AudibleAlert::NONE) { - s->sound->stop(); - } else { - s->sound->play(alert_sound); - } - } - scene.alert_text1 = scene.controls_state.getAlertText1(); - scene.alert_text2 = scene.controls_state.getAlertText2(); - scene.alert_size = scene.controls_state.getAlertSize(); - scene.alert_type = scene.controls_state.getAlertType(); - auto alertStatus = scene.controls_state.getAlertStatus(); - if (alertStatus == cereal::ControlsState::AlertStatus::USER_PROMPT) { - s->status = STATUS_WARNING; - } else if (alertStatus == cereal::ControlsState::AlertStatus::CRITICAL) { - s->status = STATUS_ALERT; - } else { - s->status = scene.controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; - } - - float alert_blinkingrate = scene.controls_state.getAlertBlinkingRate(); - if (alert_blinkingrate > 0.) { - if (s->alert_blinked) { - if (s->alert_blinking_alpha > 0.0 && s->alert_blinking_alpha < 1.0) { - s->alert_blinking_alpha += (0.05*alert_blinkingrate); - } else { - s->alert_blinked = false; - } - } else { - if (s->alert_blinking_alpha > 0.25) { - s->alert_blinking_alpha -= (0.05*alert_blinkingrate); - } else { - s->alert_blinking_alpha += 0.25; - s->alert_blinked = true; - } - } - } + scene.controls_state = sm["controlsState"].getControlsState(); + } + if (sm.updated("carState")) { + scene.car_state = sm["carState"].getCarState(); } if (sm.updated("radarState")) { - auto data = sm["radarState"].getRadarState(); - scene.lead_data[0] = data.getLeadOne(); - scene.lead_data[1] = data.getLeadTwo(); + auto radar_state = sm["radarState"].getRadarState(); + const auto line = sm["modelV2"].getModelV2().getPosition(); + update_lead(s, radar_state, line, 0); + update_lead(s, radar_state, line, 1); } if (sm.updated("liveCalibration")) { scene.world_objects_visible = true; - auto extrinsicl = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix(); - for (int i = 0; i < 3 * 4; i++) { - scene.extrinsic_matrix.v[i] = extrinsicl[i]; + auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + Eigen::Vector3d rpy; + rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + Eigen::Matrix3d device_from_calib = euler2rot(rpy); + Eigen::Matrix3d view_from_device; + view_from_device << 0,1,0, + 0,0,1, + 1,0,0; + Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); + } } } if (sm.updated("modelV2")) { - scene.model = sm["modelV2"].getModelV2(); - scene.max_distance = fmin(scene.model.getPosition().getX()[TRAJECTORY_SIZE - 1], MAX_DRAW_DISTANCE); - for (int ll_idx = 0; ll_idx < 4; ll_idx++) { - if (scene.model.getLaneLineProbs().size() > ll_idx) { - scene.lane_line_probs[ll_idx] = scene.model.getLaneLineProbs()[ll_idx]; - } else { - scene.lane_line_probs[ll_idx] = 0.0; - } - } - - for (int re_idx = 0; re_idx < 2; re_idx++) { - if (scene.model.getRoadEdgeStds().size() > re_idx) { - scene.road_edge_stds[re_idx] = scene.model.getRoadEdgeStds()[re_idx]; - } else { - scene.road_edge_stds[re_idx] = 1.0; - } - } + update_model(s, sm["modelV2"].getModelV2()); } if (sm.updated("uiLayoutState")) { auto data = sm["uiLayoutState"].getUiLayoutState(); s->active_app = data.getActiveApp(); - scene.uilayout_sidebarcollapsed = data.getSidebarCollapsed(); + s->sidebar_collapsed = data.getSidebarCollapsed(); } - if (sm.updated("thermal")) { - scene.thermal = sm["thermal"].getThermal(); + if (sm.updated("deviceState")) { + scene.deviceState = sm["deviceState"].getDeviceState(); + } + if (sm.updated("pandaState")) { + auto pandaState = sm["pandaState"].getPandaState(); + scene.pandaType = pandaState.getPandaType(); + s->ignition = pandaState.getIgnitionLine() || pandaState.getIgnitionCan(); + } else if ((s->sm->frame - s->sm->rcv_frame("pandaState")) > 5*UI_FREQ) { + scene.pandaType = cereal::PandaState::PandaType::UNKNOWN; } if (sm.updated("ubloxGnss")) { auto data = sm["ubloxGnss"].getUbloxGnss(); @@ -205,12 +177,8 @@ void update_sockets(UIState *s) { scene.satelliteCount = data.getMeasurementReport().getNumMeas(); } } - if (sm.updated("health")) { - auto health = sm["health"].getHealth(); - scene.hwType = health.getHwType(); - s->ignition = health.getIgnitionLine() || health.getIgnitionCan(); - } else if ((s->sm->frame - s->sm->rcv_frame("health")) > 5*UI_FREQ) { - scene.hwType = cereal::HealthData::HwType::UNKNOWN; + if (sm.updated("liveLocationKalman")) { + scene.gpsOK = sm["liveLocationKalman"].getLiveLocationKalman().getGpsOK(); } if (sm.updated("carParams")) { s->longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl(); @@ -218,11 +186,12 @@ void update_sockets(UIState *s) { if (sm.updated("driverState")) { scene.driver_state = sm["driverState"].getDriverState(); } - if (sm.updated("dMonitoringState")) { - scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState(); - scene.is_rhd = scene.dmonitoring_state.getIsRHD(); - scene.frontview = scene.dmonitoring_state.getIsPreview(); - } else if ((sm.frame - sm.rcv_frame("dMonitoringState")) > UI_FREQ/2) { + if (sm.updated("driverMonitoringState")) { + scene.dmonitoring_state = sm["driverMonitoringState"].getDriverMonitoringState(); + if(!scene.frontview && !s->ignition) { + read_param(&scene.frontview, "IsDriverViewEnabled"); + } + } else if ((sm.frame - sm.rcv_frame("driverMonitoringState")) > UI_FREQ/2) { scene.frontview = false; } if (sm.updated("sensorEvents")) { @@ -236,77 +205,122 @@ void update_sockets(UIState *s) { } } } - - s->started = scene.thermal.getStarted() || scene.frontview; + s->started = scene.deviceState.getStarted() || scene.frontview; } -void ui_update(UIState *s) { - - update_sockets(s); - ui_update_vision(s); - - // Handle onroad/offroad transition - if (!s->started && s->status != STATUS_OFFROAD) { - s->status = STATUS_OFFROAD; - s->active_app = cereal::UiLayoutState::App::HOME; - s->scene.uilayout_sidebarcollapsed = false; - s->sound->stop(); - } else if (s->started && s->status == STATUS_OFFROAD) { - s->status = STATUS_DISENGAGED; - s->started_frame = s->sm->frame; - - s->active_app = cereal::UiLayoutState::App::NONE; - s->scene.uilayout_sidebarcollapsed = true; - s->alert_blinked = false; - s->alert_blinking_alpha = 1.0; - s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; +static void update_alert(UIState *s) { + UIScene &scene = s->scene; + if (s->sm->updated("controlsState")) { + auto alert_sound = scene.controls_state.getAlertSound(); + if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) { + if (alert_sound == AudibleAlert::NONE) { + s->sound->stop(); + } else { + s->sound->play(alert_sound); + } + } + scene.alert_text1 = scene.controls_state.getAlertText1(); + scene.alert_text2 = scene.controls_state.getAlertText2(); + scene.alert_size = scene.controls_state.getAlertSize(); + scene.alert_type = scene.controls_state.getAlertType(); + scene.alert_blinking_rate = scene.controls_state.getAlertBlinkingRate(); } - // Handle controls/fcamera timeout - if (s->started && !s->scene.frontview && ((s->sm)->frame - s->started_frame) > 10*UI_FREQ) { - if ((s->sm)->rcv_frame("controlsState") < s->started_frame) { + // Handle controls timeout + if (scene.deviceState.getStarted() && (s->sm->frame - s->started_frame) > 10 * UI_FREQ) { + const uint64_t cs_frame = s->sm->rcv_frame("controlsState"); + if (cs_frame < s->started_frame) { // car is started, but controlsState hasn't been seen at all - s->scene.alert_text1 = "openpilot Unavailable"; - s->scene.alert_text2 = "Waiting for controls to start"; - s->scene.alert_size = cereal::ControlsState::AlertSize::MID; - } else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 5*UI_FREQ) { + scene.alert_text1 = "openpilot Unavailable"; + scene.alert_text2 = "Waiting for controls to start"; + scene.alert_size = cereal::ControlsState::AlertSize::MID; + } else if ((s->sm->frame - cs_frame) > 5 * UI_FREQ) { // car is started, but controls is lagging or died - if (s->scene.alert_text2 != "Controls Unresponsive" && - s->scene.alert_text1 != "Camera Malfunction") { + if (scene.alert_text2 != "Controls Unresponsive") { s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT); LOGE("Controls unresponsive"); } - s->scene.alert_text1 = "TAKE CONTROL IMMEDIATELY"; - s->scene.alert_text2 = "Controls Unresponsive"; - s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; + scene.alert_text1 = "TAKE CONTROL IMMEDIATELY"; + scene.alert_text2 = "Controls Unresponsive"; + scene.alert_size = cereal::ControlsState::AlertSize::FULL; s->status = STATUS_ALERT; } - - const uint64_t frame_pkt = (s->sm)->rcv_frame("frame"); - const uint64_t frame_delayed = (s->sm)->frame - frame_pkt; - const uint64_t since_started = (s->sm)->frame - s->started_frame; - if ((frame_pkt > s->started_frame || since_started > 15*UI_FREQ) && frame_delayed > 5*UI_FREQ) { - // controls is fine, but rear camera is lagging or died - s->scene.alert_text1 = "Camera Malfunction"; - s->scene.alert_text2 = "Contact Support"; - s->scene.alert_size = cereal::ControlsState::AlertSize::FULL; - s->status = STATUS_DISENGAGED; - s->sound->stop(); - } - } - - // Read params - if ((s->sm)->frame % (5*UI_FREQ) == 0) { - read_param(&s->is_metric, "IsMetric"); - } else if ((s->sm)->frame % (6*UI_FREQ) == 0) { - int param_read = read_param(&s->last_athena_ping, "LastAthenaPingTime"); - if (param_read != 0) { // Failed to read param - s->scene.athenaStatus = NET_DISCONNECTED; - } else if (nanos_since_boot() - s->last_athena_ping < 70e9) { - s->scene.athenaStatus = NET_CONNECTED; - } else { - s->scene.athenaStatus = NET_ERROR; - } } } + +static void update_params(UIState *s) { + const uint64_t frame = s->sm->frame; + + if (frame % (5*UI_FREQ) == 0) { + read_param(&s->is_metric, "IsMetric"); + } else if (frame % (6*UI_FREQ) == 0) { + s->scene.athenaStatus = NET_DISCONNECTED; + uint64_t last_ping = 0; + if (read_param(&last_ping, "LastAthenaPingTime") == 0) { + s->scene.athenaStatus = nanos_since_boot() - last_ping < 70e9 ? NET_CONNECTED : NET_ERROR; + } + } +} + +static void update_vision(UIState *s) { + if (!s->vipc_client->connected && s->started) { + if (s->vipc_client->connect(false)){ + ui_init_vision(s); + } + } + + if (s->vipc_client->connected){ + VisionBuf * buf = s->vipc_client->recv(); + if (buf != nullptr){ + s->last_frame = buf; + } else { +#if defined(QCOM) || defined(QCOM2) + LOGE("visionIPC receive timeout"); +#endif + } + } +} + +static void update_status(UIState *s) { + if (s->started && s->sm->updated("controlsState")) { + auto alert_status = s->scene.controls_state.getAlertStatus(); + if (alert_status == cereal::ControlsState::AlertStatus::USER_PROMPT) { + s->status = STATUS_WARNING; + } else if (alert_status == cereal::ControlsState::AlertStatus::CRITICAL) { + s->status = STATUS_ALERT; + } else { + s->status = s->scene.controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; + } + } + + // Handle onroad/offroad transition + static bool started_prev = false; + if (s->started != started_prev) { + if (s->started) { + s->status = STATUS_DISENGAGED; + s->started_frame = s->sm->frame; + + read_param(&s->scene.is_rhd, "IsRHD"); + s->active_app = cereal::UiLayoutState::App::NONE; + s->sidebar_collapsed = true; + s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; + s->vipc_client = s->scene.frontview ? s->vipc_client_front : s->vipc_client_rear; + } else { + s->status = STATUS_OFFROAD; + s->active_app = cereal::UiLayoutState::App::HOME; + s->sidebar_collapsed = false; + s->sound->stop(); + s->vipc_client->connected = false; + } + } + started_prev = s->started; +} + +void ui_update(UIState *s) { + update_params(s); + update_sockets(s); + update_status(s); + update_alert(s); + update_vision(s); +} diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index b18e51f20..f184ffc5d 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -13,18 +13,22 @@ #include #include +#include #include #include #include "nanovg.h" #include "common/mat.h" -#include "common/visionipc.h" #include "common/visionimg.h" #include "common/framebuffer.h" #include "common/modeldata.h" #include "common/params.h" +#include "common/glutil.h" +#include "common/transformations/orientation.hpp" #include "sound.hpp" +#include "visionipc.h" +#include "visionipc_client.h" #define COLOR_BLACK nvgRGBA(0, 0, 0, 255) #define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x) @@ -38,6 +42,7 @@ typedef struct Rect { int x, y, w, h; int centerX() const { return x + w / 2; } + int centerY() const { return y + h / 2; } int right() const { return x + w; } int bottom() const { return y + h; } bool ptInRect(int px, int py) const { @@ -54,11 +59,6 @@ const Rect home_btn = {60, 1080 - 180 - 40, 180, 180}; const int UI_FREQ = 20; // Hz -const int MODEL_PATH_MAX_VERTICES_CNT = TRAJECTORY_SIZE*2; -const int TRACK_POINTS_MAX_CNT = TRAJECTORY_SIZE*4; - -const int SET_SPEED_NA = 255; - typedef enum NetStatus { NET_CONNECTED, NET_DISCONNECTED, @@ -74,93 +74,80 @@ typedef enum UIStatus { } UIStatus; static std::map bg_colors = { +#ifdef QCOM {STATUS_OFFROAD, nvgRGBA(0x07, 0x23, 0x39, 0xf1)}, +#else + {STATUS_OFFROAD, nvgRGBA(0x0, 0x0, 0x0, 0xff)}, +#endif {STATUS_DISENGAGED, nvgRGBA(0x17, 0x33, 0x49, 0xc8)}, {STATUS_ENGAGED, nvgRGBA(0x17, 0x86, 0x44, 0xf1)}, {STATUS_WARNING, nvgRGBA(0xDA, 0x6F, 0x25, 0xf1)}, {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, }; -typedef struct { - float x[TRAJECTORY_SIZE]; - float y[TRAJECTORY_SIZE]; - float z[TRAJECTORY_SIZE]; -} line; - - -typedef struct UIScene { - - mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. - bool world_objects_visible; - - bool is_rhd; - bool frontview; - bool uilayout_sidebarcollapsed; - // responsive layout - Rect viz_rect; - - std::string alert_text1; - std::string alert_text2; - std::string alert_type; - cereal::ControlsState::AlertSize alert_size; - - cereal::HealthData::HwType hwType; - int satelliteCount; - NetStatus athenaStatus; - - cereal::ThermalData::Reader thermal; - cereal::RadarState::LeadData::Reader lead_data[2]; - cereal::ControlsState::Reader controls_state; - cereal::DriverState::Reader driver_state; - cereal::DMonitoringState::Reader dmonitoring_state; - cereal::ModelDataV2::Reader model; - line path; - line outer_left_lane_line; - line left_lane_line; - line right_lane_line; - line outer_right_lane_line; - line left_road_edge; - line right_road_edge; - float max_distance; - float lane_line_probs[4]; - float road_edge_stds[2]; -} UIScene; - typedef struct { float x, y; } vertex_data; typedef struct { - vertex_data v[MODEL_PATH_MAX_VERTICES_CNT]; + vertex_data v[TRAJECTORY_SIZE * 2]; int cnt; } line_vertices_data; -typedef struct { - vertex_data v[TRACK_POINTS_MAX_CNT]; - int cnt; -} track_vertices_data; +typedef struct UIScene { + mat3 view_from_calib; + bool world_objects_visible; + + bool is_rhd; + bool frontview; + + std::string alert_text1; + std::string alert_text2; + std::string alert_type; + float alert_blinking_rate; + cereal::ControlsState::AlertSize alert_size; + + cereal::PandaState::PandaType pandaType; + NetStatus athenaStatus; + + cereal::DeviceState::Reader deviceState; + cereal::RadarState::LeadData::Reader lead_data[2]; + cereal::CarState::Reader car_state; + cereal::ControlsState::Reader controls_state; + cereal::DriverState::Reader driver_state; + cereal::DriverMonitoringState::Reader dmonitoring_state; + + // gps + int satelliteCount; + bool gpsOK; + + // modelV2 + float lane_line_probs[4]; + float road_edge_stds[2]; + line_vertices_data track_vertices; + line_vertices_data lane_line_vertices[4]; + line_vertices_data road_edge_vertices[2]; + + // lead + vertex_data lead_vertices[2]; +} UIScene; typedef struct UIState { + VisionIpcClient * vipc_client; + VisionIpcClient * vipc_client_front; + VisionIpcClient * vipc_client_rear; + VisionBuf * last_frame; + // framebuffer - FramebufferState *fb; + std::unique_ptr fb; int fb_w, fb_h; // NVG NVGcontext *vg; - // fonts and images - int font_sans_regular; - int font_sans_semibold; - int font_sans_bold; - int img_wheel; - int img_turn; - int img_face; - int img_button_settings; - int img_button_home; - int img_battery; - int img_battery_charging; - int img_network[6]; + // images + std::map images; SubMaster *sm; @@ -169,18 +156,10 @@ typedef struct UIState { UIScene scene; cereal::UiLayoutState::App active_app; - // vision state - bool vision_connected; - VisionStream stream; - // graphics - GLuint frame_program; - GLuint frame_texs[UI_BUF_COUNT]; - EGLImageKHR khr[UI_BUF_COUNT]; - void *priv_hnds[UI_BUF_COUNT]; + std::unique_ptr gl_shader; + std::unique_ptr texture[UI_BUF_COUNT]; - GLint frame_pos_loc, frame_texcoord_loc; - GLint frame_texture_loc, frame_transform_loc; GLuint frame_vao[2], frame_vbo[2], frame_ibo[2]; mat4 rear_frame_mat, front_frame_mat; @@ -192,17 +171,11 @@ typedef struct UIState { bool ignition; bool is_metric; bool longitudinal_control; - uint64_t last_athena_ping; uint64_t started_frame; - bool alert_blinked; - float alert_blinking_alpha; - - track_vertices_data track_vertices; - line_vertices_data lane_line_vertices[4]; - line_vertices_data road_edge_vertices[2]; - - Rect video_rect; + bool sidebar_collapsed; + Rect video_rect, viz_rect; + float car_space_transform[6]; } UIState; void ui_init(UIState *s); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index d1eee4d33..a5ef3722d 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -36,14 +36,14 @@ from typing import List, Tuple, Optional from common.basedir import BASEDIR from common.params import Params -from selfdrive.hardware import EON, TICI +from selfdrive.hardware import EON, TICI, HARDWARE from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert +from selfdrive.hardware.tici.agnos import flash_agnos_update LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") -NEOS_VERSION = os.getenv("UPDATER_NEOS_VERSION", "/VERSION") NEOSUPDATE_DIR = os.getenv("UPDATER_NEOSUPDATE_DIR", "/data/neoupdate") OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper") @@ -214,10 +214,26 @@ def finalize_update() -> None: cloudlog.info("done finalizing overlay") -def handle_neos_update(wait_helper: WaitTimeHelper) -> None: - with open(NEOS_VERSION, "r") as f: - cur_neos = f.read().strip() +def handle_agnos_update(wait_helper): + cur_version = HARDWARE.get_os_version() + updated_version = run(["bash", "-c", r"unset AGNOS_VERSION && source launch_env.sh && \ + echo -n $AGNOS_VERSION"], OVERLAY_MERGED).strip() + cloudlog.info(f"AGNOS version check: {cur_version} vs {updated_version}") + if cur_version == updated_version: + return + + # prevent an openpilot getting swapped in with a mismatched or partially downloaded agnos + set_consistent_flag(False) + + cloudlog.info(f"Beginning background installation for AGNOS {updated_version}") + + manifest_path = os.path.join(OVERLAY_MERGED, "selfdrive/hardware/tici/agnos.json") + flash_agnos_update(manifest_path, cloudlog) + + +def handle_neos_update(wait_helper: WaitTimeHelper) -> None: + cur_neos = HARDWARE.get_os_version() updated_neos = run(["bash", "-c", r"unset REQUIRED_NEOS_VERSION && source launch_env.sh && \ echo -n $REQUIRED_NEOS_VERSION"], OVERLAY_MERGED).strip() @@ -294,6 +310,8 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool: if EON: handle_neos_update(wait_helper) + elif TICI: + handle_agnos_update(wait_helper) # Create the finalized, ready-to-swap update finalize_update() @@ -392,5 +410,6 @@ def main(): dismount_overlay() + if __name__ == "__main__": main() diff --git a/selfdrive/version.py b/selfdrive/version.py index ef975f894..b06841832 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -52,6 +52,7 @@ comma_remote: bool = False tested_branch: bool = False origin = get_git_remote() branch = get_git_full_branchname() +commit = get_git_commit() if (origin is not None) and (branch is not None): try: @@ -74,7 +75,7 @@ if (origin is not None) and (branch is not None): try: dirty_files = run_cmd(["git", "diff-index", branch, "--"]) cloudlog.event("dirty comma branch", version=version, dirty=dirty, origin=origin, branch=branch, - dirty_files=dirty_files, commit=get_git_commit(), origin_commit=get_git_commit(branch)) + dirty_files=dirty_files, commit=commit, origin_commit=get_git_commit(branch)) except subprocess.CalledProcessError: pass diff --git a/site_scons/site_tools/cython.py b/site_scons/site_tools/cython.py index 45ba797c4..c29147553 100644 --- a/site_scons/site_tools/cython.py +++ b/site_scons/site_tools/cython.py @@ -1,18 +1,52 @@ +import re import SCons from SCons.Action import Action +from SCons.Scanner import Scanner +pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M) +pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M) +cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M) + + +def pyx_scan(node, env, path, arg=None): + contents = node.get_text_contents() + + # from cimport ... + matches = pyx_from_import_re.findall(contents) + # cimport + matches += pyx_import_re.findall(contents) + + # Modules can be either .pxd or .pyx files + files = [m.replace('.', '/') + '.pxd' for m in matches] + files += [m.replace('.', '/') + '.pyx' for m in matches] + + # cdef extern from + files += cdef_import_re.findall(contents) + + # Handle relative imports + cur_dir = str(node.get_dir()) + files = [cur_dir + f if f.startswith('/') else f for f in files] + + # Filter out non-existing files (probably system imports) + files = [f for f in files if env.File(f).exists()] + return env.File(files) + + +pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True) cythonAction = Action("$CYTHONCOM") + def create_builder(env): try: cython = env['BUILDERS']['Cython'] except KeyError: cython = SCons.Builder.Builder( - action = cythonAction, - emitter = {}, - suffix = cython_suffix_emitter, - single_source = 1 - ) + action=cythonAction, + emitter={}, + suffix=cython_suffix_emitter, + single_source=1 + ) + env.Append(SCANNERS=pyxscanner) env['BUILDERS']['Cython'] = cython return cython