diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index d1d6913adc..17de278936 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -71,7 +71,6 @@ jobs: /usr/local/Cellar ~/github_brew_cache_entries.txt key: macos-cache-${{ hashFiles('tools/mac_setup.sh') }} - restore-keys: macos-cache- - name: Brew link restored dependencies if: steps.dependency-cache.outputs.cache-hit == 'true' run: | diff --git a/.gitignore b/.gitignore index 75b9d55114..ec33cd9f75 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ venv/ +.clang-format .DS_Store .tags .ipynb_checkpoints @@ -43,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 diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 8363a06fdd..02d4758d85 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -1,28 +1,32 @@ FROM commaai/openpilot-base:latest ENV PYTHONUNBUFFERED 1 -ENV PYTHONPATH /tmp/openpilot:${PYTHONPATH} -RUN mkdir -p /tmp/openpilot +ENV OPENPILOT_PATH /home/batman/openpilot/ +ENV PYTHONPATH ${OPENPILOT_PATH}:${PYTHONPATH} -COPY SConstruct \ - .pylintrc \ - .pre-commit-config.yaml \ - /tmp/openpilot/ +RUN mkdir -p ${OPENPILOT_PATH} +WORKDIR ${OPENPILOT_PATH} -COPY ./pyextra /tmp/openpilot/pyextra -COPY ./phonelibs /tmp/openpilot/phonelibs -COPY ./site_scons /tmp/openpilot/site_scons -COPY ./laika /tmp/openpilot/laika -COPY ./laika_repo /tmp/openpilot/laika_repo -COPY ./rednose /tmp/openpilot/rednose -COPY ./tools /tmp/openpilot/tools -COPY ./release /tmp/openpilot/release -COPY ./common /tmp/openpilot/common -COPY ./opendbc /tmp/openpilot/opendbc -COPY ./cereal /tmp/openpilot/cereal -COPY ./panda /tmp/openpilot/panda -COPY ./selfdrive /tmp/openpilot/selfdrive +COPY Pipfile Pipfile.lock $OPENPILOT_PATH +RUN pip install --no-cache-dir pipenv==2020.8.13 && \ + pipenv install --system --deploy --dev --clear && \ + pip uninstall -y pipenv + +COPY SConstruct ${OPENPILOT_PATH} + +COPY ./pyextra ${OPENPILOT_PATH}/pyextra +COPY ./phonelibs ${OPENPILOT_PATH}/phonelibs +COPY ./site_scons ${OPENPILOT_PATH}/site_scons +COPY ./laika ${OPENPILOT_PATH}/laika +COPY ./laika_repo ${OPENPILOT_PATH}/laika_repo +COPY ./rednose ${OPENPILOT_PATH}/rednose +COPY ./tools ${OPENPILOT_PATH}/tools +COPY ./release ${OPENPILOT_PATH}/release +COPY ./common ${OPENPILOT_PATH}/common +COPY ./opendbc ${OPENPILOT_PATH}/opendbc +COPY ./cereal ${OPENPILOT_PATH}/cereal +COPY ./panda ${OPENPILOT_PATH}/panda +COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive -WORKDIR /tmp/openpilot RUN scons -j$(nproc) diff --git a/Jenkinsfile b/Jenkinsfile index eb4770fa78..a28b7cbfd9 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -114,7 +114,7 @@ pipeline { ["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"], - //["test cpu usage", "cd selfdrive/test/ && ./test_cpu_usage.py"], + ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"], ["build devel", "cd release && CI_PUSH=${env.CI_PUSH} ./build_devel.sh"], ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"], ["test spinner build", "cd selfdrive/ui/spinner && make clean && make"], diff --git a/README.md b/README.md index 1235323de3..42bb717915 100644 --- a/README.md +++ b/README.md @@ -97,7 +97,7 @@ 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 | @@ -151,12 +151,12 @@ 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 | Santa Fe 2019-20 | All | Stock | 0mph | 0mph | | Hyundai | Sonata 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 | diff --git a/SConstruct b/SConstruct index 5ac384187a..3aa0c4577d 100644 --- a/SConstruct +++ b/SConstruct @@ -33,6 +33,12 @@ 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" @@ -375,3 +381,7 @@ if arch != "Darwin": 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/cereal b/cereal index 61a3b45ed2..610fa77bc4 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 61a3b45ed2597b24be6e9d8920b2cd36609267db +Subproject commit 610fa77bc41ddf58e57f52ef678222a435cb6748 diff --git a/common/numpy_helpers.py b/common/numpy_helpers.py new file mode 100644 index 0000000000..7b1efe8976 --- /dev/null +++ b/common/numpy_helpers.py @@ -0,0 +1,22 @@ +import numpy as np + + +def deep_interp_np(x, xp, fp, axis=None): + if axis is not None: + fp = fp.swapaxes(0,axis) + x = np.atleast_1d(x) + xp = np.array(xp) + if len(xp) < 2: + return np.repeat(fp, len(x), axis=0) + if min(np.diff(xp)) < 0: + raise RuntimeError('Bad x array for interpolation') + j = np.searchsorted(xp, x) - 1 + j = np.clip(j, 0, len(xp)-2) + d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0) + vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T + if axis is not None: + vals_interp = vals_interp.swapaxes(0,axis) + if len(vals_interp) == 1: + return vals_interp[0] + else: + return vals_interp diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index b104011e28..0356de536b 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], diff --git a/installer/updater/update_agnos.json b/installer/updater/update_agnos.json index 533fb0bb99..f6dca50984 100644 --- a/installer/updater/update_agnos.json +++ b/installer/updater/update_agnos.json @@ -1,17 +1,17 @@ [ { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate-staging/system-1e5748c84d9b48bbe599c88da63e46b7ed1c0f1245486cfbcfb05399bd16dbb6.img.xz", - "hash": "de6afb8651dc011fed8a883bac191e61d4b58e2e1c84c2717816b64f71afd3b1", - "hash_raw": "1e5748c84d9b48bbe599c88da63e46b7ed1c0f1245486cfbcfb05399bd16dbb6", + "url": "https://commadist.azureedge.net/agnosupdate-staging/system-3fa2b899a6ca13faed640de9098ec16f15df816493a8a30b70c3e71cbb7a7a23.img.xz", + "hash": "23d1b6ab6c375f3d32815ed257382f42d779778d892c262e699070816e400d5a", + "hash_raw": "3fa2b899a6ca13faed640de9098ec16f15df816493a8a30b70c3e71cbb7a7a23", "size": 10737418240, "sparse": true }, { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-21649cb35935a92776155e7bb23e437e7e7eb0500c082df89f59a6d4a4ed639a.img.xz", - "hash": "21649cb35935a92776155e7bb23e437e7e7eb0500c082df89f59a6d4a4ed639a", - "hash_raw": "21649cb35935a92776155e7bb23e437e7e7eb0500c082df89f59a6d4a4ed639a", + "url": "https://commadist.azureedge.net/agnosupdate-staging/boot-52b630e250141dbe89db5bb8f5447f3b20bd75539bdd8e23a80bd2217fe0b86c.img.xz", + "hash": "52b630e250141dbe89db5bb8f5447f3b20bd75539bdd8e23a80bd2217fe0b86c", + "hash_raw": "52b630e250141dbe89db5bb8f5447f3b20bd75539bdd8e23a80bd2217fe0b86c", "size": 14678016, "sparse": false } diff --git a/laika_repo b/laika_repo index 4b7947374e..f8f5788df3 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 4b7947374e31df02587dcdd4c7562a78344e11b3 +Subproject commit f8f5788df38cb286e1d136e96fb109f197deee37 diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index e52fa6db4b..fc3358c101 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -4,6 +4,8 @@ if [ -z "$BASEDIR" ]; then BASEDIR="/data/openpilot" fi +unset REQUIRED_NEOS_VERSION +unset AGNOS_VERSION source "$BASEDIR/launch_env.sh" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" @@ -195,6 +197,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" diff --git a/launch_env.sh b/launch_env.sh index cb3c241df6..b8ab0d294e 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -11,7 +11,7 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then fi if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="0.1" + export AGNOS_VERSION="0.2" fi if [ -z "$PASSIVE" ]; then diff --git a/release/files_common b/release/files_common index 6dd9dcefbb..76058366df 100644 --- a/release/files_common +++ b/release/files_common @@ -307,6 +307,7 @@ selfdrive/loggerd/omx_encoder.h selfdrive/loggerd/logger.cc selfdrive/loggerd/logger.h selfdrive/loggerd/loggerd.cc +selfdrive/loggerd/bootlog.cc selfdrive/loggerd/raw_logger.cc selfdrive/loggerd/raw_logger.h selfdrive/loggerd/include/msm_media_info.h @@ -333,7 +334,6 @@ selfdrive/thermald/power_monitoring.py selfdrive/test/__init__.py selfdrive/test/helpers.py selfdrive/test/setup_device_ci.sh -selfdrive/test/test_cpu_usage.py selfdrive/test/test_fingerprints.py selfdrive/test/test_manager.py @@ -349,7 +349,7 @@ selfdrive/ui/android/spinner/spinner.c selfdrive/ui/android/text/Makefile selfdrive/ui/android/text/text -selfdrive/ui/android/text/text.c +selfdrive/ui/android/text/text.cc selfdrive/ui/qt/*.cc selfdrive/ui/qt/*.hpp diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 27e8b587f1..8f08c390ba 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -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/tests/helpers.py b/selfdrive/athena/tests/helpers.py index b30ff4b3af..ef64968f7b 100644 --- a/selfdrive/athena/tests/helpers.py +++ b/selfdrive/athena/tests/helpers.py @@ -1,6 +1,4 @@ import http.server -import multiprocessing -import queue import random import requests import socket @@ -8,6 +6,7 @@ import time from functools import wraps from multiprocessing import Process +from common.timeout import Timeout class EchoSocket(): def __init__(self, port): @@ -79,41 +78,27 @@ class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): self.end_headers() -def http_server(port_queue, **kwargs): - while 1: - try: - port = random.randrange(40000, 50000) - port_queue.put(port) - http.server.test(**kwargs, port=port) - except OSError as e: - if e.errno == 98: - continue - - def with_http_server(func): @wraps(func) def inner(*args, **kwargs): - port_queue = multiprocessing.Queue() - host = '127.0.0.1' - p = Process(target=http_server, - args=(port_queue,), - kwargs={ - 'HandlerClass': HTTPRequestHandler, - 'bind': host}) - p.start() - start = time.monotonic() - port = None - while 1: - if time.monotonic() - start > 10: - raise Exception('HTTP Server did not start') - - try: - port = port_queue.get(timeout=0.1) - requests.put(f'http://{host}:{port}/qlog.bz2', data='') - break - except (requests.exceptions.ConnectionError, queue.Empty): + with Timeout(2, 'HTTP Server did not start'): + p = None + host = '127.0.0.1' + while p is None or p.exitcode is not None: + port = random.randrange(40000, 50000) + p = Process(target=http.server.test, + kwargs={'port': port, 'HandlerClass': HTTPRequestHandler, 'bind': host}) + p.start() time.sleep(0.1) + with Timeout(2): + while True: + try: + requests.put(f'http://{host}:{port}/qlog.bz2', data='') + break + except requests.exceptions.ConnectionError: + time.sleep(0.1) + try: return func(*args, f'http://{host}:{port}', **kwargs) finally: diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 45e12af03a..503aacb032 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -64,10 +64,8 @@ class TestAthenadMethods(unittest.TestCase): try: item = athenad.UploadItem(path=fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') - try: + with self.assertRaises(requests.exceptions.ConnectionError): athenad._do_upload(item) - except requests.exceptions.ConnectionError: - pass item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') resp = athenad._do_upload(item) diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py deleted file mode 100755 index 71db229a21..0000000000 --- a/selfdrive/boardd/tests/replay_many.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 -import os -import sys -import time -import signal -import traceback -import usb1 -from panda import Panda, PandaDFU -from multiprocessing import Pool - -jungle = "JUNGLE" in os.environ -if jungle: - from panda_jungle import PandaJungle # pylint: disable=import-error - -import cereal.messaging as messaging -from selfdrive.boardd.boardd import can_capnp_to_can_list - -def initializer(): - """Ignore CTRL+C in the worker process. - source: https://stackoverflow.com/a/44869451 """ - signal.signal(signal.SIGINT, signal.SIG_IGN) - -def send_thread(sender_serial): - global jungle - while True: - try: - if jungle: - sender = PandaJungle(sender_serial) - else: - sender = Panda(sender_serial) - sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) - - sender.set_can_loopback(False) - can_sock = messaging.sub_sock('can') - - while True: - tsc = messaging.recv_one(can_sock) - snd = can_capnp_to_can_list(tsc.can) - snd = list(filter(lambda x: x[-1] <= 2, snd)) - - try: - sender.can_send_many(snd) - except usb1.USBErrorTimeout: - pass - - # Drain panda message buffer - sender.can_recv() - except Exception: - traceback.print_exc() - time.sleep(1) - -if __name__ == "__main__": - if jungle: - serials = PandaJungle.list() - else: - serials = Panda.list() - num_senders = len(serials) - - if num_senders == 0: - print("No senders found. Exiting") - sys.exit(1) - else: - print("%d senders found. Starting broadcast" % num_senders) - - if "FLASH" in os.environ: - for s in PandaDFU.list(): - PandaDFU(s).recover() - - time.sleep(1) - for s in serials: - Panda(s).recover() - Panda(s).flash() - - pool = Pool(num_senders, initializer=initializer) - pool.map_async(send_thread, serials) - - while True: - try: - time.sleep(10) - except KeyboardInterrupt: - pool.terminate() - pool.join() - raise diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index a01c84cf36..1fd311d42d 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -58,7 +58,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, frame_buf_count = frame_cnt; // RAW frame - frame_size = ci->frame_height * ci->frame_stride; + 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); @@ -121,14 +121,14 @@ CameraBuf::~CameraBuf() { } bool CameraBuf::acquire() { - std::unique_lock lk(frame_queue_mutex); - - bool got_frame = frame_queue_cv.wait_for(lk, std::chrono::milliseconds(1), [this]{ return !frame_queue.empty(); }); - if (!got_frame) return false; + { + std::unique_lock lk(frame_queue_mutex); + bool got_frame = frame_queue_cv.wait_for(lk, std::chrono::milliseconds(1), [this]{ return !frame_queue.empty(); }); + if (!got_frame) return false; - cur_buf_idx = frame_queue.front(); - frame_queue.pop(); - lk.unlock(); + cur_buf_idx = frame_queue.front(); + frame_queue.pop(); + } const FrameMetadata &frame_data = camera_bufs_metadata[cur_buf_idx]; if (frame_data.frame_id == -1) { @@ -139,7 +139,6 @@ bool CameraBuf::acquire() { cur_frame_data = frame_data; cur_rgb_buf = vipc_server->get_buffer(rgb_type); - cur_rgb_idx = cur_rgb_buf->idx; cl_event debayer_event; cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl; @@ -164,7 +163,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)); @@ -174,8 +172,7 @@ bool CameraBuf::acquire() { CL_CHECK(clReleaseEvent(debayer_event)); cur_yuv_buf = vipc_server->get_buffer(yuv_type); - cur_yuv_idx = cur_yuv_buf->idx; - yuv_metas[cur_yuv_idx] = frame_data; + 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); VisionIpcBufExtra extra = { @@ -218,25 +215,25 @@ 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]; - - int goff = x_min*3 + y_min*stride; - for (int r=0;rcur_rgb_buf); + const uint8_t *dat = (const uint8_t *)b->cur_rgb_buf->addr; + int scale = env_scale; + int x_min = env_xmin; int y_min = env_ymin; int x_max = b->rgb_width-1; int y_max = b->rgb_height-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]; + + 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; } + framed.setImage(kj::arrayPtr((const uint8_t*)resized_dat, (size_t)new_width*new_height*3)); + delete[] resized_dat; } static void create_thumbnail(MultiCameraState *s, const CameraBuf *b) { @@ -414,7 +411,7 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i 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_image(framed, b); } pm->send("frontFrame", msg); } diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 309bccd6dc..a069af3408 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -102,12 +102,10 @@ public: mat3 yuv_transform; FrameMetadata yuv_metas[YUV_COUNT]; - size_t yuv_buf_size; VisionStreamType rgb_type, yuv_type; int rgb_width, rgb_height, rgb_stride; - int cur_yuv_idx, cur_rgb_idx; FrameMetadata cur_frame_data; VisionBuf *cur_rgb_buf; @@ -122,7 +120,6 @@ public: std::unique_ptr camera_bufs_metadata; int frame_buf_count; - int frame_size; release_cb release_callback; @@ -137,7 +134,7 @@ public: 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 fill_frame_image(cereal::FrameData::Builder &framed, 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); diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 6086ca7bf9..55a503417e 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -1557,9 +1557,7 @@ 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 rear_op_id_last = 0; int front_op_id_last = 0; @@ -1584,8 +1582,6 @@ static void* ops_thread(void* arg) { util::sleep_for(50); } - - return NULL; } static void update_lapmap(MultiCameraState *s, const CameraBuf *b, const int cnt) { @@ -1662,7 +1658,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { 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); + fill_frame_image(framed, b); } framed.setFocusVal(s->rear.focus); framed.setFocusConf(s->rear.confidence); @@ -1679,10 +1675,8 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { } void cameras_run(MultiCameraState *s) { - pthread_t ops_thread_handle; - int err = pthread_create(&ops_thread_handle, NULL, ops_thread, s); - assert(err == 0); std::vector threads; + threads.push_back(std::thread(ops_thread, s)); 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)); @@ -1778,9 +1772,6 @@ void cameras_run(MultiCameraState *s) { LOG(" ************** STOPPING **************"); - err = pthread_join(ops_thread_handle, NULL); - assert(err == 0); - for (auto &t : threads) t.join(); cameras_close(s); diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 90c4496103..d5e633097d 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -1070,8 +1070,7 @@ void camera_autoexposure(CameraState *s, float grey_frac) { cam_exp[s->camera_num].store(tmp); } -static void* ae_thread(void* arg) { - MultiCameraState *s = (MultiCameraState*)arg; +static void ae_thread(MultiCameraState *s) { CameraState *c_handles[3] = {&s->wide, &s->rear, &s->front}; int op_id_last[3] = {0}; @@ -1090,8 +1089,6 @@ static void* ae_thread(void* arg) { util::sleep_for(50); } - - return NULL; } void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) { @@ -1106,7 +1103,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { auto framed = c == &s->rear ? msg.initEvent().initFrame() : msg.initEvent().initWideFrame(); fill_frame_data(framed, b->cur_frame_data, cnt); if ((c == &s->rear && env_send_rear) || (c == &s->wide && env_send_wide)) { - fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride); + fill_frame_image(framed, b); } if (c == &s->rear) { framed.setTransform(b->yuv_transform.v); @@ -1114,35 +1111,16 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) { s->pm->send(c == &s->rear ? "frame" : "wideFrame", msg); if (cnt % 3 == 0) { - int exposure_x; - int exposure_y; - int exposure_width; - int exposure_height; - if (c == &s->rear) { - exposure_x = 96; - exposure_y = 160; - exposure_width = 1734; - exposure_height = 986; - } else { // c == &s->wide - exposure_x = 96; - exposure_y = 250; - exposure_width = 1734; - exposure_height = 524; - } - int skip = 2; - set_exposure_target(c, (const uint8_t *)b->cur_yuv_buf->y, exposure_x, exposure_x + exposure_width, skip, exposure_y, exposure_y + exposure_height, skip); + const auto [x, y, w, h] = (c == &s->wide) ? 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) { - int err; - // start threads LOG("-- Starting threads"); - pthread_t ae_thread_handle; - err = pthread_create(&ae_thread_handle, NULL, - ae_thread, s); - assert(err == 0); std::vector threads; + threads.push_back(std::thread(ae_thread, s)); 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(start_process_thread(s, "wideview", &s->wide, camera_process_frame)); @@ -1193,9 +1171,6 @@ void cameras_run(MultiCameraState *s) { LOG(" ************** STOPPING **************"); - err = pthread_join(ae_thread_handle, NULL); - assert(err == 0); - for (auto &t : threads) t.join(); cameras_close(s); diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 46d33f58eb..74c8563c09 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -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}, diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 29d5db5a9e..2584618ebd 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/honda/values.py b/selfdrive/car/honda/values.py index 412c96ea67..d9790c681a 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -453,6 +453,7 @@ FW_VERSIONS = { 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', diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 4c1dba1099..a9001ca8d5 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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.steeringAngle) < CS.CP.maxSteerAngle # fix for Genesis hard fault at low speed if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5d936ead54..3ba983ddd8 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.maxSteerAngle = 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 @@ -58,6 +65,8 @@ class CarInterface(CarInterfaceBase): 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.3], [0.05]] + if eps_modified: + ret.maxSteerAngle = 1000. elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]: ret.lateralTuning.pid.kf = 0.00006 ret.mass = 1275. + STD_CARGO_KG diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 67a824bab9..131b9e77fc 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -35,7 +35,7 @@ class CAR: 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" @@ -140,7 +140,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 +160,17 @@ 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 ', diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7c51c82cf8..cf587754af 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -53,7 +53,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.indi.timeConstantV = [1.0] ret.lateralTuning.indi.actuatorEffectivenessBP = [0.] ret.lateralTuning.indi.actuatorEffectivenessV = [1.0] - ret.steerActuatorDelay = 0.5 + ret.steerActuatorDelay = 0.3 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 12f83a12c0..7778bc5c9f 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -298,23 +298,34 @@ IGNORED_FINGERPRINTS = [CAR.RAV4H_TSS2, CAR.HIGHLANDERH_TSS2, CAR.LEXUS_RXH_TSS2 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: { @@ -685,6 +696,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', diff --git a/selfdrive/common/clutil.h b/selfdrive/common/clutil.h index 793d6e7f35..71523c2734 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/framebuffer.cc b/selfdrive/common/framebuffer.cc index 30ef7ae8ea..97f710e137 100644 --- a/selfdrive/common/framebuffer.cc +++ b/selfdrive/common/framebuffer.cc @@ -32,22 +32,22 @@ struct FramebufferState { EGLContext context; }; -extern "C" void framebuffer_swap(FramebufferState *s) { +void framebuffer_swap(FramebufferState *s) { 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(FramebufferState *s, int mode) { SurfaceComposerClient::setDisplayPowerMode(s->dtoken, mode); } -extern "C" FramebufferState* framebuffer_init( +FramebufferState* framebuffer_init( const char* name, int32_t layer, int alpha, int *out_w, int *out_h) { status_t status; diff --git a/selfdrive/common/framebuffer.h b/selfdrive/common/framebuffer.h index 45053bbb14..a722b80c95 100644 --- a/selfdrive/common/framebuffer.h +++ b/selfdrive/common/framebuffer.h @@ -1,9 +1,5 @@ #pragma once -#ifdef __cplusplus -extern "C" { -#endif - typedef struct FramebufferState FramebufferState; FramebufferState* framebuffer_init( @@ -39,7 +35,3 @@ enum { * functionality. */ HWC_POWER_MODE_DOZE_SUSPEND = 3, }; - -#ifdef __cplusplus -} -#endif diff --git a/selfdrive/common/gpio.h b/selfdrive/common/gpio.h index 479b4f7e06..f501342690 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 9946b40f07..8d41b2b388 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/swaglog.cc b/selfdrive/common/swaglog.cc index 9d90347547..c6bf5fa2c6 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -6,7 +6,7 @@ #include #include -#include +#include #include #include "json11.hpp" @@ -17,7 +17,7 @@ #include "swaglog.h" typedef struct LogState { - pthread_mutex_t lock; + std::mutex lock; bool inited; json11::Json::object ctx_j; void *zctx; @@ -25,9 +25,7 @@ typedef struct LogState { int print_level; } LogState; -static LogState s = { - .lock = PTHREAD_MUTEX_INITIALIZER, -}; +static LogState s = {}; static void cloudlog_bind_locked(const char* k, const char* v) { s.ctx_j[k] = v; @@ -65,7 +63,7 @@ static void cloudlog_init() { void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) { - pthread_mutex_lock(&s.lock); + std::lock_guard lk(s.lock); cloudlog_init(); char* msg_buf = NULL; @@ -75,7 +73,6 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func va_end(args); if (!msg_buf) { - pthread_mutex_unlock(&s.lock); return; } @@ -101,12 +98,10 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func 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 3a828ed49b..fae44152da 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/util.h b/selfdrive/common/util.h index 96b3d6d5e1..2797cbd44a 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -96,6 +96,12 @@ inline std::string getenv_default(const char* env_var, const char * suffix, cons 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 { diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a09ba171df..930e8606fd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -52,8 +52,10 @@ 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', 'frontFrame'] if SIMULATION else None + self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'ubloxRaw', + 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', + 'frame', 'frontFrame'], ignore_alive=ignore) self.can_sock = can_sock if can_sock is None: @@ -198,7 +200,7 @@ class Controls: 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: + 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 @@ -210,10 +212,6 @@ class Controls: 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: @@ -228,11 +226,19 @@ class Controls: self.events.add(EventName.relayMalfunction) if self.sm['plan'].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): + # 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(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL): + self.events.add(EventName.cameraMalfunction) + if self.sm['model'].frameDropPerc > 20: + self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \ @@ -388,8 +394,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 path_plan.dPathPoints[0] < -0.1 + right_deviation = actuators.steer < 0 and path_plan.dPathPoints[0] > 0.1 if left_deviation or right_deviation: self.events.add(EventName.steerSaturated) @@ -431,8 +437,8 @@ class Controls: 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['model'].leftLane.poly[3] < (1.08 - CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (self.sm['model'].rightLane.poly[3] > -(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) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ea7002475f..ee6c6665c5 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -7,11 +7,12 @@ V_CRUISE_MAX = 144 V_CRUISE_MIN = 8 V_CRUISE_DELTA = 8 V_CRUISE_ENABLE_MIN = 40 +MPC_N = 16 +CAR_ROTATION_RADIUS = 1.5 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 4b54e41867..2d6c0be68f 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -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: { diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 789ef8571f..d57801cbf2 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -3,103 +3,81 @@ import numpy as np from cereal import log CAMERA_OFFSET = 0.06 # m from center car to camera +TRAJECTORY_SIZE = 33 -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 - + 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 - - 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() + path_from_left_lane = self.lll_y + clipped_lane_width / 2.0 + path_from_right_lane = self.rll_y - clipped_lane_width / 2.0 + + 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/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript index e609c914b1..15dbeb19b5 100644 --- a/selfdrive/controls/lib/lateral_mpc/SConscript +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -1,6 +1,7 @@ Import('env', 'arch') cpp_path = [ + "#selfdrive", "#phonelibs/acado/include", "#phonelibs/acado/include/acado", "#phonelibs/qpoases/INCLUDE", diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 5f4a9a28df..dbf56f24a3 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,34 @@ int main( ) DifferentialState xx; // x position DifferentialState yy; // y position DifferentialState psi; // vehicle heading - DifferentialState delta; + DifferentialState tire_angle; 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 tire_angle_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 * tire_angle *curvature_factor); + f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * tire_angle *curvature_factor); + f << dot(psi) == v_ego * tire_angle * curvature_factor; + f << dot(tire_angle) == tire_angle_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 + 1.0 ) * psi; // Angular rate error - h << (v_ref + 1.0 ) * t; + h << (v_ego + 1.0 ) * tire_angle_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 +55,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 + 1.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 +78,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) <= tire_angle <= deg2rad(50)); + ocp.setNOD(3); 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 4972dbc3ff..84f81a9fc3 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,31 @@ 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 curvature_factor, 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+1] = v_ego; + acadoVariables.od[i+2] = rotation_radius; + } + for (i = 0; i < N; i+= 1){ + acadoVariables.y[NY*i + 0] = target_y[i]; + acadoVariables.y[NY*i + 1] = (v_ego + 1.0) * target_psi[i]; + acadoVariables.y[NY*i + 2] = 0.0; } + acadoVariables.yN[0] = target_y[N]; + acadoVariables.yN[1] = (2.0 * v_ego + 1.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 +96,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 f069b62c06..38c20f6a60 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 3 /** 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 3 (row major format) * - * Matrix containing 21 online data vectors. + * Matrix containing 17 online data vectors. */ -real_t od[ 357 ]; +real_t od[ 51 ]; -/** 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: 28 */ +real_t rk_xxx[ 28 ]; /** 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: 28 */ +real_t state[ 28 ]; -/** Column vector of size: 80 */ -real_t d[ 80 ]; - -/** Column vector of size: 100 */ -real_t Dy[ 100 ]; +/** Column vector of size: 64 */ +real_t d[ 64 ]; -/** Column vector of size: 4 */ -real_t DyN[ 4 ]; +/** Column vector of size: 48 */ +real_t Dy[ 48 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t evGx[ 320 ]; +/** Column vector of size: 2 */ +real_t DyN[ 2 ]; -/** Column vector of size: 80 */ -real_t evGu[ 80 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t evGx[ 256 ]; -/** Column vector of size: 11 */ -real_t objAuxVar[ 11 ]; +/** Column vector of size: 64 */ +real_t evGu[ 64 ]; -/** Row vector of size: 22 */ -real_t objValueIn[ 22 ]; +/** Row vector of size: 8 */ +real_t objValueIn[ 8 ]; -/** Row vector of size: 30 */ -real_t objValueOut[ 30 ]; +/** Row vector of size: 18 */ +real_t objValueOut[ 18 ]; -/** Matrix of size: 80 x 4 (row major format) */ -real_t Q1[ 320 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t Q1[ 256 ]; -/** Matrix of size: 80 x 5 (row major format) */ -real_t Q2[ 400 ]; +/** Matrix of size: 64 x 3 (row major format) */ +real_t Q2[ 192 ]; -/** Column vector of size: 20 */ -real_t R1[ 20 ]; +/** Column vector of size: 16 */ +real_t R1[ 16 ]; -/** Matrix of size: 20 x 5 (row major format) */ -real_t R2[ 100 ]; +/** Matrix of size: 16 x 3 (row major format) */ +real_t R2[ 48 ]; -/** 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 ]; +/** Matrix of size: 64 x 4 (row major format) */ +real_t QGx[ 256 ]; -/** Column vector of size: 80 */ -real_t Qd[ 80 ]; +/** Column vector of size: 64 */ +real_t Qd[ 64 ]; -/** Column vector of size: 84 */ -real_t QDy[ 84 ]; +/** Column vector of size: 68 */ +real_t QDy[ 68 ]; -/** Matrix of size: 20 x 4 (row major format) */ -real_t H10[ 80 ]; +/** Matrix of size: 16 x 4 (row major format) */ +real_t H10[ 64 ]; -/** Matrix of size: 24 x 24 (row major format) */ -real_t H[ 576 ]; +/** Matrix of size: 20 x 20 (row major format) */ +real_t H[ 400 ]; -/** Matrix of size: 40 x 24 (row major format) */ -real_t A[ 960 ]; +/** Matrix of size: 32 x 20 (row major format) */ +real_t A[ 640 ]; -/** Column vector of size: 24 */ -real_t g[ 24 ]; +/** Column vector of size: 20 */ +real_t g[ 20 ]; -/** Column vector of size: 24 */ -real_t lb[ 24 ]; +/** Column vector of size: 20 */ +real_t lb[ 20 ]; -/** Column vector of size: 24 */ -real_t ub[ 24 ]; +/** Column vector of size: 20 */ +real_t ub[ 20 ]; -/** Column vector of size: 40 */ -real_t lbA[ 40 ]; +/** Column vector of size: 32 */ +real_t lbA[ 32 ]; -/** Column vector of size: 40 */ -real_t ubA[ 40 ]; +/** Column vector of size: 32 */ +real_t ubA[ 32 ]; -/** Column vector of size: 24 */ -real_t x[ 24 ]; +/** Column vector of size: 20 */ +real_t x[ 20 ]; -/** Column vector of size: 64 */ -real_t y[ 64 ]; +/** 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 5df7cb47f5..7b8fc9e0cf 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,38 +25,52 @@ 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[0] = ((od[1]*a[0])-((od[2]*a[1])*((od[1]*xd[3])*od[0]))); +out[1] = ((od[1]*a[2])+((od[2]*a[3])*((od[1]*xd[3])*od[0]))); out[2] = ((od[1]*xd[3])*od[0]); 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[4] = ((od[1]*a[5])-(((od[2]*a[7])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[16])*od[0])))); +out[5] = ((od[1]*a[8])-(((od[2]*a[9])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[17])*od[0])))); +out[6] = ((od[1]*a[10])-(((od[2]*a[11])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[18])*od[0])))); +out[7] = ((od[1]*a[12])-(((od[2]*a[13])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[19])*od[0])))); +out[8] = ((od[1]*a[15])+(((od[2]*a[17])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[16])*od[0])))); +out[9] = ((od[1]*a[18])+(((od[2]*a[19])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[17])*od[0])))); +out[10] = ((od[1]*a[20])+(((od[2]*a[21])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[18])*od[0])))); +out[11] = ((od[1]*a[22])+(((od[2]*a[23])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[19])*od[0])))); out[12] = ((od[1]*xd[16])*od[0]); out[13] = ((od[1]*xd[17])*od[0]); out[14] = ((od[1]*xd[18])*od[0]); @@ -65,19 +79,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[20] = ((od[1]*a[24])-(((od[2]*a[25])*((od[1]*xd[3])*od[0]))+((od[2]*a[1])*((od[1]*xd[23])*od[0])))); +out[21] = ((od[1]*a[26])+(((od[2]*a[27])*((od[1]*xd[3])*od[0]))+((od[2]*a[3])*((od[1]*xd[23])*od[0])))); out[22] = ((od[1]*xd[23])*od[0]); 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; @@ -104,20 +118,6 @@ 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 +147,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 f2665a45c0..86b5f1548a 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 47d41855e6..5b003f08bf 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 347e77fa73..e84bd102b7 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,9 @@ 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 * 3]; +acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 3 + 2]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -98,253 +84,151 @@ 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[1]+(real_t)(1.0000000000000000e+00))*xd[2]); +out[2] = ((od[1]+(real_t)(1.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[1]+(real_t)(1.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[1]+(real_t)(1.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[1])+(real_t)(1.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[1])+(real_t)(1.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 * 3]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 3 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3 + 2]; 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[48]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; 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 +298,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 +334,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 +377,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 +397,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 +436,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 +665,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 +680,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 +802,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 +836,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 +854,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 +870,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 +885,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 +899,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 +912,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 +924,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 +935,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 +945,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 +954,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 +962,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 +969,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 +1006,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 +1072,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 +1089,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 +1105,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 +1120,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 +1134,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 +1147,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 +1159,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 +1170,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 +1180,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 +1189,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 +1197,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 +1204,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 +1239,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 +1255,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 +1270,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 +1284,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 +1297,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 +1309,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 +1320,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 +1330,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 +1339,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 +1347,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 +1354,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 +1388,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 +1403,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 +1417,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 +1430,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 +1442,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 +1453,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 +1463,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 +1472,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 +1480,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 +1487,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 +1520,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 +1534,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 +1547,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 +1559,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 +1570,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 +1580,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 +1589,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 +1597,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 +1604,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 +1636,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 +1649,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 +1661,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 +1672,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 +1682,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 +1691,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 +1699,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 +1706,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 +1737,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 +1749,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 +1760,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 +1770,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 +1779,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 +1787,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 +1794,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 +1824,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 +1835,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 +1845,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 +1854,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 +1862,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 +1869,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 +1898,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 +1908,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 +1917,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 +1925,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 +1932,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 +1960,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 +1969,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 +1977,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 +1984,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 +2011,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 +2019,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 +2026,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 +2052,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 +2059,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 +2084,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 +2264,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 +2345,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 +2363,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 +2379,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 +2394,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 +2408,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 +2421,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 +2433,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 +2444,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 +2454,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 +2463,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 +2471,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 +2478,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 +2515,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 +2531,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 +2606,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 ]) ); - -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]; +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[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 +2712,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 +2735,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 +2750,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 +2764,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 +2777,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 +2789,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 +2800,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 +2810,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 +2819,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 +2827,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 +2834,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 +2960,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 +2986,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 +3051,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 +3188,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 +3228,16 @@ 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 * 3]; +acadoWorkspace.state[26] = acadoVariables.od[index * 3 + 1]; +acadoWorkspace.state[27] = acadoVariables.od[index * 3 + 2]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -4973,7 +3251,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 +3261,49 @@ 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]; - -acado_integrate(acadoWorkspace.state, 1, 19); - -acadoVariables.x[80] = acadoWorkspace.state[0]; -acadoVariables.x[81] = acadoWorkspace.state[1]; -acadoVariables.x[82] = acadoWorkspace.state[2]; -acadoVariables.x[83] = acadoWorkspace.state[3]; +acadoWorkspace.state[25] = acadoVariables.od[48]; +acadoWorkspace.state[26] = acadoVariables.od[49]; +acadoWorkspace.state[27] = acadoVariables.od[50]; + +acado_integrate(acadoWorkspace.state, 1, 15); + +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 +3314,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 +3324,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 +3340,50 @@ 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 * 3]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 3 + 2]; 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[48]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[49]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; 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 9fc3a6c40b..c7a0bc9fbd 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, tire_angle, tire_angle_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 tire_angle[N+1]; + double tire_angle_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 curvature_factor, double rotation_radius, + double target_y[N+1], double target_psi[N+1]); """) libmpc = ffi.dlopen(libmpc_fn) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index a58eae0054..c3f239704e 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,10 +1,12 @@ 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 -from selfdrive.controls.lib.lane_planner import LanePlanner +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 @@ -40,13 +42,6 @@ DESIRES = { } -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() @@ -63,40 +58,46 @@ class PathPlanner(): self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False + self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) + self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) + self.t_idxs = np.arange(TRAJECTORY_SIZE) + 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.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].delta = 0.0 + self.cur_state[0].tire_angle = 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 + steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset + steering_wheel_angle_deg = sm['carState'].steeringAngle + measured_tire_angle = -math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR - 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']) + + 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 @@ -161,36 +162,50 @@ class PathPlanner(): # 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) + 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) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed + assert len(y_pts) == MPC_N + 1 + assert len(heading_pts) == MPC_N + 1 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) + float(v_ego_mpc), + curvature_factor, + 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.tire_angle = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) + + # TODO this needs more thought, use .2s extra for now to estimate other delays + delay = CP.steerActuatorDelay + .2 + next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle) + next_tire_angle_rate = interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.tire_angle_rate) # 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) + tire_angle_desired = next_tire_angle + desired_tire_angle_rate = next_tire_angle_rate else: - delta_desired = math.radians(angle_steers - angle_offset) / VM.sR - rate_desired = 0.0 - - self.cur_state[0].delta = delta_desired + tire_angle_desired = measured_tire_angle + desired_tire_angle_rate = 0.0 - self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) + # negative sign, controls uses different convention + self.desired_steering_wheel_angle_deg = -float(math.degrees(tire_angle_desired * VM.sR)) + steering_wheel_angle_offset_deg + self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_tire_angle_rate * VM.sR)) # Check for infeasable MPC solution - mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) + mpc_nans = any(math.isnan(x) for x in self.mpc_solution.tire_angle) 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 + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.cur_state.tire_angle = measured_tire_angle if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t @@ -201,18 +216,16 @@ class PathPlanner(): 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.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2']) 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.dPathPoints = [float(x) for x in y_pts] + plan_send.pathPlan.lProb = float(self.LP.lll_prob) + plan_send.pathPlan.rProb = float(self.LP.rll_prob) + plan_send.pathPlan.dProb = float(self.LP.d_prob) + + plan_send.pathPlan.angleSteers = float(self.desired_steering_wheel_angle_deg) + plan_send.pathPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg) plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) @@ -228,6 +241,6 @@ class PathPlanner(): 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.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/planner.py b/selfdrive/controls/lib/planner.py index 2567b46067..7b1d4fc852 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -186,7 +186,7 @@ class Planner(): plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) - plan_send.plan.mdMonoTime = sm.logMonoTime['model'] + plan_send.plan.mdMonoTime = sm.logMonoTime['modelV2'] plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState'] # longitudal plan diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 1f19266f1b..f8e584914a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,8 +23,8 @@ def plannerd_thread(sm=None, pm=None): 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']) @@ -37,7 +37,7 @@ def plannerd_thread(sm=None, pm=None): while True: sm.update() - if sm.updated['model']: + if sm.updated['modelV2']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 278afe18f0..1c088d79e4 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -3,74 +3,62 @@ import numpy as np from selfdrive.car.honda.interface import CarInterface from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS -def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., - l_prob=1., r_prob=1., p_prob=1., - poly_l=np.array([0., 0., 0., 1.8]), poly_r=np.array([0., 0., 0., -1.8]), poly_p=np.array([0., 0., 0., 0.]), +def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., tire_angle_init=0., lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc - libmpc.init(1.0, 3.0, 1.0, 1.0) + libmpc.init(1.0, 1.0, 1.0) mpc_solution = libmpc_py.ffi.new("log_t *") - p_l = poly_l.copy() - p_l[3] += poly_shift - - p_r = poly_r.copy() - p_r[3] += poly_shift - - p_p = poly_p.copy() - p_p[3] += poly_shift - - d_poly = p_p + y_pts = poly_shift * np.ones(MPC_N + 1) + heading_pts = np.zeros(MPC_N + 1) CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) curvature_factor = VM.curvature_factor(v_ref) - l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l))) - r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r))) - d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly))) - cur_state = libmpc_py.ffi.new("state_t *") - cur_state[0].x = x_init - cur_state[0].y = y_init - cur_state[0].psi = psi_init - cur_state[0].delta = delta_init + cur_state.x = x_init + cur_state.y = y_init + cur_state.psi = psi_init + cur_state.tire_angle = tire_angle_init # converge in no more than 20 iterations for _ in range(20): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob, - curvature_factor, v_ref, lane_width) + libmpc.run_mpc(cur_state, mpc_solution, v_ref, + curvature_factor, CAR_ROTATION_RADIUS, + list(y_pts), list(heading_pts)) return mpc_solution class TestLateralMpc(unittest.TestCase): - def _assert_null(self, sol, delta=1e-6): + def _assert_null(self, sol, tire_angle=1e-6): for i in range(len(sol[0].y)): - self.assertAlmostEqual(sol[0].y[i], 0., delta=delta) - self.assertAlmostEqual(sol[0].psi[i], 0., delta=delta) - self.assertAlmostEqual(sol[0].delta[i], 0., delta=delta) + self.assertAlmostEqual(sol[0].y[i], 0., delta=tire_angle) + self.assertAlmostEqual(sol[0].psi[i], 0., delta=tire_angle) + self.assertAlmostEqual(sol[0].tire_angle[i], 0., delta=tire_angle) - def _assert_simmetry(self, sol, delta=1e-6): + def _assert_simmetry(self, sol, tire_angle=1e-6): for i in range(len(sol[0][0].y)): - self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=delta) - self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=delta) - self.assertAlmostEqual(sol[0][0].delta[i], -sol[1][0].delta[i], delta=delta) - self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].tire_angle[i], -sol[1][0].tire_angle[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle) - def _assert_identity(self, sol, ignore_y=False, delta=1e-6): + def _assert_identity(self, sol, ignore_y=False, tire_angle=1e-6): for i in range(len(sol[0][0].y)): - self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=delta) - self.assertAlmostEqual(sol[0][0].delta[i], sol[1][0].delta[i], delta=delta) - self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta) + self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].tire_angle[i], sol[1][0].tire_angle[i], delta=tire_angle) + self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle) if not ignore_y: - self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=delta) + self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=tire_angle) def test_straight(self): sol = run_mpc() @@ -88,10 +76,10 @@ class TestLateralMpc(unittest.TestCase): sol.append(run_mpc(poly_shift=poly_shift)) self._assert_simmetry(sol) - def test_delta_symmetry(self): + def test_tire_angle_symmetry(self): sol = [] - for delta_init in [-0.1, 0.1]: - sol.append(run_mpc(delta_init=delta_init)) + for tire_angle_init in [-0.1, 0.1]: + sol.append(run_mpc(tire_angle_init=tire_angle_init)) self._assert_simmetry(sol) def test_psi_symmetry(self): @@ -100,21 +88,14 @@ class TestLateralMpc(unittest.TestCase): sol.append(run_mpc(psi_init=psi_init)) self._assert_simmetry(sol) - def test_prob_symmetry(self): - sol = [] - lane_width = 3. - for r_prob in [0., 1.]: - sol.append(run_mpc(r_prob=r_prob, l_prob=1.-r_prob, lane_width=lane_width)) - self._assert_simmetry(sol) - def test_y_shift_vs_poly_shift(self): shift = 1. sol = [] sol.append(run_mpc(y_init=shift)) sol.append(run_mpc(poly_shift=-shift)) - # need larger delta than standard, otherwise it false triggers. + # need larger tire_angle than standard, otherwise it false triggers. # this is acceptable because the 2 cases are very different from the optimizer standpoint - self._assert_identity(sol, ignore_y=True, delta=1e-5) + self._assert_identity(sol, ignore_y=True, tire_angle=1e-5) def test_no_overshoot(self): y_init = 1. diff --git a/selfdrive/debug/internal/can_replay.py b/selfdrive/debug/internal/can_replay.py new file mode 100755 index 0000000000..67ef9fcc20 --- /dev/null +++ b/selfdrive/debug/internal/can_replay.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +import os +import time +from multiprocessing import Process +from tqdm import tqdm + +os.environ['TESTING_CLOSET'] = '1' +os.environ['FILEREADER_CACHE'] = '1' + +from common.realtime import config_realtime_process, Ratekeeper +from selfdrive.boardd.boardd import can_capnp_to_can_list +from selfdrive.pandad import set_panda_power +from tools.lib.logreader import LogReader + +from panda import Panda +try: + from panda_jungle import PandaJungle # pylint: disable=import-error +except Exception: + PandaJungle = None # type: ignore + +ROUTE = "77611a1fac303767/2020-03-24--09-50-38" +NUM_SEGS = 2 # route has 82 segments available + +print("Loading log...") +CAN_MSGS = [] +for i in tqdm(list(range(1, NUM_SEGS+1))): + log_url = f"https://commadataci.blob.core.windows.net/openpilotci/{ROUTE}/{i}/rlog.bz2" + lr = LogReader(log_url) + CAN_MSGS += [can_capnp_to_can_list(m.can) for m in lr if m.which() == 'can'] + +def send_thread(sender, core): + config_realtime_process(core, 55) + + if "Jungle" in str(type(sender)): + for i in [0, 1, 2, 3, 0xFFFF]: + sender.can_clear(i) + sender.set_ignition(False) + time.sleep(5) + sender.set_ignition(True) + sender.set_panda_power(True) + else: + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + sender.set_can_loopback(False) + + log_idx = 0 + rk = Ratekeeper(100) + while True: + snd = CAN_MSGS[log_idx] + log_idx = (log_idx + 1) % len(CAN_MSGS) + snd = list(filter(lambda x: x[-1] <= 2, snd)) + sender.can_send_many(snd) + + # Drain panda message buffer + sender.can_recv() + rk.keep_time() + +def connect(): + serials = {} + while True: + # look for new devices + for p in [Panda, PandaJungle]: + if p is None: + continue + + for s in p.list(): + if s not in serials: + print("starting send thread for", s) + serials[s] = Process(target=send_thread, args=(p(s), 3)) + serials[s].start() + + # try to join all send procs + cur_serials = serials.copy() + for s, p in cur_serials.items(): + p.join(0.01) + if p.exitcode is not None: + del serials[s] + + time.sleep(1) + +if __name__ == "__main__": + set_panda_power(False) + time.sleep(1) + + if "FLASH" in os.environ and PandaJungle is not None: + for s in PandaJungle.list(): + PandaJungle(s).flash() + + while True: + try: + connect() + except Exception: + pass diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py index 558cdcf836..b1cd7102e5 100755 --- a/selfdrive/debug/mpc/live_lateral_mpc.py +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -75,7 +75,7 @@ def mpc_vwr_thread(addr="127.0.0.1"): r_path_y = np.polyval(l_poly, path_x) if pp is not None: - p_path_y = np.polyval(pp.pathPlan.dPoly, path_x) + p_path_y = np.polyval(pp.pathPlan.dPolyDEPRECATED, path_x) lineP.set_xdata(p_path_y) lineP.set_ydata(path_x) diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py index 5f551ebd0a..6fa9606279 100755 --- a/selfdrive/debug/mpc/test_mpc_wobble.py +++ b/selfdrive/debug/mpc/test_mpc_wobble.py @@ -2,11 +2,11 @@ # type: ignore import matplotlib.pyplot as plt from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS import math libmpc = libmpc_py.libmpc -libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.) +libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.) cur_state = libmpc_py.ffi.new("state_t *") cur_state[0].x = 0.0 @@ -24,30 +24,15 @@ times = [] curvature_factor = 0.3 v_ref = 1.0 * 20.12 # 45 mph -LANE_WIDTH = 3.7 -p = [0.0, 0.0, 0.0, 0.0] -p_l = p[:] -p_l[3] += LANE_WIDTH / 2.0 - -p_r = p[:] -p_r[3] -= LANE_WIDTH / 2.0 - -l_poly = libmpc_py.ffi.new("double[4]", p_l) -r_poly = libmpc_py.ffi.new("double[4]", p_r) -p_poly = libmpc_py.ffi.new("double[4]", p) - -l_prob = 1.0 -r_prob = 1.0 -p_prob = 1.0 - for i in range(1): cur_state[0].delta = math.radians(510. / 13.) - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) + libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref], + curvature_factor, CAR_ROTATION_RADIUS, + [0.0]*MPC_N, [0.0]*MPC_N) timesi = [] ct = 0 -for i in range(21): +for i in range(MPC_N + 1): timesi.append(ct) if i <= 4: ct += 0.05 diff --git a/selfdrive/debug/profiling/clpeak/.gitignore b/selfdrive/debug/profiling/clpeak/.gitignore new file mode 100644 index 0000000000..d575c207c1 --- /dev/null +++ b/selfdrive/debug/profiling/clpeak/.gitignore @@ -0,0 +1 @@ +clpeak/ diff --git a/selfdrive/debug/profiling/clpeak/build.sh b/selfdrive/debug/profiling/clpeak/build.sh new file mode 100755 index 0000000000..1dcb05cbbc --- /dev/null +++ b/selfdrive/debug/profiling/clpeak/build.sh @@ -0,0 +1,22 @@ +#!/usr/bin/bash + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +cd $DIR + +if [ ! -d "$DIR/clpeak" ]; then + git clone https://github.com/krrishnarraj/clpeak.git + + cd clpeak + git fetch + git checkout ec2d3e70e1abc7738b81f9277c7af79d89b2133b + git reset --hard origin/master + git submodule update --init --recursive --remote + + git apply ../run_continuously.patch +fi + +cd clpeak +mkdir build || true +cd build +cmake .. +cmake --build . diff --git a/selfdrive/debug/profiling/clpeak/clpeak3 b/selfdrive/debug/profiling/clpeak/clpeak3 new file mode 100755 index 0000000000..4b3c4c130e Binary files /dev/null and b/selfdrive/debug/profiling/clpeak/clpeak3 differ diff --git a/selfdrive/debug/profiling/clpeak/run_continuously.patch b/selfdrive/debug/profiling/clpeak/run_continuously.patch new file mode 100644 index 0000000000..075c65bd2a --- /dev/null +++ b/selfdrive/debug/profiling/clpeak/run_continuously.patch @@ -0,0 +1,13 @@ +diff --git a/src/clpeak.cpp b/src/clpeak.cpp +index 8cb192b..b6fe6f5 100644 +--- a/src/clpeak.cpp ++++ b/src/clpeak.cpp +@@ -47,7 +47,7 @@ int clPeak::runAll() + + log->xmlOpenTag("clpeak"); + log->xmlAppendAttribs("os", OS_NAME); +- for (size_t p = 0; p < platforms.size(); p++) ++ for (size_t p = 0; p < platforms.size(); (p+1 % platforms.size())) + { + if (forcePlatform && (p != specifiedPlatform)) + continue; diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 9e6bf72ab1..b597406bac 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,7 +1,9 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'gpucommon') -src = ['loggerd.cc', 'logger.cc'] -libs = ['zmq', 'capnp', 'kj', 'z', + +logger_lib = env.Library('logger', ["logger.cc"]) +src = ['loggerd.cc'] +libs = [logger_lib, 'zmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', 'OpenCL', common, cereal, messaging, visionipc] @@ -22,3 +24,4 @@ if arch == "Darwin": 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 0000000000..0da3872848 --- /dev/null +++ b/selfdrive/loggerd/bootlog.cc @@ -0,0 +1,37 @@ +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" +#include "logger.h" +#include "messaging.hpp" + +int main(int argc, char** argv) { + LoggerState logger; + logger_init(&logger, "bootlog", false); + + char segment_path[4096]; + int err = logger_next(&logger, LOG_ROOT.c_str(), segment_path, sizeof(segment_path), nullptr); + assert(err == 0); + LOGW("bootlog to %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(&logger, bytes.begin(), bytes.size(), false); + + logger_close(&logger); + return 0; +} diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc index 0cec20f680..4ac496d2d3 100644 --- a/selfdrive/loggerd/logger.cc +++ b/selfdrive/loggerd/logger.cc @@ -12,12 +12,99 @@ #include #include +#include +#include +#include +#ifdef QCOM +#include +#endif #include "messaging.hpp" #include "common/swaglog.h" - +#include "common/params.h" +#include "common/util.h" +#include "common/version.h" #include "logger.h" +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))); +} + +void log_init_data(LoggerState *s) { + 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++; + } + } + + auto bytes = msg.toBytes(); + 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(); @@ -43,15 +130,9 @@ 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) { +void logger_init(LoggerState *s, const char* log_name, 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; - } - + umask(0); pthread_mutex_init(&s->lock, NULL); @@ -110,20 +191,10 @@ static LoggerHandle* logger_open(LoggerState *s, const char* root_path) { 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; - } - } - + pthread_mutex_init(&h->lock, NULL); h->refcnt++; + log_init_data(s); return h; fail: LOGE("logger failed to open files"); @@ -203,7 +274,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); } diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h index ab8798481c..d845d19742 100644 --- a/selfdrive/loggerd/logger.h +++ b/selfdrive/loggerd/logger.h @@ -1,13 +1,16 @@ -#ifndef LOGGER_H -#define LOGGER_H +#pragma once #include #include #include #include +#include +#include -#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 @@ -28,10 +31,6 @@ typedef struct LoggerHandle { typedef struct LoggerState { pthread_mutex_t lock; - - uint8_t* init_data; - size_t init_data_len; - int part; char route_name[64]; char log_name[64]; @@ -41,7 +40,7 @@ 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); +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 +50,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 36af8dd46f..c8348e3e2a 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -10,9 +10,7 @@ #include #include -#include -#include -#include + #include #include #include @@ -20,11 +18,6 @@ #include #include -#ifdef QCOM -#include -#endif - -#include "common/version.h" #include "common/timing.h" #include "common/params.h" #include "common/swaglog.h" @@ -107,11 +100,6 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = { namespace { constexpr int SEGMENT_LENGTH = 60; -#if defined(QCOM) || defined(QCOM2) -std::string LOG_ROOT = "/data/media/0/realdata"; -#else -std::string LOG_ROOT = util::getenv_default("HOME", "/.comma/media/0/realdata", "/data/media/0/realdata"); -#endif double randrange(double a, double b) __attribute__((unused)); double randrange(double a, double b) { @@ -123,11 +111,6 @@ double randrange(double a, double b) { ExitHandler do_exit; -static bool file_exists(const std::string& fn) { - std::ifstream f(fn); - return f.good(); -} - class RotateState { public: SubSocket* fpkt_sock; @@ -328,82 +311,6 @@ void encoder_thread(int cam_idx) { } -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 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) { const char* dot = strrchr(fpath, '.'); if (dot && strcmp(dot, ".lock") == 0) { @@ -416,60 +323,15 @@ static 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.c_str(), 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); -} - int main(int argc, char** argv) { setpriority(PRIO_PROCESS, 0, -12); - 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(); // setup messaging @@ -500,11 +362,7 @@ int main(int argc, char** argv) { } // init logger - { - auto words = gen_init_data(); - auto bytes = words.asBytes(); - logger_init(&s.logger, "rlog", bytes.begin(), bytes.size(), true); - } + logger_init(&s.logger, "rlog", true); // init encoders pthread_mutex_init(&s.rotate_lock, NULL); @@ -515,6 +373,7 @@ int main(int argc, char** argv) { s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true; #if defined(QCOM) || defined(QCOM2) + bool record_front = Params().read_db_bool("RecordFront"); if (record_front) { encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_DCAMERA)); s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true; @@ -555,7 +414,10 @@ int main(int argc, char** argv) { } 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) { @@ -622,20 +484,12 @@ int main(int argc, char** argv) { 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 encoders for (auto &r : s.rotate_state) r.rotate(); pthread_mutex_unlock(&s.rotate_lock); } - - 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)); - } } LOGW("closing encoders"); diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc index c3533a2849..4bda785e89 100644 --- a/selfdrive/loggerd/omx_encoder.cc +++ b/selfdrive/loggerd/omx_encoder.cc @@ -23,6 +23,11 @@ #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) // ***** OMX callback functions ***** @@ -159,8 +164,6 @@ static const char* omx_color_fomat_name(uint32_t format) { // ***** encoder functions ***** OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) { - int err; - this->filename = filename; this->width = width; this->height = height; @@ -182,7 +185,7 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int } auto component = (OMX_STRING)(h265 ? "OMX.qcom.video.encoder.hevc" : "OMX.qcom.video.encoder.avc"); - err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks); + int err = OMX_GetHandle(&this->handle, component, this, &omx_callbacks); if (err != OMX_ErrorNone) { LOGE("error getting codec: %x", err); } @@ -194,9 +197,7 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int OMX_PARAM_PORTDEFINITIONTYPE in_port = {0}; in_port.nSize = sizeof(in_port); in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN; - err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); + 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; @@ -209,24 +210,16 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar; in_port.format.video.eColorFormat = (OMX_COLOR_FORMATTYPE)QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m; - err = OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); - - - err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &in_port); - assert(err == OMX_ErrorNone); - this->num_in_bufs = in_port.nBufferCountActual; + 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; - err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR)&out_port); - assert(err == OMX_ErrorNone); + 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; @@ -238,28 +231,19 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int } out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused; - err = OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &out_port); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamPortDefinition, (OMX_PTR) &out_port)); - err = OMX_GetParameter(this->handle, OMX_IndexParamPortDefinition, - (OMX_PTR) &out_port); - assert(err == OMX_ErrorNone); - this->num_out_bufs = out_port.nBufferCountActual; + 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; - err = OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, - (OMX_PTR) &bitrate_type); - assert(err == OMX_ErrorNone); - + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); bitrate_type.eControlRate = OMX_Video_ControlRateVariable; bitrate_type.nTargetBitrate = bitrate; - err = OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, - (OMX_PTR) &bitrate_type); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoBitrate, (OMX_PTR) &bitrate_type)); if (h265) { // setup HEVC @@ -272,23 +256,18 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int #endif hevc_type.nSize = sizeof(hevc_type); hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT; - err = OMX_GetParameter(this->handle, index_type, - (OMX_PTR) &hevc_type); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type)); hevc_type.eProfile = OMX_VIDEO_HEVCProfileMain; hevc_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5; - err = OMX_SetParameter(this->handle, index_type, - (OMX_PTR) &hevc_type); - assert(err == OMX_ErrorNone); + 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; - err = OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_GetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); avc.nBFrames = 0; avc.nPFrames = 15; @@ -299,8 +278,7 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB; avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable; - err = OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SetParameter(this->handle, OMX_IndexParamVideoAvc, &avc)); } @@ -326,40 +304,33 @@ OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int // printf("profile %d level 0x%x\n", params.eProfile, params.eLevel); // } - err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); - this->in_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(this->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_in_bufs; i++) { - err = OMX_AllocateBuffer(this->handle, &this->in_buf_headers[i], PORT_INDEX_IN, this, - in_port.nBufferSize); - assert(err == OMX_ErrorNone); + for (auto &buf : this->in_buf_headers) { + OMX_CHECK(OMX_AllocateBuffer(this->handle, &buf, PORT_INDEX_IN, this, + in_port.nBufferSize)); } - this->out_buf_headers = (OMX_BUFFERHEADERTYPE **)calloc(this->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*)); - for (int i=0; inum_out_bufs; i++) { - err = OMX_AllocateBuffer(this->handle, &this->out_buf_headers[i], PORT_INDEX_OUT, this, - out_port.nBufferSize); - assert(err == OMX_ErrorNone); + 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); - err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL)); wait_for_state(OMX_StateExecuting); // give omx all the output buffers - for (int i = 0; i < this->num_out_bufs; i++) { + for (auto &buf : this->out_buf_headers) { // printf("fill %p\n", this->out_buf_headers[i]); - err = OMX_FillThisBuffer(this->handle, this->out_buf_headers[i]); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_FillThisBuffer(this->handle, buf)); } // fill the input free queue - for (int i = 0; i < this->num_in_bufs; i++) { - queue_push(&this->free_in, (void*)this->in_buf_headers[i]); + for (auto &buf : this->in_buf_headers) { + queue_push(&this->free_in, (void*)buf); } } @@ -430,8 +401,7 @@ void OmxEncoder::handle_out_buf(OmxEncoder *e, OMX_BUFFERHEADERTYPE *out_buf) { out_buf->nTimeStamp = 0; } #endif - err = OMX_FillThisBuffer(e->handle, out_buf); - assert(err == OMX_ErrorNone); + 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, @@ -491,8 +461,7 @@ int OmxEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const u in_buf->nTimeStamp = extra->timestamp_eof/1000LL; // OMX_TICKS, in microseconds this->last_t = in_buf->nTimeStamp; - err = OMX_EmptyThisBuffer(this->handle, in_buf); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); // pump output while (true) { @@ -572,8 +541,6 @@ void OmxEncoder::encoder_open(const char* path, int segment) { } void OmxEncoder::encoder_close() { - int err; - pthread_mutex_lock(&this->lock); if (this->is_open) { @@ -586,8 +553,7 @@ void OmxEncoder::encoder_close() { in_buf->nFlags = OMX_BUFFERFLAG_EOS; in_buf->nTimeStamp = this->last_t + 1000000LL/this->fps; - err = OMX_EmptyThisBuffer(this->handle, in_buf); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_EmptyThisBuffer(this->handle, in_buf)); while (true) { OMX_BUFFERHEADERTYPE *out_buf = (OMX_BUFFERHEADERTYPE *)queue_pop(&this->done_out); @@ -617,34 +583,25 @@ void OmxEncoder::encoder_close() { } OmxEncoder::~OmxEncoder() { - int err; - assert(!this->is_open); - err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateIdle, NULL)); wait_for_state(OMX_StateIdle); - err = OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_SendCommand(this->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL)); - for (int i=0; inum_in_bufs; i++) { - err = OMX_FreeBuffer(this->handle, PORT_INDEX_IN, this->in_buf_headers[i]); - assert(err == OMX_ErrorNone); + for (auto &buf : this->in_buf_headers) { + OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_IN, buf)); } - free(this->in_buf_headers); - for (int i=0; inum_out_bufs; i++) { - err = OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, this->out_buf_headers[i]); - assert(err == OMX_ErrorNone); + for (auto &buf : this->out_buf_headers) { + OMX_CHECK(OMX_FreeBuffer(this->handle, PORT_INDEX_OUT, buf)); } - free(this->out_buf_headers); wait_for_state(OMX_StateLoaded); - err = OMX_FreeHandle(this->handle); - assert(err == OMX_ErrorNone); + OMX_CHECK(OMX_FreeHandle(this->handle)); while (queue_try_pop(&this->free_in)); while (queue_try_pop(&this->done_out)); diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h index b8fd37928a..00b7b57ded 100644 --- a/selfdrive/loggerd/omx_encoder.h +++ b/selfdrive/loggerd/omx_encoder.h @@ -5,6 +5,7 @@ #include #include +#include #include extern "C" { @@ -60,11 +61,8 @@ private: OMX_HANDLETYPE handle; - int num_in_bufs; - OMX_BUFFERHEADERTYPE** in_buf_headers; - - int num_out_bufs; - OMX_BUFFERHEADERTYPE** out_buf_headers; + std::vector in_buf_headers; + std::vector out_buf_headers; uint64_t last_t; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 870d9c3a72..84cf97270d 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -47,7 +47,7 @@ class TestLoggerd(unittest.TestCase): def _gen_bootlog(self): with Timeout(5): - out = subprocess.check_output(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"), encoding='utf-8') + out = subprocess.check_output("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"), encoding='utf-8') # check existence d = self._get_log_dir(out) diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 3874fbf0ea..1e84cbd073 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -58,6 +58,7 @@ class Uploader(): self.last_resp = None self.last_exc = None + self.immediate_folders = ["crash/"] self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1} self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2, "ecamera.hevc": 3} @@ -98,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: diff --git a/selfdrive/manager.py b/selfdrive/manager.py index f576f2bbc2..80d7c388a9 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -440,7 +440,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: diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 7104eb7a12..c1bcf88667 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -64,7 +64,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ 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 + 32; - const int crop_y_offset = 0; + const int crop_y_offset = -196; #endif int resized_width = MODEL_WIDTH; @@ -75,16 +75,16 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ 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 { 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, diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index ae4b7f9e90..91c09e0b86 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,6 +10,11 @@ #include "driving.h" #include "clutil.h" +constexpr int MODEL_PATH_DISTANCE = 192; +constexpr int POLYFIT_DEGREE = 4; +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; @@ -28,8 +33,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; @@ -49,8 +52,6 @@ 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); @@ -77,8 +78,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { // 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); } } diff --git a/selfdrive/registration.py b/selfdrive/registration.py index e2e77d9f41..d74cc091a4 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -26,7 +26,10 @@ def register(spinner=None): params.put("GitRemote", get_git_remote(default="")) params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) - needs_registration = False + 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 @@ -65,11 +68,15 @@ def register(spinner=None): cloudlog.exception("Error getting imei, trying again...") time.sleep(1) + 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=HARDWARE.get_serial(), public_key=public_key, register_token=register_token) + 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) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index f401226dbe..504256b252 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -110,10 +110,11 @@ class Plant(): self.rate = rate if not Plant.messaging_initialized: + + Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw', 'modelV2']) Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') Plant.model = messaging.pub_sock('model') - Plant.front_frame = messaging.pub_sock('frontFrame') Plant.live_params = messaging.pub_sock('liveParameters') Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.health = messaging.pub_sock('health') @@ -163,7 +164,6 @@ class Plant(): def close(self): Plant.logcan.close() Plant.model.close() - Plant.front_frame.close() Plant.live_params.close() Plant.live_location_kalman.close() @@ -394,7 +394,6 @@ class Plant(): if publish_model and self.frame % 5 == 0: md = messaging.new_message('model') cal = messaging.new_message('liveCalibration') - fp = messaging.new_message('frontFrame') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0]*50 @@ -426,7 +425,11 @@ class Plant(): # fake values? Plant.model.send(md.to_bytes()) Plant.cal.send(cal.to_bytes()) - Plant.front_frame.send(fp.to_bytes()) + for s in Plant.pm.sock.keys(): + try: + Plant.pm.send(s, messaging.new_message(s)) + except Exception: + Plant.pm.send(s, messaging.new_message(s, 1)) Plant.logcan.send(can_list_to_can_capnp(can_msgs)) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 60f886e307..a41d0db3af 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -318,6 +318,7 @@ def setup_output(): class LongitudinalControl(unittest.TestCase): @classmethod def setUpClass(cls): + os.environ['SIMULATION'] = "1" os.environ['SKIP_FW_QUERY'] = "1" os.environ['NO_CAN_TIMEOUT'] = "1" diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index f3aa093bdc..210a5899cb 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -852c79998828975cce184114537b0067b80bc608 +4d71a89ccbfd351cbe58fcf217ee2eefa48eee2d diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 15a6c4b2ae..9f0c9f273a 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -222,7 +222,7 @@ CONFIGS = [ pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [], - "model": [], "frontFrame": [], + "model": [], "frontFrame": [], "frame": [], "ubloxRaw": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, @@ -243,7 +243,7 @@ CONFIGS = [ ProcessConfig( proc_name="plannerd", pub_sub={ - "model": ["pathPlan"], "radarState": ["plan"], + "modelV2": ["pathPlan"], "radarState": ["plan"], "carState": [], "controlsState": [], "liveParameters": [], }, ignore=["logMonoTime", "valid", "plan.processingDelay"], diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7c1824d6bb..026bc9742c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -196c58580ec9cfd9691a9c4ebaad2dcd01cc0777 \ No newline at end of file +9ffa2b9927f188bdc07be62b2cc4ee00e5632522 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 5856da23b7..bb5a550ad3 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -16,12 +16,12 @@ segments = [ ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) ("TOYOTA", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) ("HONDA", "0982d79ebb0de295|2021-01-08--10-13-10--6"), # HONDA.CIVIC (NIDEC) + ("HONDA", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"), # HONDA.ACCORD (BOSCH) + ("CHRYSLER", "52d86230ee29aa84|2021-01-10--17-16-34--30"), # CHRYSLER.PACIFICA + ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA + ("GM", "ae3ed0eb20960a20|2021-01-15--15-04-06--8"), # GM.VOLT # TODO: update this route - ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA - ("HONDA", "d83f36766f8012a5|2020-02-05--18-42-21--2"), # HONDA.CIVIC_BOSCH_DIESEL (BOSCH) - ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA - ("SUBARU", "c321c6b697c5a5ff|2020-06-23--11-04-33--12"), # SUBARU.FORESTER ("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL @@ -160,6 +160,9 @@ if __name__ == "__main__": if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ (not procs_whitelisted and cfg.proc_name in args.blacklist_procs): continue + # TODO remove this hack + if cfg.proc_name == 'plannerd' and car_brand in ["GM", "SUBARU", "VOLKSWAGEN", "NISSAN"]: + continue cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs) diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py deleted file mode 100755 index 9e472dadae..0000000000 --- a/selfdrive/test/test_cpu_usage.py +++ /dev/null @@ -1,108 +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') - - # wait until everything's started - msg = None - start_time = time.monotonic() - while msg is None and time.monotonic() - start_time < 240: - msg = messaging.recv_sock(cs_sock) - - # 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_onroad.py b/selfdrive/test/test_onroad.py new file mode 100755 index 0000000000..a605613636 --- /dev/null +++ b/selfdrive/test/test_onroad.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +import os +import time +import subprocess +import unittest +from pathlib import Path + +import cereal.messaging as messaging +from common.basedir import BASEDIR +from common.timeout import Timeout +from selfdrive.loggerd.config import ROOT +from selfdrive.test.helpers import set_params_enabled +from tools.lib.logreader import LogReader + +PROCS = [ + ("selfdrive.controls.controlsd", 47.0), + ("./loggerd", 45.0), + ("selfdrive.locationd.locationd", 35.0), + ("selfdrive.controls.plannerd", 20.0), + ("selfdrive.locationd.paramsd", 12.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), +] + +# ***** test helpers ***** + +def cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + +def check_cpu_usage(first_proc, last_proc): + result = "------------------------------------------------\n" + result += "------------------ CPU Usage -------------------\n" + result += "------------------------------------------------\n" + + r = True + dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9 + for proc_name, normal_cpu_usage in PROCS: + first, last = None, None + 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 {first=} {last=}\n" + r = False + result += "------------------------------------------------\n" + print(result) + return r + + +class TestOnroad(unittest.TestCase): + + @classmethod + def setUpClass(cls): + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" + set_params_enabled() + + initial_segments = set(Path(ROOT).iterdir()) + + # start manager and run openpilot for a minute + try: + manager_path = os.path.join(BASEDIR, "selfdrive/manager.py") + proc = subprocess.Popen(["python", manager_path]) + + sm = messaging.SubMaster(['carState']) + with Timeout(60, "controls didn't start"): + while not sm.updated['carState']: + sm.update(1000) + + time.sleep(60) + finally: + proc.terminate() + if proc.wait(20) is None: + proc.kill() + + new_segments = set(Path(ROOT).iterdir()) - initial_segments + + segments = [p for p in new_segments if len(list(p.iterdir())) > 1] + cls.segment = [s for s in segments if str(s).endswith("--0")][0] + cls.lr = list(LogReader(os.path.join(str(cls.segment), "rlog.bz2"))) + + def test_cpu_usage(self): + proclogs = [m for m in self.lr if m.which() == 'procLog'] + cpu_ok = check_cpu_usage(proclogs[5], proclogs[-3]) + self.assertTrue(cpu_ok) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/test/testing_closet_client.py b/selfdrive/test/testing_closet_client.py deleted file mode 100755 index 3c4e52b9d4..0000000000 --- a/selfdrive/test/testing_closet_client.py +++ /dev/null @@ -1,86 +0,0 @@ -#!/usr/bin/env python3 -import errno -import fcntl -import os -import signal -import subprocess -import sys -import time - -import requests - -from common.params import Params -from common.timeout import Timeout - -HOST = "testing.comma.life" - - -def unblock_stdout(): - # get a non-blocking stdout - child_pid, child_pty = os.forkpty() - if child_pid != 0: # parent - - # child is in its own process group, manually pass kill signals - signal.signal(signal.SIGINT, lambda signum, frame: os.kill(child_pid, signal.SIGINT)) - signal.signal(signal.SIGTERM, lambda signum, frame: os.kill(child_pid, signal.SIGTERM)) - - fcntl.fcntl(sys.stdout, fcntl.F_SETFL, fcntl.fcntl(sys.stdout, fcntl.F_GETFL) | os.O_NONBLOCK) - - while True: - try: - dat = os.read(child_pty, 4096) - except OSError as e: - if e.errno == errno.EIO: - break - continue - - if not dat: - break - - try: - sys.stdout.write(dat.decode('utf8')) - except (OSError, IOError, UnicodeDecodeError): - pass - - # os.wait() returns a tuple with the pid and a 16 bit value - # whose low byte is the signal number and whose high byte is the exit satus - exit_status = os.wait()[1] >> 8 - os._exit(exit_status) - - -def heartbeat(): - work_dir = '/data/openpilot' - - while True: - try: - with open(os.path.join(work_dir, "selfdrive", "common", "version.h")) as _versionf: - version = _versionf.read().split('"')[1] - - tmux = "" - - # try: - # tmux = os.popen('tail -n 100 /tmp/tmux_out').read() - # except Exception: - # pass - - params = Params() - msg = { - 'version': version, - 'dongle_id': params.get("DongleId").rstrip().decode('utf8'), - 'remote': subprocess.check_output(["git", "config", "--get", "remote.origin.url"], cwd=work_dir).decode('utf8').rstrip(), - 'branch': subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=work_dir).decode('utf8').rstrip(), - 'revision': subprocess.check_output(["git", "rev-parse", "HEAD"], cwd=work_dir).decode('utf8').rstrip(), - 'serial': subprocess.check_output(["getprop", "ro.boot.serialno"]).decode('utf8').rstrip(), - 'tmux': tmux, - } - with Timeout(10): - requests.post('http://%s/eon/heartbeat/' % HOST, json=msg, timeout=5.0) - except Exception as e: - print("Unable to send heartbeat", e) - - time.sleep(5) - - -if __name__ == "__main__": - unblock_stdout() - heartbeat() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 72acc1de75..83866297d7 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -276,7 +276,6 @@ def thermald_thread(): # If device is offroad we want to cool down before going onroad # since going onroad increases load and can make temps go over 107 # We only do this if there is a relay that prevents the car from faulting - thermal_status = ThermalStatus.green # default to good condition is_offroad_for_5_min = (started_ts is None) and ((not started_seen) or (off_ts is None) or (sec_since_boot() - off_ts > 60 * 5)) if max_cpu_temp > 107. or bat_temp >= 63. or (is_offroad_for_5_min and max_cpu_temp > 70.0): # onroad not allowed @@ -293,6 +292,8 @@ def thermald_thread(): elif max_cpu_temp > 75.0: # hysteresis between uploader not allowed and all good thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow) + else: + thermal_status = ThermalStatus.green # default to good condition # **** starting logic **** diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 1989dbd0c9..8d23ed022d 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -1,23 +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 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 version, origin, branch, dirty +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 * 10 # Allow larger size for 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: @@ -30,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...") @@ -39,41 +86,100 @@ 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(crash_function.split(' ')[2:]) + 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)) + os.remove(fn) def main(): + # TODO: turn on when all tombstones are recovered + # clear_apport_folder() # Clear apport folder on start, otherwise duplicate crashes won't register initial_tombstones = set(get_tombstones()) tags = { @@ -91,7 +197,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 656913b945..1c532837bb 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -5,7 +5,6 @@ Import('env', 'qt_env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] libs = [common, 'zmq', 'capnp', 'kj', 'm', 'OpenCL', cereal, messaging, gpucommon, visionipc] - if qt_env is None: libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', 'OpenCL'] @@ -20,16 +19,18 @@ else: widgets = qt_env.Library("qt_widgets", ["qt/qt_window.cc", "qt/qt_sound.cc", "qt/widgets/keyboard.cc", "qt/widgets/input_field.cc", "qt/widgets/drive_stats.cc", - "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc", "qt/widgets/offroad_alerts.cc"], + "qt/offroad/wifi.cc", "qt/offroad/wifiManager.cc", "qt/widgets/toggle.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc", "qt/widgets/QrCode.cc"], LIBS=qt_libs) qt_libs.append(widgets) + if arch == "Darwin": # fix OpenCL del qt_libs[qt_libs.index('OpenCL')] qt_env['FRAMEWORKS'] += ['OpenCL'] - qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc"] + src + qt_src = ["qt/ui.cc", "qt/window.cc", "qt/home.cc", "qt/api.cc", "qt/offroad/settings.cc", "qt/offroad/onboarding.cc"] + src + qt_env.Program("_ui", qt_src, LIBS=qt_libs) # spinner and text window diff --git a/selfdrive/ui/android/text/Makefile b/selfdrive/ui/android/text/Makefile index 3401cae0ff..c83a7a0519 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 87% rename from selfdrive/ui/android/text/text.c rename to selfdrive/ui/android/text/text.cc index 43c43e4689..e6987325ae 100644 --- a/selfdrive/ui/android/text/text.c +++ b/selfdrive/ui/android/text/text.cc @@ -21,8 +21,9 @@ #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; @@ -36,8 +37,8 @@ int main(int argc, char** argv) { 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); @@ -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; diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 5b714069a8..88549a9eb7 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -48,27 +48,30 @@ bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_ 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]; + 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) { - vertex_data out; - if (!car_space_to_full_frame(s, x_in, y_in, 0.0, &out)) return; + vertex_data out = {}; + car_space_to_full_frame(s, x_in, y_in, 0.0, &out); auto [x, y] = out; - sz = std::clamp((sz * 30) / (x_in / 3 + 30), 15.0f, 30.0f); + sz = std::clamp((sz * 30) / (x_in / 3 + 30), 15.0f, 30.0f) * zoom; + y = std::fmin(s->scene.viz_rect.bottom() - sz * .6, y); + x = std::clamp(x, 0.f, s->scene.viz_rect.right() - sz / 2); // glow float g_xo = sz/5; @@ -91,20 +94,19 @@ 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, int x, int 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); - const Rect rect = {x - (img_size / 2), img_y ? img_y : y - (size / 4), img_size, img_size}; - ui_draw_image(vg, rect, 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, int x, int 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){ @@ -195,22 +197,9 @@ static void ui_draw_vision_lane_lines(UIState *s) { // 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]); - // Draw lane edges and vision/mpc tracks ui_draw_vision_lane_lines(s); @@ -223,7 +212,7 @@ static void ui_draw_world(UIState *s) { draw_lead(s, scene->lead_data[1]); } } - nvgRestore(s->vg); + nvgResetScissor(s->vg); } static void ui_draw_vision_maxspeed(UIState *s) { @@ -231,18 +220,17 @@ static void ui_draw_vision_maxspeed(UIState *s) { const bool is_cruise_set = maxspeed != 0 && maxspeed != SET_SPEED_NA; if (is_cruise_set && !s->is_metric) { maxspeed *= 0.6225; } - auto [x, y, w, h] = std::tuple(s->scene.viz_rect.x + (bdr_s * 2), s->scene.viz_rect.y + (bdr_s * 1.5), 184, 202); - - ui_draw_rect(s->vg, x, y, w, h, COLOR_BLACK_ALPHA(100), 30); - ui_draw_rect(s->vg, x, y, w, h, COLOR_WHITE_ALPHA(100), 20, 10); + const Rect rect = {s->scene.viz_rect.x + (bdr_s * 2), int(s->scene.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); - ui_draw_text(s->vg, x + w / 2, 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) { const std::string maxspeed_str = std::to_string((int)std::nearbyint(maxspeed)); - ui_draw_text(s->vg, x + w / 2, 242, maxspeed_str.c_str(), 48 * 2.5, COLOR_WHITE, s->font_sans_bold); + ui_draw_text(s, rect.centerX(), 242, maxspeed_str.c_str(), 48 * 2.5, COLOR_WHITE, "sans-bold"); } else { - ui_draw_text(s->vg, x + w / 2, 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"); } } @@ -250,8 +238,8 @@ static void ui_draw_vision_speed(UIState *s) { const float speed = std::max(0.0, s->scene.controls_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); - ui_draw_text(s->vg, s->scene.viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, s->font_sans_bold); - ui_draw_text(s->vg, s->scene.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->scene.viz_rect.centerX(), 240, speed_str.c_str(), 96 * 2.5, COLOR_WHITE, "sans-bold"); + ui_draw_text(s, s->scene.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) { @@ -262,7 +250,7 @@ static void ui_draw_vision_event(UIState *s) { // draw winding road sign const int img_turn_size = 160*1.5; const Rect rect = {viz_event_x - (img_turn_size / 4), viz_event_y + bdr_s - 25, img_turn_size, img_turn_size}; - ui_draw_image(s->vg, rect, s->img_turn, 1.0f); + ui_draw_image(s, rect, "trafficSign_turn", 1.0f); } else if (s->scene.controls_state.getEngageable()) { // draw steering wheel const int bg_wheel_size = 96; @@ -270,7 +258,7 @@ static void ui_draw_vision_event(UIState *s) { 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); } } @@ -278,7 +266,7 @@ 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()); + ui_draw_circle_image(s, face_x, face_y, face_size, "driver_face", s->scene.dmonitoring_state.getFaceDetected()); } static void ui_draw_driver_view(UIState *s) { @@ -294,32 +282,32 @@ static void ui_draw_driver_view(UIState *s) { 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_draw_rect(s->vg, blackout_x, rect.y, blackout_w, rect.h, gradient); - ui_draw_rect(s->vg, blackout_x, rect.y, blackout_w, rect.h, COLOR_BLACK_ALPHA(144)); + 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.x, rect.y, rect.w, rect.h, bg_colors[STATUS_OFFROAD], 0, 1); + ui_draw_rect(s->vg, rect, bg_colors[STATUS_OFFROAD], 1); const bool face_detected = s->scene.dmonitoring_state.getFaceDetected(); if (face_detected) { auto fxy_list = s->scene.driver_state.getFacePosition(); float face_x = fxy_list[0]; float face_y = fxy_list[1]; - float fbox_x = valid_rect.centerX() + (is_rhd ? face_x : -face_x) * valid_rect.w; - float fbox_y = valid_rect.centerY() + face_y * valid_rect.h; + 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; 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; - const float 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), 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 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->vg, icon_x, icon_y, face_size, s->img_face, face_detected); + ui_draw_circle_image(s, icon_x, icon_y, face_size, "driver_face", face_detected); } static void ui_draw_vision_header(UIState *s) { @@ -329,7 +317,7 @@ static void ui_draw_vision_header(UIState *s) { viz_rect.x, 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, {viz_rect.x, viz_rect.y, viz_rect.w, header_h}, gradient); ui_draw_vision_maxspeed(s); ui_draw_vision_speed(s); @@ -354,36 +342,33 @@ static void ui_draw_vision_alert(UIState *s) { NVGcolor color = bg_colors[s->status]; color.a *= get_alert_alpha(scene->alert_blinking_rate); - int alr_s = alert_size_map[scene->alert_size]; - - 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 + 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); + const int alr_h = alert_size_map[scene->alert_size] + bdr_s; + const Rect rect = {.x = scene->viz_rect.x - bdr_s, + .y = s->fb_h - alr_h, + .w = scene->viz_rect.w + (bdr_s * 2), + .h = alr_h}; - 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); } } @@ -431,8 +416,9 @@ void ui_draw(UIState *s) { s->scene.viz_rect.w -= sbr_w; } - const bool draw_vision = s->started && s->status != STATUS_OFFROAD && - s->active_app == cereal::UiLayoutState::App::NONE && s->vipc_client->connected; + const bool draw_alerts = s->started && s->status != STATUS_OFFROAD && + s->active_app == cereal::UiLayoutState::App::NONE; + const bool draw_vision = draw_alerts && s->vipc_client->connected; // GL drawing functions ui_draw_background(s); @@ -450,40 +436,42 @@ void ui_draw(UIState *s) { 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, const Rect &r, int image, float alpha){ - nvgBeginPath(vg); - NVGpaint imgPaint = nvgImagePattern(vg, r.x, r.y, r.w, r.h, 0, image, alpha); - nvgRect(vg, r.x, r.y, r.w, r.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 @@ -537,36 +525,38 @@ 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); - - 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 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); + } + + // 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 @@ -633,4 +623,17 @@ 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); + + // 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 7119579dee..7bab393a34 100644 --- a/selfdrive/ui/paint.hpp +++ b/selfdrive/ui/paint.hpp @@ -3,7 +3,8 @@ bool car_space_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, vertex_data *out, float margin=0.0); void ui_draw(UIState *s); -void ui_draw_image(NVGcontext *vg, const Rect &r, 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 0000000000..3d9653dd6d --- /dev/null +++ b/selfdrive/ui/qt/api.cc @@ -0,0 +1,127 @@ +#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) { + QJsonObject header; + header.insert("alg", "RS256"); + QJsonObject payload; + QString dongle_id = QString::fromStdString(Params().get("DongleId")); + 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) { + networkAccessManager = new QNetworkAccessManager(parent); + 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; + callId = QRandomGenerator::global()->bounded(1000); + 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 0000000000..4244c36324 --- /dev/null +++ b/selfdrive/ui/qt/api.hpp @@ -0,0 +1,47 @@ +#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 = 60); + 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; + int callId; + 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); +}; \ No newline at end of file diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 7ce4ac0bfd..d388490d64 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -1,15 +1,15 @@ #include -#include #include +#include #include -#include -#include +#include #include -#include -#include #include -#include +#include +#include +#include +#include #include "common/params.h" @@ -17,23 +17,23 @@ #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); // top header - QHBoxLayout *header_layout = new QHBoxLayout(); + 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); @@ -47,9 +47,19 @@ OffroadHome::OffroadHome(QWidget *parent) : QWidget(parent) { main_layout->addSpacing(25); center_layout = new QStackedLayout(); - DriveStats *drive = new DriveStats; + QHBoxLayout* statsAndSetup = new QHBoxLayout(); + + DriveStats* drive = new DriveStats; drive->setFixedSize(1000, 800); - center_layout->addWidget(drive); + 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())); @@ -113,14 +123,13 @@ void OffroadHome::refresh() { font-weight: bold; background-color: #E22C2C; )"); - if (alerts_widget->updateAvailable){ + 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); @@ -145,8 +154,8 @@ 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(); @@ -161,14 +170,13 @@ void HomeWindow::mousePressEvent(QMouseEvent *e) { } } - -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; } @@ -182,8 +190,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())); @@ -211,15 +218,12 @@ 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); + timer->start(1000 / UI_FREQ); backlight_timer->start(BACKLIGHT_DT * 1000); } @@ -227,11 +231,11 @@ 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; } @@ -239,19 +243,23 @@ void GLWindow::backlightUpdate() { } void GLWindow::timerUpdate() { - if (ui_state->started != onroad) { - onroad = ui_state->started; + // Connecting to visionIPC requires opengl to be current + if (!ui_state.vipc_client->connected){ + makeCurrent(); + } + + if (ui_state.started != onroad) { + onroad = ui_state.started; emit offroadTransition(!onroad); -#ifdef QCOM2 - timer->setInterval(onroad ? 0 : 1000); -#endif + + // 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); } - // Fix awake timeout if running 1 Hz when offroad - int dt = timer->interval() == 0 ? 1 : 20; - handle_display_state(ui_state, dt, false); + handle_display_state(&ui_state, false); - ui_update(ui_state); + ui_update(&ui_state); repaint(); } @@ -260,18 +268,18 @@ 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); - } + handle_display_state(&ui_state, true); } - FramebufferState* framebuffer_init(const char* name, int32_t layer, int alpha, int *out_w, int *out_h) { + *out_w = vwp_w; + *out_h = vwp_h; return (FramebufferState*)1; // not null } diff --git a/selfdrive/ui/qt/home.hpp b/selfdrive/ui/qt/home.hpp index 5f71e0e4f6..bdf0ded979 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,11 +20,11 @@ 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); @@ -36,8 +35,8 @@ protected: void paintGL() override; private: - QTimer *timer; - QTimer *backlight_timer; + QTimer* timer; + QTimer* backlight_timer; QtSound sound; @@ -58,42 +57,40 @@ 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 openSettings(); 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/widgets/QrCode.cc b/selfdrive/ui/qt/widgets/QrCode.cc new file mode 100644 index 0000000000..b9de86215e --- /dev/null +++ b/selfdrive/ui/qt/widgets/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/selfdrive/ui/qt/widgets/QrCode.hpp b/selfdrive/ui/qt/widgets/QrCode.hpp new file mode 100644 index 0000000000..7341e41029 --- /dev/null +++ b/selfdrive/ui/qt/widgets/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/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc index e9929083d6..9da32a4ace 100644 --- a/selfdrive/ui/qt/widgets/drive_stats.cc +++ b/selfdrive/ui/qt/widgets/drive_stats.cc @@ -1,27 +1,23 @@ #include #include -#include #include -#include -#include +#include #include #include -#include +#include #include -#include - -#include -#include -#include +#include +#include +#include -#include "drive_stats.hpp" +#include "api.hpp" #include "common/params.h" #include "common/util.h" +#include "drive_stats.hpp" +#include "home.hpp" - -constexpr double MILE_TO_KM = 1.60934; - +const double MILE_TO_KM = 1.60934; #if defined(QCOM) || defined(QCOM2) const std::string private_key_path = "/persist/comma/id_rsa"; @@ -29,128 +25,93 @@ const std::string private_key_path = "/persist/comma/id_rsa"; const std::string private_key_path = util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa"); #endif - -QByteArray rsa_sign(QByteArray data) { - auto file = QFile(private_key_path.c_str()); - bool r = file.open(QIODevice::ReadOnly); - assert(r); - - auto key = file.readAll(); - - 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 create_jwt(QString dongle_id, int expiry=3600) { - QJsonObject header; - header.insert("alg", "RS256"); - header.insert("typ", "JWT"); - - auto t = QDateTime::currentSecsSinceEpoch(); - QJsonObject payload; - payload.insert("identity", dongle_id); - payload.insert("nbf", t); - payload.insert("iat", t); - payload.insert("exp", t + expiry); - - QString jwt = - QJsonDocument(header).toJson(QJsonDocument::Compact).toBase64() + - '.' + - QJsonDocument(payload).toJson(QJsonDocument::Compact).toBase64(); - - auto hash = QCryptographicHash::hash(jwt.toUtf8(), QCryptographicHash::Sha256); - auto sig = rsa_sign(hash); - - jwt += '.' + sig.toBase64(); - - return jwt; +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; +QLayout* build_stat(QString name, int stat) { + QVBoxLayout* layout = new QVBoxLayout; - QLabel *metric = new QLabel(QString("%1").arg(stat)); + QLabel* metric = new QLabel(QString("%1").arg(stat)); metric->setStyleSheet(R"( font-size: 72px; font-weight: 700; )"); layout->addWidget(metric, 0, Qt::AlignLeft); - - QLabel *label = new QLabel(name); + + QLabel* label = new QLabel(name); label->setStyleSheet(R"( font-size: 32px; font-weight: 600; )"); layout->addWidget(label, 0, Qt::AlignLeft); - + return layout; } -void DriveStats::replyFinished(QNetworkReply *l) { - QString answer = l->readAll(); - answer.chop(1); +void DriveStats::parseError(QString response) { + clearLayouts(vlayout); + vlayout->addWidget(new QLabel("No internet connection")); +} - QJsonDocument doc = QJsonDocument::fromJson(answer.toUtf8()); +void DriveStats::parseResponse(QString response) { + response.chop(1); + clearLayouts(vlayout); + QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8()); if (doc.isNull()) { - qDebug() << "JSON Parse failed"; + qDebug() << "JSON Parse failed on getting past drives statistics"; + return; } + QString IsMetric = QString::fromStdString(Params().get("IsMetric")); - bool metric = (IsMetric =="1"); + bool metric = (IsMetric == "1"); QJsonObject json = doc.object(); auto all = json["all"].toObject(); auto week = json["week"].toObject(); - QGridLayout *gl = new QGridLayout(); + QGridLayout* gl = new QGridLayout(); - int all_distance = all["distance"].toDouble()*(metric ? MILE_TO_KM : 1); + 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); + 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); - setLayout(gl); + 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: 600; } )"); -} - -DriveStats::DriveStats(QWidget *parent) : QWidget(parent) { - QString dongle_id = QString::fromStdString(Params().get("DongleId")); - QString token = create_jwt(dongle_id); - - QNetworkAccessManager *manager = new QNetworkAccessManager(this); - connect(manager, &QNetworkAccessManager::finished, this, &DriveStats::replyFinished); - QNetworkRequest request; - request.setUrl(QUrl("https://api.commadotai.com/v1.1/devices/" + dongle_id + "/stats")); - request.setRawHeader("Authorization", ("JWT "+token).toUtf8()); + 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))); - manager->get(request); } diff --git a/selfdrive/ui/qt/widgets/drive_stats.hpp b/selfdrive/ui/qt/widgets/drive_stats.hpp index 991de7e2ce..bf4cdae000 100644 --- a/selfdrive/ui/qt/widgets/drive_stats.hpp +++ b/selfdrive/ui/qt/widgets/drive_stats.hpp @@ -1,15 +1,21 @@ #pragma once -#include #include +#include +#include +#include "api.hpp" class DriveStats : public QWidget { Q_OBJECT public: - explicit DriveStats(QWidget *parent = 0); + explicit DriveStats(QWidget* parent = 0); private: - void replyFinished(QNetworkReply *l); + QVBoxLayout* vlayout; + +private slots: + void parseError(QString response); + void parseResponse(QString response); }; diff --git a/selfdrive/ui/qt/widgets/setup.cc b/selfdrive/ui/qt/widgets/setup.cc new file mode 100644 index 0000000000..99596584ba --- /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(this); + QLabel* commaPrime = new QLabel("COMMA PRIME", this); + commaPrime->setStyleSheet(R"( + font-size: 60px; + )"); + mainLayout->addWidget(commaPrime); + + username = new QLabel("", this); + mainLayout->addWidget(username); + + mainLayout->addSpacing(200); + + QLabel* commaPoints = new QLabel("COMMA POINTS", this); + commaPoints->setStyleSheet(R"( + font-size: 60px; + color: #b8b8b8; + )"); + mainLayout->addWidget(commaPoints); + + points = new QLabel("", this); + 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(this); + + QLabel* upgradeNow = new QLabel("Upgrade now", this); + vlayout->addWidget(upgradeNow); + + QLabel* description = new QLabel("Become a comma prime member in the comma app and get premium features!", this); + 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, this); + feature->setStyleSheet(R"( + font-size: 40px; + )"); + + vlayout->addWidget(feature); + vlayout->addSpacing(15); + } + + setLayout(vlayout); +} + + + +SetupWidget::SetupWidget(QWidget* parent) : QWidget(parent) { + QVBoxLayout* backgroundLayout = new QVBoxLayout(this); + + backgroundLayout->addSpacing(100); + + QFrame* background = new QFrame(this); + + mainLayout = new QStackedLayout(this); + + QWidget* blankWidget = new QWidget(this); + mainLayout->addWidget(blankWidget); + + QWidget* finishRegistration = new QWidget(this); + + QVBoxLayout* finishRegistationLayout = new QVBoxLayout(this); + finishRegistationLayout->addSpacing(50); + QPushButton* finishButton = new QPushButton("Finish registration", this); + finishButton->setFixedHeight(200); + finishButton->setStyleSheet(R"( + border-radius: 30px; + font-size: 60px; + font-weight: bold; + background: #787878; + )"); + QObject::connect(finishButton, SIGNAL(released()), this, SLOT(showQrCode())); + finishRegistationLayout->addWidget(finishButton); + + QLabel* registrationDescription = new QLabel("Pair your device with comma connect app", this); + registrationDescription->setStyleSheet(R"( + font-size: 50px; + )"); + + registrationDescription->setWordWrap(true); + finishRegistationLayout->addWidget(registrationDescription); + + finishRegistration->setLayout(finishRegistationLayout); + mainLayout->addWidget(finishRegistration); + + QVBoxLayout* qrLayout = new QVBoxLayout(this); + + QLabel* qrLabel = new QLabel("Pair with Comma Connect app!", this); + qrLabel->setStyleSheet(R"( + font-size: 40px; + )"); + qrLayout->addWidget(qrLabel); + + qrLayout->addWidget(new PairingQRWidget(this)); + + QWidget* q = new QWidget(this); + q->setLayout(qrLayout); + mainLayout->addWidget(q); + + PrimeAdWidget* primeAd = new PrimeAdWidget(this); + mainLayout->addWidget(primeAd); + + PrimeUserWidget* primeUserWidget = new PrimeUserWidget(this); + mainLayout->addWidget(primeUserWidget); + + background->setLayout(mainLayout); + background->setStyleSheet(R"( + .QFrame { + border-radius: 40px; + padding: 60px; + } + )"); + 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 0000000000..0224ace560 --- /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/sidebar.cc b/selfdrive/ui/sidebar.cc index 1b2699fc79..4423abef39 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -2,7 +2,7 @@ #include #include #include - +#include "common/util.h" #include "paint.hpp" #include "sidebar.hpp" @@ -12,17 +12,17 @@ static void draw_background(UIState *s) { #else const NVGcolor color = nvgRGBA(0x39, 0x39, 0x39, 0xff); #endif - ui_draw_rect(s->vg, 0, 0, sbr_w, s->fb_h, color); + ui_fill_rect(s->vg, {0, 0, sbr_w, s->fb_h}, color); } 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, s->img_button_settings, alpha); + ui_draw_image(s, settings_btn, "button_settings", 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->vg, home_btn, s->img_button_home, alpha); + ui_draw_image(s, home_btn, "button_home", alpha); } static void draw_network_strength(UIState *s) { @@ -32,17 +32,16 @@ static void draw_network_strength(UIState *s) { {cereal::ThermalData::NetworkStrength::MODERATE, 3}, {cereal::ThermalData::NetworkStrength::GOOD, 4}, {cereal::ThermalData::NetworkStrength::GREAT, 5}}; - const Rect rect = {58, 196, 176, 27}; 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, rect, s->img_network[img_idx], 1.0f); + ui_draw_image(s, {58, 196, 176, 27}, util::string_format("network_%d", img_idx).c_str(), 1.0f); } static void draw_battery_icon(UIState *s) { - int battery_img = s->scene.thermal.getBatteryStatus() == "Charging" ? s->img_battery_charging : s->img_battery; + const char *battery_img = s->scene.thermal.getBatteryStatus() == "Charging" ? "battery_charging" : "battery"; const Rect rect = {160, 255, 76, 36}; - ui_draw_rect(s->vg, rect.x + 6, rect.y + 5, - ((rect.w - 19) * (s->scene.thermal.getBatteryPercent() * 0.01)), rect.h - 11, COLOR_WHITE); - ui_draw_image(s->vg, rect, battery_img, 1.0f); + ui_fill_rect(s->vg, {rect.x + 6, rect.y + 5, + int((rect.w - 19) * s->scene.thermal.getBatteryPercent() * 0.01), rect.h - 11}, COLOR_WHITE); + ui_draw_image(s, rect, battery_img, 1.0f); } static void draw_network_type(UIState *s) { @@ -59,17 +58,12 @@ static void draw_network_type(UIState *s) { const char *network_type = network_type_map[s->scene.thermal.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 draw_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; - NVGcolor status_color; if (severity == 0) { @@ -80,32 +74,32 @@ static void draw_metric(UIState *s, const char *label_str, const char *value_str 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); } } @@ -126,7 +120,7 @@ static void draw_panda_metric(UIState *s) { std::string panda_message = "VEHICLE\nONLINE"; if (s->scene.hwType == cereal::HealthData::HwType::UNKNOWN) { panda_severity = 2; - panda_message = "NO\nVEHICLE"; + panda_message = "NO\nPANDA"; } else if (s->started) { if (s->scene.satelliteCount < 6) { panda_severity = 1; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 23d16ef4ff..acbe4bb7cc 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -46,7 +46,6 @@ void ui_init(UIState *s) { 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); @@ -103,7 +102,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { update_line_data(s, model.getPosition(), 0.5, 0, &scene.track_vertices, path_length); } -void update_sockets(UIState *s) { +static void update_sockets(UIState *s) { UIScene &scene = s->scene; SubMaster &sm = *(s->sm); @@ -203,7 +202,7 @@ void update_sockets(UIState *s) { s->started = scene.thermal.getStarted() || scene.frontview; } -static void ui_read_params(UIState *s) { +static void update_params(UIState *s) { const uint64_t frame = s->sm->frame; if (frame % (5*UI_FREQ) == 0) { @@ -217,7 +216,7 @@ static void ui_read_params(UIState *s) { } } -void ui_update_vision(UIState *s) { +static void update_vision(UIState *s) { if (!s->vipc_client->connected && s->started) { s->vipc_client = s->scene.frontview ? s->vipc_client_front : s->vipc_client_rear; @@ -235,9 +234,9 @@ void ui_update_vision(UIState *s) { } void ui_update(UIState *s) { - ui_read_params(s); + update_params(s); update_sockets(s); - ui_update_vision(s); + update_vision(s); // Handle onroad/offroad transition if (!s->started && s->status != STATUS_OFFROAD) { @@ -255,7 +254,7 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::NONE; } - // Handle controls/fcamera timeout + // Handle controls 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) { // car is started, but controlsState hasn't been seen at all @@ -264,8 +263,7 @@ void ui_update(UIState *s) { s->scene.alert_size = cereal::ControlsState::AlertSize::MID; } else if (((s->sm)->frame - (s->sm)->rcv_frame("controlsState")) > 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 (s->scene.alert_text2 != "Controls Unresponsive") { s->sound->play(AudibleAlert::CHIME_WARNING_REPEAT); LOGE("Controls unresponsive"); } @@ -275,17 +273,5 @@ void ui_update(UIState *s) { s->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(); - } } } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 23045d46ed..1363cddf72 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -151,18 +151,8 @@ typedef struct UIState { // 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; @@ -189,6 +179,7 @@ typedef struct UIState { uint64_t started_frame; Rect video_rect; + float car_space_transform[6]; } UIState; void ui_init(UIState *s); diff --git a/selfdrive/version.py b/selfdrive/version.py index ef975f8940..b06841832b 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/tools/mac_setup.sh b/tools/mac_setup.sh index 3afc32c165..062b349f7d 100755 --- a/tools/mac_setup.sh +++ b/tools/mac_setup.sh @@ -1,24 +1,26 @@ #!/bin/bash -e -# Install brew if required. +# Install brew if required if [[ $(command -v brew) == "" ]]; then echo "Installing Hombrew" /bin/bash -c "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install.sh)" fi -brew install capnp \ - coreutils \ - eigen \ - ffmpeg \ - glfw \ - libarchive \ - libusb \ - libtool \ - llvm \ - openssl \ - pyenv \ - qt5 \ - zeromq +brew bundle --file=- <<-EOS +brew "capnp" +brew "coreutils" +brew "eigen" +brew "ffmpeg" +brew "glfw" +brew "libarchive" +brew "libusb" +brew "libtool" +brew "llvm" +brew "openssl" +brew "pyenv" +brew "qt5" +brew "zeromq" +EOS if [[ $SHELL == "/bin/zsh" ]]; then RC_FILE="$HOME/.zshrc" diff --git a/tools/nui/FileReader.cpp b/tools/nui/FileReader.cpp index fe360da98b..13b7f78078 100644 --- a/tools/nui/FileReader.cpp +++ b/tools/nui/FileReader.cpp @@ -74,6 +74,10 @@ LogReader::LogReader(const QString& file, Events *events_, QReadWriteLock* event }); } +LogReader::~LogReader() { + delete parser; +} + void LogReader::mergeEvents(int dled) { auto amsg = kj::arrayPtr((const capnp::word*)(raw.data() + event_offset), (dled-event_offset)/sizeof(capnp::word)); Events events_local; diff --git a/tools/nui/FileReader.hpp b/tools/nui/FileReader.hpp index 6d53bdee8e..514758e10f 100644 --- a/tools/nui/FileReader.hpp +++ b/tools/nui/FileReader.hpp @@ -1,5 +1,4 @@ -#ifndef FILEREADER_HPP -#define FILEREADER_HPP +#pragma once #include #include @@ -20,7 +19,8 @@ #include "channel.hpp" class FileReader : public QObject { -Q_OBJECT + Q_OBJECT + public: FileReader(const QString& file_); void startRequest(const QUrl &url); @@ -28,10 +28,13 @@ public: virtual void readyRead(); void httpFinished(); virtual void done() {}; + public slots: void process(); + protected: QNetworkReply *reply; + private: QNetworkAccessManager *qnam; QElapsedTimer timer; @@ -44,9 +47,12 @@ class LogReader : public FileReader { Q_OBJECT public: LogReader(const QString& file, Events *, QReadWriteLock* events_lock_, QMap > *eidx_); + ~LogReader(); + void readyRead(); void done() { is_done = true; }; bool is_done = false; + private: bz_stream bStream; @@ -63,6 +69,3 @@ private: QReadWriteLock* events_lock; QMap > *eidx; }; - -#endif - diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index fc88b3dc1d..4c432a8501 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -10,8 +10,6 @@ from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, tici_f_frame_size, tici_f_focal_length) from selfdrive.config import RADAR_TO_CAMERA from selfdrive.config import UIParams as UP -from selfdrive.controls.lib.lane_planner import (compute_path_pinv, - model_polyfit) from tools.lib.lazy_property import lazy_property RED = (255, 0, 0) @@ -23,8 +21,6 @@ WHITE = (255, 255, 255) _PATH_X = np.arange(192.) _PATH_XD = np.arange(192.) -_PATH_PINV = compute_path_pinv(50) - _FULL_FRAME_SIZE = { } @@ -247,14 +243,11 @@ def draw_var(y, x, var, color, img, calibration, top_down): class ModelPoly(object): def __init__(self, model_path): - if len(model_path.points) == 0 and len(model_path.poly) == 0: + if len(model_path.poly) == 0: self.valid = False return - if len(model_path.poly): - self.poly = np.array(model_path.poly) - else: - self.poly = model_polyfit(model_path.points, _PATH_PINV) + self.poly = np.array(model_path.poly) self.prob = model_path.prob self.std = model_path.std diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 0f38c27411..1f26072dd1 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -184,7 +184,7 @@ def ui_thread(addr, frame_address): if len(sm['model'].path.poly) > 0: model_data = extract_model_data(sm['model']) plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration, - top_down, np.array(sm['pathPlan'].dPoly)) + top_down, np.array(sm['pathPlan'].dPolyDEPRECATED)) # MPC if sm.updated['liveMpc']: diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index cb823c9a18..3b3d8e9e40 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -61,7 +61,7 @@ def cam_callback(image): dat = messaging.new_message('frame') dat.frame = { - "frameId": frame_id, # TODO: can we get frame ID from the CARLA camera? + "frameId": image.frame, "image": img.tostring(), "transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] } @@ -89,7 +89,8 @@ def health_function(): dat.health = { 'ignitionLine': True, 'hwType': "blackPanda", - 'controlsAllowed': True + 'controlsAllowed': True, + 'safetyModel': 'hondaNidec' } pm.send('health', dat) time.sleep(0.5) diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index c66bf94e20..0ae11102a4 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -44,6 +44,14 @@ def keyboard_poll_thread(q): q.put(str("cruise_down")) if c == '3': q.put(str("cruise_cancel")) + if c == 'w': + q.put(str("throttle_%f" % 1.0)) + if c == 'a': + q.put(str("steer_%f" % 0.15)) + if c == 's': + q.put(str("brake_%f" % 1.0)) + if c == 'd': + q.put(str("steer_%f" % -0.15)) if c == 'q': exit(0) diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index 2be3d0cc38..b165e3935d 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -16,4 +16,19 @@ if ! $(apt list --installed | grep -q nvidia-container-toolkit); then fi docker pull carlasim/carla:0.9.7 -docker run -it --net=host --gpus all carlasim/carla:0.9.7 + +if [ -z "$WINDOW" ]; then + docker run -it --net=host --gpus all carlasim/carla:0.9.7 +else + docker run --name openpilot_carla \ + --privileged --rm \ + --net=host \ + -e SDL_VIDEODRIVER=x11 \ + -e DISPLAY=$DISPLAY \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -it \ + --gpus all \ + carlasim/carla:0.9.7 \ + ./CarlaUE4.sh -quality-level=Medium +fi +