diff --git a/README.md b/README.md index f8de745a27186a..01ca0d824831b0 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ Welcome to openpilot [openpilot](http://github.com/commaai/openpilot) is an open source driving agent. -Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas and Acuras. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan). +Currently it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for Hondas, Acuras and Toyotas. It's about on par with Tesla Autopilot at launch, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan). The openpilot codebase has been written to be concise and enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier. @@ -29,13 +29,18 @@ Supported Cars - Honda CR-V Touring 2015-2016 - Can only be enabled above 25 mph -- Toyota RAV-4 2016+ with TSS-P (alpha!) +- Toyota RAV-4 2016+ non-hybrid with TSS-P - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) and can be enabled above 20 mph + - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can be enabled above 20 mph - Toyota Prius 2017 (alpha!) - By default it uses stock Toyota ACC for longitudinal control - - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) + - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Prius_.28for_openpilot.29) + - Lateral control needs improvements + +- Toyota RAV-4 2017 hybrid (alpha!) + - By default it uses stock Toyota ACC for longitudinal control + - openpilot longitudinal control available after unplugging the [Driving Support ECU](https://community.comma.ai/wiki/index.php/Toyota#Rav4_.28for_openpilot.29) and can do stop and go In Progress Cars ------ @@ -50,6 +55,12 @@ Community WIP Cars - [Classic Tesla Model S (pre-AP)](https://github.com/commaai/openpilot/pull/145) +- [Honda Odyssey 2018 with Honda Sensing](https://github.com/commaai/openpilot/pull/155) + +- [Honda Pilot 2017 with Honda Sensing](https://github.com/commaai/openpilot/pull/161) + +- [Acura RDX 2018 with AcuraWatch Plus](https://github.com/commaai/openpilot/pull/162) + Directory structure ------ diff --git a/RELEASES.md b/RELEASES.md index e4d234a06ac395..fbcabd13d3f586 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,12 @@ +Version 0.4.0.1 (2017-12-21) +========================== + * New UI to match chffrplus + * Improved lateral control tuning to fix oscillations on Civic + * Add alpha support for 2017 Toyota Rav4 Hybrid + * Reduced CPU usage + * Removed unnecessary utilization of fan at max speed + * Minor bug fixes + Version 0.3.9 (2017-11-21) ========================== * Add alpha support for 2017 Toyota Prius diff --git a/apk/ai.comma.plus.black.apk b/apk/ai.comma.plus.black.apk new file mode 100644 index 00000000000000..b9243077f351c7 Binary files /dev/null and b/apk/ai.comma.plus.black.apk differ diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk new file mode 100644 index 00000000000000..06fcf2a513f480 Binary files /dev/null and b/apk/ai.comma.plus.frame.apk differ diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk new file mode 100644 index 00000000000000..db04675cc8afbf Binary files /dev/null and b/apk/ai.comma.plus.offroad.apk differ diff --git a/apk/com.baseui.apk b/apk/com.baseui.apk deleted file mode 100644 index 43c08418f3d406..00000000000000 Binary files a/apk/com.baseui.apk and /dev/null differ diff --git a/cereal/car.capnp b/cereal/car.capnp index 6b58254d8856fc..63857ed744c631 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -12,12 +12,13 @@ $Java.outerClassname("Car"); struct CarEvent @0x9b1657f34caf3ad3 { name @0 :EventName; enable @1 :Bool; - preEnable @7 :Bool; noEntry @2 :Bool; warning @3 :Bool; userDisable @4 :Bool; softDisable @5 :Bool; immediateDisable @6 :Bool; + preEnable @7 :Bool; + permanent @8 :Bool; enum EventName @0xbaa8c5d505f727de { # TODO: copy from error list @@ -64,9 +65,10 @@ struct CarState { events @13 :List(CarEvent); # car speed - vEgo @1 :Float32; # best estimate of speed - aEgo @16 :Float32; # best estimate of acceleration - vEgoRaw @17 :Float32; # unfiltered speed + vEgo @1 :Float32; # best estimate of speed + aEgo @16 :Float32; # best estimate of acceleration + vEgoRaw @17 :Float32; # unfiltered speed from CAN sensors + yawRate @22 :Float32; # best estimate of yaw rate standstill @18 :Bool; wheelSpeeds @2 :WheelSpeeds; @@ -309,4 +311,5 @@ struct CarParams { directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake stoppingControl @34 :Bool; # Does the car allows full control even at lows speeds when stopping startAccel @35 :Float32; # Required acceleraton to overcome creep braking + steerRateCost @40 :Float32; # Lateral MPC cost on steering rate } diff --git a/cereal/log.capnp b/cereal/log.capnp index e1d61b88bc4c07..c3ddad9bd30dd2 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -28,6 +28,7 @@ struct InitData { version @4 :Text; gitCommit @10 :Text; gitBranch @11 :Text; + gitRemote @13 :Text; androidBuildInfo @5 :AndroidBuildInfo; androidSensors @6 :List(AndroidSensor); @@ -154,6 +155,8 @@ struct SensorEventData { pressure @9 :SensorVec; magneticUncalibrated @11 :SensorVec; gyroUncalibrated @12 :SensorVec; + proximity @13: Float32; + light @14: Float32; } source @8 :SensorSource; @@ -242,8 +245,11 @@ struct ThermalData { freeSpace @7 :Float32; batteryPercent @8 :Int16; batteryStatus @9 :Text; + usbOnline @12 :Bool; fanSpeed @10 :UInt16; + started @11 :Bool; + startedTs @13 :UInt64; } struct HealthData { @@ -349,6 +355,7 @@ struct Live100Data { jerkFactor @12 :Float32; angleSteers @13 :Float32; # Steering angle in degrees. angleSteersDes @29 :Float32; + curvature @37 :Float32; # path curvature from vehicle model hudLeadDEPRECATED @14 :Int32; cumLagMs @15 :Float32; @@ -361,6 +368,8 @@ struct Live100Data { rearViewCam @23 :Bool; alertText1 @24 :Text; alertText2 @25 :Text; + alertStatus @38 :AlertStatus; + alertSize @39 :AlertSize; awarenessStatus @26 :Float32; angleOffset @27 :Float32; @@ -378,6 +387,20 @@ struct Live100Data { stopping @2; starting @3; } + + enum AlertStatus { + normal @0; # low priority alert for user's convenience + userPrompt @1; # mid piority alert that might require user intervention + critical @2; # high priority alert that needs immediate user intervention + } + + enum AlertSize { + none @0; # don't display the alert + small @1; # small box + mid @2; # mid screen + full @3; # full screen + } + } struct LiveEventData { @@ -600,6 +623,23 @@ struct NavUpdate { } } +struct NavStatus { + isNavigating @0 :Bool; + currentAddress @1 :Address; + + struct Address { + title @0 :Text; + lat @1 :Float64; + lng @2 :Float64; + house @3 :Text; + address @4 :Text; + street @5 :Text; + city @6 :Text; + state @7 :Text; + country @8 :Text; + } +} + struct CellInfo { timestamp @0 :UInt64; repr @1 :Text; # android toString() for now @@ -1300,5 +1340,6 @@ struct Event { clocks @35 :Clocks; liveMpc @36 :LiveMpcData; liveLongitudinalMpc @37 :LiveLongitudinalMpcData; + navStatus @38 :NavStatus; } } diff --git a/common/basedir.py b/common/basedir.py index 991f6aaed92ae4..99760fa3343653 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,4 +1,4 @@ import os -BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) diff --git a/common/fingerprints.py b/common/fingerprints.py index 9881115b69cd7f..d5b9cc91d0db60 100644 --- a/common/fingerprints.py +++ b/common/fingerprints.py @@ -19,6 +19,9 @@ "TOYOTA RAV4 2017": { 36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 }, + "TOYOTA RAV4 2017 HYBRID": { + 36L: 8, 37L: 8, 170L: 8, 180L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 581L: 5, 608L: 8, 610L: 5, 643L: 7, 713L: 8, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 3, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1005L: 2, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1184L: 8, 1185L: 8, 1186L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1197L: 8, 1198L: 8, 1199L: 8, 1212L: 8, 1227L: 8, 1232L: 8, 1235L: 8, 1264L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8 + }, "TOYOTA PRIUS 2017": { 36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8 }, diff --git a/common/kalman/ekf.py b/common/kalman/ekf.py index 454966d64d52a4..feebe0883f1070 100644 --- a/common/kalman/ekf.py +++ b/common/kalman/ekf.py @@ -35,7 +35,7 @@ def __repr__(self): # A generic sensor class that does no pre-processing of data class SimpleSensor: # obs_model can be - # a full obesrvation model matrix, or + # a full observation model matrix, or # an integer or tuple of indices into ekf.state, indicating which variables are being directly observed # covar can be # a full covariance matrix @@ -129,7 +129,7 @@ def update(self, reading): print "covar:\n",self.covar def update_scalar(self, reading): - # like update but knowing that measurment is a scalar + # like update but knowing that measurement is a scalar # this avoids matrix inversions and speeds up (surprisingly) drived.py a lot # innovation = reading.data - np.matmul(reading.obs_model, self.state) @@ -188,7 +188,7 @@ def calc_transfer_fun(self, dt): Current implementations calculate A and J as functions of state. Control input can be added trivially by adding a control parameter to predict() and calc_tranfer_update(), - and using it during calcualtion of A and J + and using it during calculation of A and J """ diff --git a/common/params.py b/common/params.py index 4701608561db28..d6503291eb4e4d 100755 --- a/common/params.py +++ b/common/params.py @@ -52,12 +52,14 @@ class UnknownKeyName(Exception): "Version": TxType.PERSISTANT, "GitCommit": TxType.PERSISTANT, "GitBranch": TxType.PERSISTANT, + "GitRemote": TxType.PERSISTANT, # written: baseui # read: ui, controls "IsMetric": TxType.PERSISTANT, "IsRearViewMirror": TxType.PERSISTANT, "IsFcwEnabled": TxType.PERSISTANT, "HasAcceptedTerms": TxType.PERSISTANT, + "IsUploadVideoOverCellularEnabled": TxType.PERSISTANT, # written: visiond # read: visiond, controlsd "CalibrationParams": TxType.PERSISTANT, @@ -69,6 +71,7 @@ class UnknownKeyName(Exception): "CarParams": TxType.CLEAR_ON_CAR_START, "Passive": TxType.PERSISTANT, + "DoUninstall": TxType.CLEAR_ON_MANAGER_START, } def fsync_dir(path): diff --git a/common/profiler.py b/common/profiler.py index 26fbeb382b57d9..83e5672e0411de 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -1,32 +1,47 @@ -from common.realtime import sec_since_boot +import time class Profiler(object): def __init__(self, enabled=False): self.enabled = enabled - self.cp = [] - self.start_time = sec_since_boot() + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.clock() self.last_time = self.start_time - - def checkpoint(self, name): - if not self.enabled: - return - tt = sec_since_boot() - self.cp.append((name, tt - self.last_time)) - self.last_time = tt + self.tot = 0. def reset(self, enabled=False): self.enabled = enabled - self.cp = [] - self.start_time = sec_since_boot() + self.cp = {} + self.cp_ignored = [] + self.iter = 0 + self.start_time = time.clock() self.last_time = self.start_time + def checkpoint(self, name, ignore=False): + # ignore flag needed when benchmarking threads with ratekeeper + if not self.enabled: + return + tt = time.clock() + if name not in self.cp: + self.cp[name] = 0. + if ignore: + self.cp_ignored.append(name) + self.cp[name] += tt - self.last_time + if not ignore: + self.tot += tt - self.last_time + self.last_time = tt + def display(self): if not self.enabled: return + self.iter += 1 print "******* Profiling *******" - tot = 0.0 - for n, ms in self.cp: - print "%30s: %7.2f" % (n, ms*1000.0) - tot += ms - print " TOTAL: %7.2f" % (tot*1000.0) + for n in self.cp: + ms = self.cp[n] + if n in self.cp_ignored: + print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100), " IGNORED" + else: + print "%30s: %7.2f perc: %1.0f" % (n, ms*1000.0, ms/self.tot*100) + print "Iter clock: %2.6f TOTAL: %2.2f" % (self.tot/self.iter, self.tot) diff --git a/launch_openpilot.sh b/launch_openpilot.sh new file mode 100755 index 00000000000000..4b55f80f110af3 --- /dev/null +++ b/launch_openpilot.sh @@ -0,0 +1,39 @@ +#!/usr/bin/bash + +function launch { + # apply update + if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + git reset --hard @{u} && + git clean -xdf && + exec "${BASH_SOURCE[0]}" + fi + + # check if NEOS update is required + while [ "$(cat /VERSION)" -lt 4 ] && [ ! -e /data/media/0/noupdate ]; do + curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater + sleep 10 + done + + # no cpu rationing for now + echo 0-3 > /dev/cpuset/background/cpus + echo 0-3 > /dev/cpuset/system-background/cpus + echo 0-3 > /dev/cpuset/foreground/boost/cpus + echo 0-3 > /dev/cpuset/foreground/cpus + echo 0-3 > /dev/cpuset/android/cpus + + # wait for network + (cd selfdrive/ui/spinner && exec ./spinner 'waiting for network...') & spin_pid=$! + until ping -W 1 -c 1 8.8.8.8; do sleep 1; done + kill $spin_pid + + export PYTHONPATH="$PWD" + + # start manager + cd selfdrive + ./manager.py + + # if broken, keep on screen error + while true; do sleep 1; done +} + +launch diff --git a/selfdrive/assets/OpenSans-Regular.ttf b/selfdrive/assets/OpenSans-Regular.ttf new file mode 100644 index 00000000000000..2e31d02424ed50 Binary files /dev/null and b/selfdrive/assets/OpenSans-Regular.ttf differ diff --git a/selfdrive/assets/OpenSans-SemiBold.ttf b/selfdrive/assets/OpenSans-SemiBold.ttf new file mode 100644 index 00000000000000..99db86aa0282e4 Binary files /dev/null and b/selfdrive/assets/OpenSans-SemiBold.ttf differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 48aecca0cd365f..285134de9e3320 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -130,7 +130,11 @@ bool usb_connect() { #endif // no output is the default - libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + if (getenv("RECVMOCK")) { + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT); + } else { + libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + } if (safety_setter_thread_handle == -1) { err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py index 0afeee40ef334b..7a5ae475911b0e 100644 --- a/selfdrive/can/libdbc_py.py +++ b/selfdrive/can/libdbc_py.py @@ -1,11 +1,12 @@ import os +import sys import subprocess from cffi import FFI can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") -subprocess.check_output(["make"], cwd=can_dir) +subprocess.check_call(["make"], stdout=sys.stderr, cwd=can_dir) ffi = FFI() ffi.cdef(""" diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 853b0b12a74e46..828f95a939987b 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -242,7 +242,7 @@ class CANParser { const auto& state = kv.second; if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { if (state.seen > 0) { - INFO("%X TIMEOUT\n", state.address); + DEBUG("%X TIMEOUT\n", state.address); } can_valid = false; } diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 516ed7c0c0dbe9..4526d35c405e8c 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -6,12 +6,9 @@ from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging -from .honda.interface import CarInterface as HondaInterface - -try: - from .toyota.interface import CarInterface as ToyotaInterface -except ImportError: - ToyotaInterface = None +from selfdrive.car.honda.interface import CarInterface as HondaInterface +from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface +from selfdrive.car.mock.interface import CarInterface as MockInterface try: from .simulator.interface import CarInterface as SimInterface @@ -31,9 +28,12 @@ "HONDA CR-V 2016 TOURING": HondaInterface, "TOYOTA PRIUS 2017": ToyotaInterface, "TOYOTA RAV4 2017": ToyotaInterface, + "TOYOTA RAV4 2017 HYBRID": ToyotaInterface, "simulator": SimInterface, - "simulator2": Sim2Interface + "simulator2": Sim2Interface, + + "mock": MockInterface } # **** for use live only **** @@ -75,12 +75,18 @@ def fingerprint(logcan, timeout): return (candidate_cars[0], finger) -def get_car(logcan, sendcan=None, timeout=None): +def get_car(logcan, sendcan=None, passive=True): + + # TODO: timeout only useful for replays so controlsd can start before unlogger + timeout = 1. if passive else None candidate, fingerprints = fingerprint(logcan, timeout) if candidate is None: cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) - return None, None + if passive: + candidate = "mock" + else: + return None, None interface_cls = interfaces[candidate] params = interface_cls.get_params(candidate, fingerprints) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index fbf51f36a1bed3..1ee7e2c8089afa 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -122,7 +122,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ GAS_MAX = 1004 BRAKE_MAX = 1024/4 if CS.civic: - is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx'] + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] STEER_MAX = 0x1FFF if is_fw_modified else 0x1000 elif CS.crv: STEER_MAX = 0x300 # CR-V only uses 12-bits and requires a lower value diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 6ade91d580b2cb..015ff7c5737920 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,5 +1,4 @@ import os -import time from cereal import car from common.numpy_fast import interp import selfdrive.messaging as messaging diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 62bb834cbbe281..7879a9ef1e51f4 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import os -import time import numpy as np from common.numpy_fast import clip, interp from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events +from selfdrive.controls.lib.vehicle_model import VehicleModel from cereal import car from selfdrive.services import service_list import selfdrive.messaging as messaging @@ -93,6 +93,7 @@ def __init__(self, CP, sendcan=None): # *** init the major players *** self.CS = CarState(CP) + self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: @@ -163,7 +164,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = centerToFront_civic ret.steerRatio = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car - is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185cxxx'] + is_fw_modified = os.getenv("DONGLE_ID") in ['b0f5a01cf604185c'] ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] ret.longitudinalKpBP = [0., 5., 35.] @@ -252,6 +253,8 @@ def get_params(candidate, fingerprint): ret.steerLimitAlert = True ret.startAccel = 0.5 + ret.steerRateCost = 0.5 + return ret # returns a car.CarState @@ -270,6 +273,7 @@ def update(self, c): ret.vEgo = self.CS.v_ego ret.aEgo = self.CS.a_ego ret.vEgoRaw = self.CS.v_ego_raw + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr @@ -370,11 +374,11 @@ def update(self, c): else: self.can_invalid_count = 0 if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_not_allowed: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.brake_error: - events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not ret.gearShifter == 'drive': events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if not self.CS.door_all_closed: diff --git a/selfdrive/car/mock/__init__.py b/selfdrive/car/mock/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py new file mode 100755 index 00000000000000..6acad5e5a99a2e --- /dev/null +++ b/selfdrive/car/mock/interface.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python +import os +import time +import zmq +from common.realtime import sec_since_boot +import common.numpy_fast as np +from cereal import car +from selfdrive.config import Conversions as CV +from selfdrive.services import service_list +import selfdrive.messaging as messaging +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event + +# mocked car interface to work with chffrplus +TS = 0.01 # 100Hz +YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter +# low pass gain +LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) + + +class CarInterface(object): + def __init__(self, CP, sendcan=None): + + self.CP = CP + + print "Using Mock Car Interface" + context = zmq.Context() + + # TODO: subscribe to phone sensor + self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port) + self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port) + + self.speed = 0. + self.yaw_rate = 0. + self.yaw_rate_meas = 0. + + @staticmethod + def compute_gb(accel, speed): + return accel + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1.0 + + @staticmethod + def get_params(candidate, fingerprint): + + ret = car.CarParams.new_message() + + ret.carName = "mock" + ret.radarName = "mock" + ret.carFingerprint = candidate + + ret.safetyModel = car.CarParams.SafetyModels.noOutput + + # FIXME: hardcoding honda civic 2016 touring params so they can be used to + # scale unknown params for other cars + ret.mass = 1700. + ret.rotationalInertia = 2500. + ret.wheelbase = 2.70 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 13. # reasonable + ret.longPidDeadzoneBP = [0.] + ret.longPidDeadzoneV = [0.] + ret.tireStiffnessFront = 1e6 # very stiff to neglect slip + ret.tireStiffnessRear = 1e6 # very stiff to neglect slip + ret.steerRatioRear = 0. + + ret.steerMaxBP = [0.] + ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.] + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [0.] + + ret.longitudinalKpBP = [0.] + ret.longitudinalKpV = [0.] + ret.longitudinalKiBP = [0.] + ret.longitudinalKiV = [0.] + + return ret + + # returns a car.CarState + def update(self, c): + + # get basic data from phone and gps since CAN isn't connected + sensors = messaging.recv_sock(self.sensor) + if sensors is not None: + for sensor in sensors.sensorEvents: + if sensor.type == 4: # gyro + self.yaw_rate_meas = -sensor.gyro.v[0] + + gps = messaging.recv_sock(self.gps) + if gps is not None: + self.speed = gps.gpsLocation.speed + + # create message + ret = car.CarState.new_message() + + # speeds + ret.vEgo = self.speed + ret.vEgoRaw = self.speed + ret.aEgo = 0. + self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate + ret.yawRate = self.yaw_rate + ret.standstill = self.speed < 0.01 + ret.wheelSpeeds.fl = self.speed + ret.wheelSpeeds.fr = self.speed + ret.wheelSpeeds.rl = self.speed + ret.wheelSpeeds.rr = self.speed + curvature = self.yaw_rate / max(self.speed, 1.) + ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG + + events = [] + #events.append(create_event('passive', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + ret.events = events + + return ret.as_reader() + + def apply(self, c): + # in mock no carcontrols + return False diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 097bf361d7194f..5b56e2a6f29582 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,7 @@ create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_fcw_command -from selfdrive.car.toyota.values import CAR, ECU +from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value @@ -27,60 +27,6 @@ 0x373, 0x374, 0x375, 0x380, 0x381, 0x382, 0x383] -# addr, [ecu, bus, 1/freq*100, vl] -STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'), - 0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'), - - 0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), - 0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), - 0x2E6: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), - 0x2E7: (ECU.DSU, (CAR.PRIUS,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), - - 0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - 0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), - 0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), - - 0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), - 0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), - - 0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x33E: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - 0x365: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - 0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), - 0x366: (ECU.DSU, (CAR.PRIUS,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - 0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), - - 0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 40, '\x06\x00'), - - 0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), - 0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - 0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - 0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), - 0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), - 0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), - 0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4), 1, 100, '\x20\x20\xAD'), - 0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), - 0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), - 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), - 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), - 0x470: (ECU.DSU, (CAR.PRIUS,), 1, 100, '\x00\x00\x02\x7a'), - } - - -def check_ecu_msgs(fingerprint, candidate, ecu): - # return True if fingerprint contains messages normally sent by a given ecu - ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and - candidate in STATIC_MSGS[x][1] and - STATIC_MSGS[x][2] == 0)] - - return any(msg for msg in fingerprint if msg in ecu_msgs) - - def accel_hysteresis(accel, accel_steady, enabled): # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 2b948ac493f376..e85902f944b9e5 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,5 +1,4 @@ import os -import time import selfdrive.messaging as messaging from selfdrive.car.toyota.values import CAR from selfdrive.can.parser import CANParser @@ -19,7 +18,7 @@ def parse_gear_shifter(can_gear, car_fingerprint): return "drive" elif can_gear == 0x4: return "brake" - elif car_fingerprint == CAR.RAV4: + elif car_fingerprint in [CAR.RAV4, CAR.RAV4H]: if can_gear == 0x20: return "park" elif can_gear == 0x10: @@ -47,6 +46,17 @@ def get_can_parser(CP): (550, 40), (581, 33) ] + elif CP.carFingerprint == CAR.RAV4H: + dbc_f = 'toyota_rav4_hybrid_2017_pt.dbc' + signals = [ + ("GEAR", 956, 0), + ("BRAKE_PRESSED", 550, 0), + ("GAS_PEDAL", 581, 0), + ] + checks = [ + (550, 40), + (581, 33) + ] elif CP.carFingerprint == CAR.RAV4: dbc_f = 'toyota_rav4_2017_pt.dbc' signals = [ @@ -131,6 +141,10 @@ def update(self, cp): can_gear = cp.vl[295]['GEAR'] self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] self.pedal_gas = cp.vl[581]['GAS_PEDAL'] + elif self.car_fingerprint == CAR.RAV4H: + can_gear = cp.vl[956]['GEAR'] + self.brake_pressed = cp.vl[550]['BRAKE_PRESSED'] + self.pedal_gas = cp.vl[581]['GAS_PEDAL'] elif self.car_fingerprint == CAR.RAV4: can_gear = cp.vl[956]['GEAR'] self.brake_pressed = cp.vl[548]['BRAKE_PRESSED'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index fc75335361af9a..c490c1db23d4d0 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,20 +1,24 @@ #!/usr/bin/env python import os -import time from common.realtime import sec_since_boot import common.numpy_fast as np -from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.carstate import CarState, get_can_parser -from selfdrive.car.toyota.values import CAR, ECU -from selfdrive.car.toyota.carcontroller import CarController, check_ecu_msgs from cereal import car +from selfdrive.config import Conversions as CV from selfdrive.services import service_list import selfdrive.messaging as messaging from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.vehicle_model import VehicleModel +from selfdrive.car.toyota.carstate import CarState, get_can_parser +from selfdrive.car.toyota.values import CAR, ECU, check_ecu_msgs +try: + from selfdrive.car.toyota.carcontroller import CarController +except ImportError: + CarController = None class CarInterface(object): def __init__(self, CP, sendcan=None): self.CP = CP + self.VM = VehicleModel(CP) self.frame = 0 self.can_invalid_count = 0 @@ -70,9 +74,8 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 - stop_and_go = True ret.mass = 3045./2.205 + std_cargo - ret.wheelbase = 2.70 + ret.wheelbase = 2.70 if candidate == CAR.PRIUS else 2.65 ret.centerToFront = ret.wheelbase * 0.44 ret.steerRatio = 14.5 #Rav4 2017, TODO: find exact value for Prius ret.steerKp, ret.steerKi = 0.6, 0.05 @@ -83,9 +86,9 @@ def get_params(candidate, fingerprint): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - if candidate == CAR.PRIUS: + if candidate in [CAR.PRIUS, CAR.RAV4H]: # rav4 hybrid can do stop and go ret.minEnableSpeed = -1. - elif candidate == CAR.RAV4: # TODO: hack Rav4 to do stop and go + elif candidate == CAR.RAV4: # TODO: hack ICE Rav4 to do stop and go ret.minEnableSpeed = 19. * CV.MPH_TO_MS centerToRear = ret.wheelbase - ret.centerToFront @@ -131,6 +134,11 @@ def get_params(candidate, fingerprint): ret.longitudinalKiBP = [0., 35.] ret.longitudinalKiV = [0.54, 0.36] + if candidate == CAR.PRIUS: + ret.steerRateCost = 2. + elif candidate in [CAR.RAV4, CAR.RAV4H]: + ret.steerRateCost = 1. + return ret # returns a car.CarState @@ -150,6 +158,7 @@ def update(self, c): ret.vEgo = self.CS.v_ego ret.vEgoRaw = self.CS.v_ego_raw ret.aEgo = self.CS.a_ego + ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) ret.standstill = self.CS.standstill ret.wheelSpeeds.fl = self.CS.v_wheel_fl ret.wheelSpeeds.fr = self.CS.v_wheel_fr @@ -180,7 +189,12 @@ def update(self, c): ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = 0. - ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 + if self.CP.carFingerprint == CAR.RAV4H: + # ignore standstill in hybrid rav4, since pcm allows to restart without + # receiving any special command + ret.cruiseState.standstill = False + else: + ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 # TODO: button presses buttonEvents = [] @@ -223,8 +237,8 @@ def update(self, c): events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - if self.CS.low_speed_lockout: - events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY])) + if self.CS.low_speed_lockout and self.CP.enableDsu: + events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if c.actuators.gas > 0.1: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 06373b3b1807b0..d8fa0c699e5453 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,8 +1,64 @@ class CAR: - PRIUS = "TOYOTA PRIUS 2017" - RAV4 = "TOYOTA RAV4 2017" - + PRIUS = "TOYOTA PRIUS 2017" + RAV4H = "TOYOTA RAV4 2017 HYBRID" + RAV4 = "TOYOTA RAV4 2017" + + class ECU: CAM = 0 # camera DSU = 1 # driving support unit APGS = 2 # advanced parking guidance system + + +# addr, [ecu, bus, 1/freq*100, vl] +STATIC_MSGS = {0x141: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 2, '\x00\x00\x00\x46'), + 0x128: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 3, '\xf4\x01\x90\x83\x00\x37'), + + 0x292: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), + 0x283: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), + 0x2E6: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + 0x2E7: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + + 0x240: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x241: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x244: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x245: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), + 0x248: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), + 0x344: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), + + 0x160: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), + 0x161: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), + + 0x32E: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x33E: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), + 0x365: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), + 0x365: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), + 0x366: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), + 0x366: (ECU.DSU, (CAR.RAV4,), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), + + 0x367: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 40, '\x06\x00'), + + 0x414: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), + 0x489: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48a: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), + 0x48b: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), + 0x4d3: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + 0x130: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), + 0x466: (ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 1, 100, '\x20\x20\xAD'), + 0x396: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), + 0x43A: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), + 0x43B: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x497: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), + 0x4CC: (ECU.APGS, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), + 0x4CB: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.RAV4), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + 0x470: (ECU.DSU, (CAR.PRIUS, CAR.RAV4H,), 1, 100, '\x00\x00\x02\x7a'), + } + + +def check_ecu_msgs(fingerprint, candidate, ecu): + # return True if fingerprint contains messages normally sent by a given ecu + ecu_msgs = [x for x in STATIC_MSGS if (ecu == STATIC_MSGS[x][0] and + candidate in STATIC_MSGS[x][1] and + STATIC_MSGS[x][2] == 0)] + + return any(msg for msg in fingerprint if msg in ecu_msgs) diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 777dde97d1d14e..4cf48d2e1ffc45 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.3.9-openpilot" +#define COMMA_VERSION "0.4.0.1-openpilot" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 75ace9cdbed3b0..df27d6322a01ad 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -2,7 +2,7 @@ import json from copy import copy import zmq -from cereal import car +from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper from common.profiler import Profiler @@ -30,6 +30,7 @@ AWARENESS_TIME = 360. # 6 minutes limit without user touching steering wheels AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before start decelerating the car +State = log.Live100Data.ControlState class Calibration: UNCALIBRATED = 0 @@ -37,38 +38,43 @@ class Calibration: INVALID = 2 -class State: - DISABLED = 'disabled' - ENABLED = 'enabled' - PRE_ENABLED = 'preEnabled' - SOFT_DISABLING = 'softDisabling' - - # True when actuators are controlled def isActive(state): - return state in [State.ENABLED, State.SOFT_DISABLING] + return state in [State.enabled, State.softDisabling] # True if system is engaged def isEnabled(state): - return (isActive(state) or state == State.PRE_ENABLED) + return (isActive(state) or state == State.preEnabled) -def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): +def data_sample(CI, CC, thermal, calibration, health, poller, cal_status, overtemp, free_space): # *** read can and compute car states *** CS = CI.update(CC) events = list(CS.events) + td = None + cal = None + hh = None + + for socket, event in poller.poll(0): + if socket is thermal: + td = messaging.recv_one(socket) + elif socket is calibration: + cal = messaging.recv_one(socket) + elif socket is health: + hh = messaging.recv_one(socket) + # *** thermal checking logic *** # thermal data, checked every second - td = messaging.recv_sock(thermal) if td is not None: - # overtemp above 95 deg - overtemp = any( - t > 950 - for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, - td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) + # CPU overtemp above 95 deg + overtemp_proc = any(t > 950 for t in + (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, + td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) + overtemp_bat = td.thermal.bat > 50000 # 50c + overtemp = overtemp_proc or overtemp_bat # under 15% of space free no enable allowed free_space = td.thermal.freeSpace < 0.15 @@ -80,7 +86,6 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): events.append(create_event('outOfSpace', [ET.NO_ENTRY])) # *** read calibration status *** - cal = messaging.recv_sock(cal) if cal is not None: cal_status = cal.liveCalibration.calStatus @@ -91,11 +96,10 @@ def data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space): events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) # *** health checking logic *** - hh = messaging.recv_sock(health) if hh is not None: controls_allowed = hh.health.controlsAllowed if not controls_allowed: - events.append(create_event('controlsMismatch', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) return CS, events, cal_status, overtemp, free_space @@ -123,6 +127,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition + v_cruise_kph_last = v_cruise_kph for b in CS.buttonEvents: if not CP.enableCruise and enabled and not b.pressed: if b.type == "accelCruise": @@ -138,76 +143,75 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM # ***** handle state transitions ***** # DISABLED - if state == State.DISABLED: + if state == State.disabled: if get_events(events, [ET.ENABLE]): - if get_events(events, [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): - for e in get_events(events, [ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE]): - AM.add(e, enabled) + if get_events(events, [ET.NO_ENTRY]): for e in get_events(events, [ET.NO_ENTRY]): AM.add(str(e) + "NoEntry", enabled) + else: if get_events(events, [ET.PRE_ENABLE]): - state = State.PRE_ENABLED + state = State.preEnabled else: - state = State.ENABLED + state = State.enabled AM.add("enable", enabled) # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int(round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN))) # ENABLED - elif state == State.ENABLED: + elif state == State.enabled: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif get_events(events, [ET.SOFT_DISABLE]): - state = State.SOFT_DISABLING + state = State.softDisabling soft_disable_timer = 300 # 3s TODO: use rate for e in get_events(events, [ET.SOFT_DISABLE]): AM.add(e, enabled) # SOFT DISABLING - elif state == State.SOFT_DISABLING: + elif state == State.softDisabling: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.SOFT_DISABLE]): # no more soft disabling condition, so go back to ENABLED - state = State.ENABLED + state = State.enabled elif soft_disable_timer <= 0: - state = State.DISABLED + state = State.disabled - # TODO: PRE ENABLING - elif state == State.PRE_ENABLED: + # PRE ENABLING + elif state == State.preEnabled: if get_events(events, [ET.USER_DISABLE]): - state = State.DISABLED + state = State.disabled AM.add("disable", enabled) elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): - state = State.DISABLED + state = State.disabled for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): AM.add(e, enabled) elif not get_events(events, [ET.PRE_ENABLE]): - state = State.ENABLED + state = State.enabled - return state, soft_disable_timer, v_cruise_kph + return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last -def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, - PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): +def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, + awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle): # Given the state, this function returns the actuators # reset actuators to zero @@ -217,9 +221,6 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s active = isActive(state) for b in CS.buttonEvents: - # any button event resets awarenesss_status - awareness_status = 1. - # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed and rear_view_allowed: @@ -238,24 +239,20 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s # ***** state specific actions ***** # DISABLED - if state in [State.PRE_ENABLED, State.DISABLED]: - awareness_status = 1. + if state in [State.preEnabled, State.disabled]: + LaC.reset() LoC.reset(v_pid=CS.vEgo) # ENABLED or SOFT_DISABLING - elif state in [State.ENABLED, State.SOFT_DISABLING]: - - if CS.steeringPressed: - # reset awareness status on steering - awareness_status = 1.0 + elif state in [State.enabled, State.softDisabling]: - # 6 minutes driver you're on + # decrease awareness status awareness_status -= 0.01/(AWARENESS_TIME) if awareness_status <= 0.: AM.add("driverDistracted", enabled) elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \ - awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME: + awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME: AM.add("preDriverDistracted", enabled) # parse warnings from car specific interface @@ -286,6 +283,17 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s if CP.enableCruise and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + # reset conditions for the 6 minutes timout + if CS.buttonEvents or \ + v_cruise_kph != v_cruise_kph_last or \ + CS.steeringPressed or \ + state in [State.preEnabled, State.disabled]: + awareness_status = 1. + + # parse permanent warnings to display constantly + for e in get_events(events, [ET.PERMANENT]): + AM.add(str(e) + "Permanent", enabled) + # *** process alerts *** AM.process_alerts(sec_since_boot()) @@ -293,7 +301,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_s return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle -def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate, +def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive): @@ -335,6 +343,8 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, dat.live100.rearViewCam = ('reverseGear' in [e.name for e in events] and rear_view_allowed) or rear_view_toggle dat.live100.alertText1 = AM.alert_text_1 dat.live100.alertText2 = AM.alert_text_2 + dat.live100.alertSize = AM.alert_size + dat.live100.alertStatus = AM.alert_status dat.live100.awarenessStatus = max(awareness_status, 0.0) if isEnabled(state) else 0.0 # what packets were used to process @@ -349,6 +359,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, dat.live100.vEgo = CS.vEgo dat.live100.vEgoRaw = CS.vEgoRaw dat.live100.angleSteers = CS.steeringAngle + dat.live100.curvature = VM.calc_curvature(CS.steeringAngle * CV.DEG_TO_RAD, CS.vEgo) dat.live100.steerOverride = CS.steeringPressed # high level control state @@ -410,7 +421,7 @@ def data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, def controlsd_thread(gctx, rate=100): # start the loop - set_realtime_priority(2) + set_realtime_priority(3) context = zmq.Context() @@ -429,9 +440,11 @@ def controlsd_thread(gctx, rate=100): sendcan = None # sub - thermal = messaging.sub_sock(context, service_list['thermal'].port) - health = messaging.sub_sock(context, service_list['health'].port) - cal = messaging.sub_sock(context, service_list['liveCalibration'].port) + poller = zmq.Poller() + thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller) + health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) + cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) + logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() @@ -439,10 +452,7 @@ def controlsd_thread(gctx, rate=100): CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: - if passive: - return - else: - raise Exception("unsupported car") + raise Exception("unsupported car") if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput @@ -461,7 +471,7 @@ def controlsd_thread(gctx, rate=100): # write CarParams params.put("CarParams", CP.to_bytes()) - state = State.DISABLED + state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 overtemp = False @@ -472,6 +482,7 @@ def controlsd_thread(gctx, rate=100): # 0.0 - 1.0 awareness_status = 1. + v_cruise_kph_last = 0 rk = Ratekeeper(rate, print_delay_threshold=2./1000) @@ -485,14 +496,14 @@ def controlsd_thread(gctx, rate=100): except (ValueError, KeyError): pass - prof = Profiler() + prof = Profiler(False) # off by default while 1: - prof.reset() # avoid memory leak + prof.checkpoint("Ratekeeper", ignore=True) # rk is here # sample data and compute car events - CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status, + CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, cal, health, poller, cal_status, overtemp, free_space) prof.checkpoint("Sample") @@ -502,24 +513,26 @@ def controlsd_thread(gctx, rate=100): if not passive: # update control state - state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) + state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = state_transition(CS, CP, state, events, soft_disable_timer, + v_cruise_kph, AM) prof.checkpoint("State transition") # compute actuators actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, - AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, - rear_view_allowed, rear_view_toggle) + v_cruise_kph_last, AM, rk, awareness_status, PL, LaC, LoC, VM, + angle_offset, rear_view_allowed, rear_view_toggle) prof.checkpoint("State Control") # publish data - CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, + CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") # *** run loop at fixed rate *** - if rk.keep_time(): - prof.display() + rk.keep_time() + + prof.display() def main(gctx=None): diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 30422b20ee6f3d..82174135e8b274 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -1,19 +1,25 @@ -from cereal import car +from cereal import car, log from selfdrive.swaglog import cloudlog +from common.realtime import sec_since_boot import copy # Priority -class PT: +class Priority: HIGH = 3 MID = 2 LOW = 1 + LOWEST = 0 +AlertSize = log.Live100Data.AlertSize +AlertStatus = log.Live100Data.AlertStatus class Alert(object): def __init__(self, alert_text_1, alert_text_2, + alert_status, + alert_size, alert_priority, visual_alert, audible_alert, @@ -23,6 +29,8 @@ def __init__(self, self.alert_text_1 = alert_text_1 self.alert_text_2 = alert_text_2 + self.alert_status = alert_status + self.alert_size = alert_size self.alert_priority = alert_priority self.visual_alert = visual_alert if visual_alert is not None else "none" self.audible_alert = audible_alert if audible_alert is not None else "none" @@ -31,6 +39,8 @@ def __init__(self, self.duration_hud_alert = duration_hud_alert self.duration_text = duration_text + self.start_time = 0. + # typecheck that enums are valid on startup tst = car.CarControl.new_message() tst.hudControl.visualAlert = self.visual_alert @@ -51,347 +61,418 @@ class AlertManager(object): "enable": Alert( "", "", - PT.MID, None, "beepSingle", .2, 0., 0.), + AlertStatus.normal, AlertSize.none, + Priority.MID, None, "beepSingle", .2, 0., 0.), "disable": Alert( "", "", - PT.MID, None, "beepSingle", .2, 0., 0.), + AlertStatus.normal, AlertSize.none, + Priority.MID, None, "beepSingle", .2, 0., 0.), "fcw": Alert( - "Brake", + "Brake!", "Risk of Collision", - PT.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "fcw", "chimeRepeated", 1., 2., 2.), "steerSaturated": Alert( "Take Control", "Turn Exceeds Limit", - PT.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeSingle", 1., 2., 3.), "steerTempUnavailable": Alert( "Take Control", "Steer Temporarily Unavailable", - PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), "preDriverDistracted": Alert( "Take Control", "User Distracted", - PT.LOW, "steerRequired", "chimeDouble", .4, 2., 3.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, "steerRequired", "chimeDouble", .1, .1, .1), "driverDistracted": Alert( "Take Control to Regain Speed", "User Distracted", - PT.LOW, "steerRequired", "chimeRepeated", .5, .5, .5), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1), "startup": Alert( "Always Keep Hands on Wheel", "Be Ready to Take Over Any Time", - PT.LOW, None, None, 0., 0., 15.), + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, None, None, 0., 0., 15.), "ethicalDilemma": Alert( "Take Control Immediately", "Ethical Dilemma Detected", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), "steerTempUnavailableNoEntry": Alert( "Comma Unavailable", "Steer Temporary Unavailable", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "manualRestart": Alert( "Take Control", "Resume Driving Manually", - PT.LOW, None, None, .0, 0., 1.), + AlertStatus.userPrompt, AlertSize.full, + Priority.LOW, None, None, 0., 0., .2), # Non-entry only alerts "wrongCarModeNoEntry": Alert( "Comma Unavailable", "Main Switch Off", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "dataNeededNoEntry": Alert( "Comma Unavailable", "Data needed for calibration. Upload drive, try again", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "outOfSpaceNoEntry": Alert( "Comma Unavailable", "Out of Space", - PT.LOW, None, "chimeDouble", .4, 0., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 0., 3.), "pedalPressedNoEntry": Alert( "Comma Unavailable", "Pedal Pressed", - PT.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, "brakePressed", "chimeDouble", .4, 2., 3.), "speedTooLowNoEntry": Alert( "Comma Unavailable", "Speed Too Low", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeHoldNoEntry": Alert( "Comma Unavailable", "Brake Hold Active", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "parkBrakeNoEntry": Alert( "Comma Unavailable", "Park Brake Engaged", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "lowSpeedLockoutNoEntry": Alert( "Comma Unavailable", "Cruise Fault: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), - # Cancellation alerts causing disabling + # Cancellation alerts causing soft disabling "overheat": Alert( "Take Control Immediately", "System Overheated", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "wrongGear": Alert( "Take Control Immediately", "Gear not D", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInvalid": Alert( "Take Control Immediately", - "Calibration Invalid: Reposition Neo and Recalibrate", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "calibrationInProgress": Alert( "Take Control Immediately", "Calibration in Progress", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "doorOpen": Alert( "Take Control Immediately", "Door Open", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "seatbeltNotLatched": Alert( "Take Control Immediately", "Seatbelt Unlatched", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), "espDisabled": Alert( "Take Control Immediately", "ESP Off", - PT.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.MID, "steerRequired", "chimeRepeated", 1., 3., 3.), + # Cancellation alerts causing immediate disabling "radarCommIssue": Alert( "Take Control Immediately", "Radar Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "radarFault": Alert( "Take Control Immediately", "Radar Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "modelCommIssue": Alert( "Take Control Immediately", "Model Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsFailed": Alert( "Take Control Immediately", "Controls Failed", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "controlsMismatch": Alert( "Take Control Immediately", "Controls Mismatch", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "commIssue": Alert( "Take Control Immediately", "CAN Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "steerUnavailable": Alert( "Take Control Immediately", - "Steer Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Steer Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "brakeUnavailable": Alert( "Take Control Immediately", - "Brake Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Brake Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "gasUnavailable": Alert( "Take Control Immediately", - "Gas Error: Restart the Car", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + "Gas Fault: Restart the Car", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "reverseGear": Alert( "Take Control Immediately", "Reverse Gear", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), "cruiseDisabled": Alert( "Take Control Immediately", "Cruise Is Off", - PT.HIGH, "steerRequired", "chimeRepeated", 1., 3., 3.), + AlertStatus.critical, AlertSize.full, + Priority.HIGH, "steerRequired", "chimeRepeated", 1., 3., 4.), # not loud cancellations (user is in control) "noTarget": Alert( "Comma Canceled", "No Close Lead", - PT.HIGH, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.HIGH, None, "chimeDouble", .4, 2., 3.), "speedTooLow": Alert( "Comma Canceled", "Speed Too Low", - PT.HIGH, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.HIGH, None, "chimeDouble", .4, 2., 3.), # Cancellation alerts causing non-entry "overheatNoEntry": Alert( "Comma Unavailable", "System Overheated", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "wrongGearNoEntry": Alert( "Comma Unavailable", "Gear not D", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInvalidNoEntry": Alert( "Comma Unavailable", - "Calibration Invalid: Reposition Neo and Recalibrate", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Calibration Invalid: Reposition EON and Recalibrate", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "calibrationInProgressNoEntry": Alert( "Comma Unavailable", "Calibration in Progress", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "doorOpenNoEntry": Alert( "Comma Unavailable", "Door Open", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "seatbeltNotLatchedNoEntry": Alert( "Comma Unavailable", "Seatbelt Unlatched", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "espDisabledNoEntry": Alert( "Comma Unavailable", "ESP Off", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarCommIssueNoEntry": Alert( "Comma Unavailable", "Radar Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "radarFaultNoEntry": Alert( "Comma Unavailable", "Radar Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "modelCommIssueNoEntry": Alert( "Comma Unavailable", "Model Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "controlsFailedNoEntry": Alert( "Comma Unavailable", "Controls Failed", - PT.LOW, None, "chimeDouble", .4, 2., 3.), - - "controlsMismatchNoEntry": Alert( - "Comma Unavailable", - "Controls Mismatch", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "commIssueNoEntry": Alert( "Comma Unavailable", "CAN Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "steerUnavailableNoEntry": Alert( "Comma Unavailable", - "Steer Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Steer Fault: Restart the Car", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "brakeUnavailableNoEntry": Alert( "Comma Unavailable", - "Brake Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + "Brake Fault: Restart the Car", + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "gasUnavailableNoEntry": Alert( "Comma Unavailable", "Gas Error: Restart the Car", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "reverseGearNoEntry": Alert( "Comma Unavailable", "Reverse Gear", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "cruiseDisabledNoEntry": Alert( "Comma Unavailable", "Cruise is Off", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), "noTargetNoEntry": Alert( "Comma Unavailable", "No Close Lead", - PT.LOW, None, "chimeDouble", .4, 2., 3.), + AlertStatus.normal, AlertSize.full, + Priority.LOW, None, "chimeDouble", .4, 2., 3.), + + # permanent alerts to display on small UI upper box + "steerUnavailablePermanent": Alert( + "STEER FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), + + "brakeUnavailablePermanent": Alert( + "BRAKE FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), + + "lowSpeedLockoutPermanent": Alert( + "CRUISE FAULT", + "RESTART THE CAR", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, None, None, 0., 0., .2), } def __init__(self): self.activealerts = [] - self.current_alert = None def alertPresent(self): return len(self.activealerts) > 0 def add(self, alert_type, enabled=True, extra_text=''): alert_type = str(alert_type) - this_alert = copy.copy(self.alerts[alert_type]) - this_alert.alert_text_2 += extra_text + added_alert = copy.copy(self.alerts[alert_type]) + added_alert.alert_text_2 += extra_text + added_alert.start_time = sec_since_boot() # if new alert is higher priority, log it - if self.current_alert is None or this_alert > self.current_alert: + if not self.alertPresent() or \ + added_alert.alert_priority > self.activealerts[0].alert_priority: cloudlog.event('alert_add', alert_type=alert_type, enabled=enabled) - self.activealerts.append(this_alert) - self.activealerts.sort() + self.activealerts.append(added_alert) + self.activealerts.sort(key=lambda k: k.alert_priority, reverse=True) + # TODO: cycle through alerts? def process_alerts(self, cur_time): - if self.alertPresent(): - self.alert_start_time = cur_time - self.current_alert = self.activealerts[0] - print self.current_alert + + # first get rid of all the expired alerts + self.activealerts = [a for a in self.activealerts if a.start_time + + max(a.duration_sound, a.duration_hud_alert, a.duration_text) > cur_time] + + ca = self.activealerts[0] if self.alertPresent() else None # start with assuming no alerts self.alert_text_1 = "" self.alert_text_2 = "" + self.alert_status = AlertStatus.normal + self.alert_size = AlertSize.none self.visual_alert = "none" self.audible_alert = "none" - if self.current_alert is not None: - # ewwwww - if self.alert_start_time + self.current_alert.duration_sound > cur_time: - self.audible_alert = self.current_alert.audible_alert - - if self.alert_start_time + self.current_alert.duration_hud_alert > cur_time: - self.visual_alert = self.current_alert.visual_alert + if ca: + if ca.start_time + ca.duration_sound > cur_time: + self.audible_alert = ca.audible_alert - if self.alert_start_time + self.current_alert.duration_text > cur_time: - self.alert_text_1 = self.current_alert.alert_text_1 - self.alert_text_2 = self.current_alert.alert_text_2 + if ca.start_time + ca.duration_hud_alert > cur_time: + self.visual_alert = ca.visual_alert - # disable current alert - if self.alert_start_time + max(self.current_alert.duration_sound, self.current_alert.duration_hud_alert, - self.current_alert.duration_text) < cur_time: - self.current_alert = None - - # reset - self.activealerts = [] + if ca.start_time + ca.duration_text > cur_time: + self.alert_text_1 = ca.alert_text_1 + self.alert_text_2 = ca.alert_text_2 + self.alert_status = ca.alert_status + self.alert_size = ca.alert_size diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 82455e114f1208..f98a0233c6831f 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -10,6 +10,7 @@ class EventTypes: USER_DISABLE = 'userDisable' SOFT_DISABLE = 'softDisable' IMMEDIATE_DISABLE = 'immediateDisable' + PERMANENT = 'permanent' def create_event(name, types): diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index afe87e952e98e1..1feec0f5647a0a 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -8,7 +8,7 @@ from selfdrive.swaglog import cloudlog # 100ms is a rule of thumb estimation of lag from image processing to actuator command -ACTUATORS_DELAY = 0.2 +ACTUATORS_DELAY = 0.1 _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz @@ -28,11 +28,11 @@ class LatControl(object): def __init__(self, VM): self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 - self.setup_mpc() + self.setup_mpc(VM.CP.steerRateCost) - def setup_mpc(self): + def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc - self.libmpc.init() + self.libmpc.init(steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -83,7 +83,7 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if nans: - self.libmpc.init() + self.libmpc.init(VM.CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio if t > self.last_cloudlog_t + 5.0: @@ -95,7 +95,10 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs self.pid.reset() else: dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) + # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution + # constant for 0.05s. + #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) + self.angle_steers_des = self.angle_steers_des_mpc steers_max = get_steer_max(VM.CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp index 7c0484bfc9dd18..53438a11f8f5f3 100644 --- a/selfdrive/controls/lib/lateral_mpc/generator.cpp +++ b/selfdrive/controls/lib/lateral_mpc/generator.cpp @@ -72,14 +72,12 @@ int main( ) // Angular rate error h << (v_ref + 1.0 ) * t; - DMatrix Q(5,5); - Q(0,0) = 1.0; - Q(1,1) = 1.0; - Q(2,2) = 1.0; - - Q(3,3) = 1.0; - - Q(4,4) = 1.0; + BMatrix Q(5,5); Q.setAll(true); + // Q(0,0) = 1.0; + // Q(1,1) = 1.0; + // Q(2,2) = 1.0; + // Q(3,3) = 1.0; + // Q(4,4) = 2.0; // Terminal cost Function hN; @@ -92,18 +90,27 @@ int main( ) // Heading errors hN << (2.0 * v_ref + 1.0 ) * (angle - psi); - DMatrix QN(4,4); - QN(0,0) = 1.0; - QN(1,1) = 1.0; - QN(2,2) = 1.0; - - QN(3,3) = 1.0; + BMatrix QN(4,4); 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 = samplingTime * controlHorizon; + const double tEnd = 2.5; - OCP ocp( tStart, tEnd, controlHorizon ); + OCP ocp( tStart, tEnd, numSteps); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -120,6 +127,7 @@ int main( ) mpc.set( INTEGRATOR_TYPE, INT_RK4 ); mpc.set( NUM_INTEGRATOR_STEPS, 1 * controlHorizon); mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( QP_SOLVER, QP_QPOASES ); diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 290fcb24360888..33bbe1898f4dfe 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -1,11 +1,12 @@ import os +import sys import subprocess from cffi import FFI mpc_dir = os.path.dirname(os.path.abspath(__file__)) libmpc_fn = os.path.join(mpc_dir, "libcommampc.so") -subprocess.check_output(["make", "-j4"], cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) ffi = FFI() ffi.cdef(""" @@ -14,13 +15,13 @@ } state_t; typedef struct { - double x[50]; - double y[50]; - double psi[50]; - double delta[50]; + double x[20]; + double y[20]; + double psi[20]; + double delta[20]; } log_t; -void init(); +void init(double steer_rate_cost); int run_mpc(state_t * x0, log_t * solution, double l_poly[4], double r_poly[4], double p_poly[4], double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width); diff --git a/selfdrive/controls/lib/lateral_mpc/mpc.c b/selfdrive/controls/lib/lateral_mpc/mpc.c index 6157b7653a745e..3c082319eec6ba 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc.c @@ -28,7 +28,7 @@ typedef struct { double delta[N]; } log_t; -void init(){ +void init(double steerRateCost){ acado_initializeSolver(); int i; @@ -42,6 +42,22 @@ void init(){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = 3; + } + acadoVariables.W[25 * i + 0] = 1.0 * f; + acadoVariables.W[25 * i + 6] = 1.0 * f; + acadoVariables.W[25 * i + 12] = 1.0 * f; + acadoVariables.W[25 * i + 18] = 1.0 * f; + acadoVariables.W[25 * i + 24] = steerRateCost * f; + } + acadoVariables.WN[0] = 1.0; + acadoVariables.WN[5] = 1.0; + acadoVariables.WN[10] = 1.0; + acadoVariables.WN[15] = 1.0; } int run_mpc(state_t * x0, log_t * solution, diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h index 4401190a78ee8f..dbb20c566dab6c 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_common.h @@ -62,7 +62,7 @@ extern "C" /** Indicator for fixed initial state. */ #define ACADO_INITIAL_STATE_FIXED 1 /** Number of control/estimation intervals. */ -#define ACADO_N 50 +#define ACADO_N 20 /** Number of online data values. */ #define ACADO_NOD 18 /** Number of path constraints. */ @@ -80,9 +80,7 @@ extern "C" /** Number of references/measurements on the last (N + 1)st node. */ #define ACADO_NYN 4 /** Total number of QP optimization variables. */ -#define ACADO_QP_NV 54 -/** Number of integration steps per shooting interval. */ -#define ACADO_RK_NIS 1 +#define ACADO_QP_NV 24 /** Number of Runge-Kutta stages per integration step. */ #define ACADO_RK_NSTAGES 4 /** Providing interface for arrival cost. */ @@ -90,7 +88,7 @@ extern "C" /** Indicator for usage of non-hard-coded linear terms in the objective. */ #define ACADO_USE_LINEAR_TERMS 0 /** Indicator for type of fixed weighting matrices. */ -#define ACADO_WEIGHTING_MATRICES_TYPE 0 +#define ACADO_WEIGHTING_MATRICES_TYPE 2 /* @@ -104,36 +102,42 @@ extern "C" typedef struct ACADOvariables_ { int dummy; -/** Matrix of size: 51 x 4 (row major format) +/** Matrix of size: 21 x 4 (row major format) * - * Matrix containing 51 differential variable vectors. + * Matrix containing 21 differential variable vectors. */ -real_t x[ 204 ]; +real_t x[ 84 ]; -/** Column vector of size: 50 +/** Column vector of size: 20 * - * Matrix containing 50 control variable vectors. + * Matrix containing 20 control variable vectors. */ -real_t u[ 50 ]; +real_t u[ 20 ]; -/** Matrix of size: 51 x 18 (row major format) +/** Matrix of size: 21 x 18 (row major format) * - * Matrix containing 51 online data vectors. + * Matrix containing 21 online data vectors. */ -real_t od[ 918 ]; +real_t od[ 378 ]; -/** Column vector of size: 250 +/** Column vector of size: 100 * - * Matrix containing 50 reference/measurement vectors of size 5 for first 50 nodes. + * Matrix containing 20 reference/measurement vectors of size 5 for first 20 nodes. */ -real_t y[ 250 ]; +real_t y[ 100 ]; /** Column vector of size: 4 * - * Reference/measurement vector for the 51. node. + * Reference/measurement vector for the 21. node. */ real_t yN[ 4 ]; +/** Matrix of size: 100 x 5 (row major format) */ +real_t W[ 500 ]; + +/** Matrix of size: 4 x 4 (row major format) */ +real_t WN[ 16 ]; + /** Column vector of size: 4 * * Current state feedback vector. @@ -165,20 +169,20 @@ real_t rk_kkk[ 96 ]; /** Row vector of size: 43 */ real_t state[ 43 ]; -/** Column vector of size: 200 */ -real_t d[ 200 ]; +/** Column vector of size: 80 */ +real_t d[ 80 ]; -/** Column vector of size: 250 */ -real_t Dy[ 250 ]; +/** Column vector of size: 100 */ +real_t Dy[ 100 ]; /** Column vector of size: 4 */ real_t DyN[ 4 ]; -/** Matrix of size: 200 x 4 (row major format) */ -real_t evGx[ 800 ]; +/** Matrix of size: 80 x 4 (row major format) */ +real_t evGx[ 320 ]; -/** Column vector of size: 200 */ -real_t evGu[ 200 ]; +/** Column vector of size: 80 */ +real_t evGu[ 80 ]; /** Column vector of size: 19 */ real_t objAuxVar[ 19 ]; @@ -189,17 +193,20 @@ real_t objValueIn[ 23 ]; /** Row vector of size: 30 */ real_t objValueOut[ 30 ]; -/** Matrix of size: 200 x 4 (row major format) */ -real_t Q1[ 800 ]; +/** Matrix of size: 80 x 4 (row major format) */ +real_t Q1[ 320 ]; -/** Matrix of size: 200 x 5 (row major format) */ -real_t Q2[ 1000 ]; +/** Matrix of size: 80 x 5 (row major format) */ +real_t Q2[ 400 ]; -/** Column vector of size: 50 */ -real_t R1[ 50 ]; +/** Column vector of size: 20 */ +real_t R1[ 20 ]; -/** Matrix of size: 50 x 5 (row major format) */ -real_t R2[ 250 ]; +/** Matrix of size: 20 x 5 (row major format) */ +real_t R2[ 100 ]; + +/** Column vector of size: 80 */ +real_t S1[ 80 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t QN1[ 16 ]; @@ -213,50 +220,50 @@ real_t Dx0[ 4 ]; /** Matrix of size: 4 x 4 (row major format) */ real_t T[ 16 ]; -/** Column vector of size: 5100 */ -real_t E[ 5100 ]; +/** Column vector of size: 840 */ +real_t E[ 840 ]; -/** Column vector of size: 5100 */ -real_t QE[ 5100 ]; +/** Column vector of size: 840 */ +real_t QE[ 840 ]; -/** Matrix of size: 200 x 4 (row major format) */ -real_t QGx[ 800 ]; +/** Matrix of size: 80 x 4 (row major format) */ +real_t QGx[ 320 ]; -/** Column vector of size: 200 */ -real_t Qd[ 200 ]; +/** Column vector of size: 80 */ +real_t Qd[ 80 ]; -/** Column vector of size: 204 */ -real_t QDy[ 204 ]; +/** Column vector of size: 84 */ +real_t QDy[ 84 ]; -/** Matrix of size: 50 x 4 (row major format) */ -real_t H10[ 200 ]; +/** Matrix of size: 20 x 4 (row major format) */ +real_t H10[ 80 ]; -/** Matrix of size: 54 x 54 (row major format) */ -real_t H[ 2916 ]; +/** Matrix of size: 24 x 24 (row major format) */ +real_t H[ 576 ]; -/** Matrix of size: 100 x 54 (row major format) */ -real_t A[ 5400 ]; +/** Matrix of size: 40 x 24 (row major format) */ +real_t A[ 960 ]; -/** Column vector of size: 54 */ -real_t g[ 54 ]; +/** Column vector of size: 24 */ +real_t g[ 24 ]; -/** Column vector of size: 54 */ -real_t lb[ 54 ]; +/** Column vector of size: 24 */ +real_t lb[ 24 ]; -/** Column vector of size: 54 */ -real_t ub[ 54 ]; +/** Column vector of size: 24 */ +real_t ub[ 24 ]; -/** Column vector of size: 100 */ -real_t lbA[ 100 ]; +/** Column vector of size: 40 */ +real_t lbA[ 40 ]; -/** Column vector of size: 100 */ -real_t ubA[ 100 ]; +/** Column vector of size: 40 */ +real_t ubA[ 40 ]; -/** Column vector of size: 54 */ -real_t x[ 54 ]; +/** Column vector of size: 24 */ +real_t x[ 24 ]; -/** Column vector of size: 154 */ -real_t y[ 154 ]; +/** Column vector of size: 64 */ +real_t y[ 64 ]; } ACADOworkspace; @@ -270,10 +277,11 @@ real_t y[ 154 ]; * * \param rk_eta Working array to pass the input values and return the results. * \param resetIntegrator The internal memory of the integrator can be reset. + * \param rk_index Number of the shooting interval. * * \return Status code of the integrator. */ -int acado_integrate( real_t* const rk_eta, int resetIntegrator ); +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ); /** Export of an ACADO symbolic function. * @@ -306,7 +314,7 @@ void acado_initializeNodesByForwardSimulation( ); /** Shift differential variables vector by one interval. * - * \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation. + * \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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/mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c index afff2ce7c6ede4..b931088991a33d 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_integrator.c @@ -72,11 +72,13 @@ out[23] = (real_t)(1.0000000000000000e+00); } /* Fixed step size:0.05 */ -int acado_integrate( real_t* const rk_eta, int resetIntegrator ) +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 numInts = numSteps[rk_index]; acadoWorkspace.rk_ttt = 0.0000000000000000e+00; rk_eta[4] = 1.0000000000000000e+00; rk_eta[5] = 0.0000000000000000e+00; @@ -120,6 +122,7 @@ acadoWorkspace.rk_xxx[42] = rk_eta[42]; for (run1 = 0; run1 < 1; ++run1) { +for(run1 = 0; run1 < numInts; run1++ ) { acadoWorkspace.rk_xxx[0] = + rk_eta[0]; acadoWorkspace.rk_xxx[1] = + rk_eta[1]; acadoWorkspace.rk_xxx[2] = + rk_eta[2]; @@ -145,107 +148,108 @@ 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.5000000000000001e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +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]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[24] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[25] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[26] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[27] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[28] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[29] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[30] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[31] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[32] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[33] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[34] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[35] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[36] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[37] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[38] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[39] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[40] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[41] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[42] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[43] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[44] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[45] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[46] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-02*acadoWorkspace.rk_kkk[47] + rk_eta[23]; +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]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000003e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +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]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) ); -rk_eta[0] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[0] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[24] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[72]; -rk_eta[1] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[1] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[25] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[73]; -rk_eta[2] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[2] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[26] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[74]; -rk_eta[3] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[3] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[27] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[75]; -rk_eta[4] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[4] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[28] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[76]; -rk_eta[5] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[5] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[29] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[77]; -rk_eta[6] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[6] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[30] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[78]; -rk_eta[7] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[7] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[31] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[79]; -rk_eta[8] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[8] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[32] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[80]; -rk_eta[9] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[9] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[33] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[81]; -rk_eta[10] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[10] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[34] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[82]; -rk_eta[11] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[11] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[35] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[83]; -rk_eta[12] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[12] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[36] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[84]; -rk_eta[13] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[13] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[37] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[85]; -rk_eta[14] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[14] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[38] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[86]; -rk_eta[15] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[15] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[39] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[87]; -rk_eta[16] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[16] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[40] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[88]; -rk_eta[17] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[17] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[41] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[89]; -rk_eta[18] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[18] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[42] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[90]; -rk_eta[19] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[19] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[43] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[91]; -rk_eta[20] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[20] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[44] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[92]; -rk_eta[21] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[21] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[45] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[93]; -rk_eta[22] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[22] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[46] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[94]; -rk_eta[23] += + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[23] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[47] + (real_t)1.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)8.3333333333333332e-03*acadoWorkspace.rk_kkk[95]; +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]; acadoWorkspace.rk_ttt += 1.0000000000000000e+00; } +} error = 0; return error; } diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp index 09c7da96cad333..f2665a45c09b39 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.cpp @@ -40,7 +40,7 @@ int acado_solve( void ) { acado_nWSR = QPOASES_NWSRMAX; - QProblem qp(54, 100); + QProblem qp(24, 40); 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/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp index 30bbd8ab82306a..47d41855e6fe89 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_qpoases_interface.hpp @@ -37,9 +37,9 @@ */ /** Maximum number of optimization variables. */ -#define QPOASES_NVMAX 54 +#define QPOASES_NVMAX 24 /** Maximum number of constraints. */ -#define QPOASES_NCMAX 100 +#define QPOASES_NCMAX 40 /** Maximum number of working set recalculations. */ #define QPOASES_NWSRMAX 500 /** Print level for qpOASES. */ diff --git a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c index 963a36430a9bb7..ed897e3c02ae4c 100644 --- a/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lateral_mpc/mpc_export/acado_solver.c @@ -35,7 +35,7 @@ int ret; int lRun1; ret = 0; -for (lRun1 = 0; lRun1 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1]; @@ -62,7 +62,7 @@ acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 18 + 15]; acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 18 + 16]; acadoWorkspace.state[42] = acadoVariables.od[lRun1 * 18 + 17]; -ret = acado_integrate(acadoWorkspace.state, 1); +ret = acado_integrate(acadoWorkspace.state, 1, lRun1); acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4]; acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5]; @@ -207,28 +207,28 @@ out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000 out[19] = (real_t)(0.0000000000000000e+00); } -void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpQ1, real_t* const tmpQ2 ) +void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 ) { -tmpQ2[0] = + tmpFx[0]; -tmpQ2[1] = + tmpFx[4]; -tmpQ2[2] = + tmpFx[8]; -tmpQ2[3] = + tmpFx[12]; -tmpQ2[4] = + tmpFx[16]; -tmpQ2[5] = + tmpFx[1]; -tmpQ2[6] = + tmpFx[5]; -tmpQ2[7] = + tmpFx[9]; -tmpQ2[8] = + tmpFx[13]; -tmpQ2[9] = + tmpFx[17]; -tmpQ2[10] = + tmpFx[2]; -tmpQ2[11] = + tmpFx[6]; -tmpQ2[12] = + tmpFx[10]; -tmpQ2[13] = + tmpFx[14]; -tmpQ2[14] = + tmpFx[18]; -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[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]; @@ -247,34 +247,34 @@ tmpQ1[14] = + tmpQ2[15]*tmpFx[2] + tmpQ2[16]*tmpFx[6] + tmpQ2[17]*tmpFx[10] + tm tmpQ1[15] = + tmpQ2[15]*tmpFx[3] + tmpQ2[16]*tmpFx[7] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[19]; } -void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpR1, real_t* const tmpR2 ) +void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 ) { -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[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]; } -void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpQN1, real_t* const tmpQN2 ) +void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 ) { -tmpQN2[0] = + tmpFx[0]; -tmpQN2[1] = + tmpFx[4]; -tmpQN2[2] = + tmpFx[8]; -tmpQN2[3] = + tmpFx[12]; -tmpQN2[4] = + tmpFx[1]; -tmpQN2[5] = + tmpFx[5]; -tmpQN2[6] = + tmpFx[9]; -tmpQN2[7] = + tmpFx[13]; -tmpQN2[8] = + tmpFx[2]; -tmpQN2[9] = + tmpFx[6]; -tmpQN2[10] = + tmpFx[10]; -tmpQN2[11] = + tmpFx[14]; -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[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]; @@ -296,7 +296,7 @@ tmpQN1[15] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[11] void acado_evaluateObjective( ) { int runObj; -for (runObj = 0; runObj < 50; ++runObj) +for (runObj = 0; runObj < 20; ++runObj) { acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1]; @@ -329,33 +329,33 @@ acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2]; acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3]; acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4]; -acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 5 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) ); -acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 25 ]), &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 5 ]) ); } -acadoWorkspace.objValueIn[0] = acadoVariables.x[200]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[201]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[202]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[203]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[900]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[901]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[902]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[903]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[904]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[905]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[906]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[907]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[908]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[909]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[910]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[911]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[912]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[913]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[914]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[915]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[916]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[917]; +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[360]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[361]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[362]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[363]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[364]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[365]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[366]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[367]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[368]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[369]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[370]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[371]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[372]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[373]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[374]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[375]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[376]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[377]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; @@ -363,7 +363,7 @@ acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; acadoWorkspace.DyN[3] = acadoWorkspace.objValueOut[3]; -acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoWorkspace.QN1, acadoWorkspace.QN2 ); +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 4 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); } @@ -433,22 +433,22 @@ Gu2[3] = Gu1[3]; void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) { -acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3]; +acadoWorkspace.H[(iRow * 24 + 96) + (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 * 54 + 216) + (iCol + 4)] = R11[0]; +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = R11[0]; } void acado_zeroBlockH11( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] = 0.0000000000000000e+00; +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = 0.0000000000000000e+00; } void acado_copyHTH( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 54 + 216) + (iCol + 4)] = acadoWorkspace.H[(iCol * 54 + 216) + (iRow + 4)]; +acadoWorkspace.H[(iRow * 24 + 96) + (iCol + 4)] = acadoWorkspace.H[(iCol * 24 + 96) + (iRow + 4)]; } void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) @@ -512,18 +512,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[54] = 0.0000000000000000e+00; -acadoWorkspace.H[55] = 0.0000000000000000e+00; -acadoWorkspace.H[56] = 0.0000000000000000e+00; -acadoWorkspace.H[57] = 0.0000000000000000e+00; -acadoWorkspace.H[108] = 0.0000000000000000e+00; -acadoWorkspace.H[109] = 0.0000000000000000e+00; -acadoWorkspace.H[110] = 0.0000000000000000e+00; -acadoWorkspace.H[111] = 0.0000000000000000e+00; -acadoWorkspace.H[162] = 0.0000000000000000e+00; -acadoWorkspace.H[163] = 0.0000000000000000e+00; -acadoWorkspace.H[164] = 0.0000000000000000e+00; -acadoWorkspace.H[165] = 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; } void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) @@ -532,18 +532,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[54] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12]; -acadoWorkspace.H[55] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13]; -acadoWorkspace.H[56] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14]; -acadoWorkspace.H[57] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15]; -acadoWorkspace.H[108] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12]; -acadoWorkspace.H[109] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13]; -acadoWorkspace.H[110] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14]; -acadoWorkspace.H[111] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15]; -acadoWorkspace.H[162] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12]; -acadoWorkspace.H[163] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13]; -acadoWorkspace.H[164] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14]; -acadoWorkspace.H[165] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*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]; } void acado_macCTSlx( real_t* const C0, real_t* const g0 ) @@ -571,24 +571,332 @@ int lRun2; int lRun3; int lRun4; int lRun5; -/** Row vector of size: 100 */ -static const int xBoundIndices[ 100 ] = -{ 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, 86, 87, 90, 91, 94, 95, 98, 99, 102, 103, 106, 107, 110, 111, 114, 115, 118, 119, 122, 123, 126, 127, 130, 131, 134, 135, 138, 139, 142, 143, 146, 147, 150, 151, 154, 155, 158, 159, 162, 163, 166, 167, 170, 171, 174, 175, 178, 179, 182, 183, 186, 187, 190, 191, 194, 195, 198, 199, 202, 203 }; +/** 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 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); -for (lRun1 = 1; lRun1 < 50; ++lRun1) -{ -acado_moveGxT( &(acadoWorkspace.evGx[ lRun1 * 16 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ lRun1 * 4-4 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]), &(acadoWorkspace.d[ lRun1 * 4 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ lRun1 * 16-16 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]) ); -for (lRun2 = 0; lRun2 < lRun1; ++lRun2) -{ -lRun4 = (((lRun1) * (lRun1-1)) / (2)) + (lRun2); -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.E[ lRun3 * 4 ]) ); -} -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_moveGuE( &(acadoWorkspace.evGu[ lRun1 * 4 ]), &(acadoWorkspace.E[ lRun3 * 4 ]) ); -} +acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T ); +acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) ); +acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 16 ]) ); + +acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 4 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 4 ]), &(acadoWorkspace.E[ 8 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.d[ 8 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.evGx[ 32 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.E[ 12 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.E[ 16 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 8 ]), &(acadoWorkspace.E[ 20 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.d[ 12 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.evGx[ 48 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.E[ 28 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.E[ 32 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 36 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.d[ 16 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.evGx[ 64 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 40 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.E[ 44 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.E[ 48 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 52 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 16 ]), &(acadoWorkspace.E[ 56 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.d[ 20 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.evGx[ 80 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.E[ 60 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.E[ 64 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 68 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.E[ 72 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.E[ 76 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 20 ]), &(acadoWorkspace.E[ 80 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.d[ 24 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.evGx[ 96 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 84 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.E[ 88 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.E[ 92 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 96 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.E[ 100 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.E[ 104 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 108 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.d[ 28 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.evGx[ 112 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 112 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.E[ 116 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.E[ 120 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 124 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.E[ 128 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.E[ 132 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 136 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 28 ]), &(acadoWorkspace.E[ 140 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.d[ 32 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.evGx[ 128 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.E[ 144 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.E[ 148 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 152 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.E[ 156 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.E[ 160 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 164 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.E[ 168 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.E[ 172 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 32 ]), &(acadoWorkspace.E[ 176 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 36 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.evGx[ 144 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 180 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.E[ 184 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.E[ 188 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 192 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.E[ 196 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.E[ 200 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 204 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.E[ 208 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.E[ 212 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 216 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.d[ 40 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 160 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 220 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.E[ 224 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.E[ 228 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 232 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.E[ 236 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.E[ 240 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 244 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.E[ 248 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.E[ 252 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 256 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 40 ]), &(acadoWorkspace.E[ 260 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.d[ 44 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.evGx[ 176 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.E[ 264 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.E[ 268 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 272 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.E[ 276 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.E[ 280 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 284 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.E[ 288 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.E[ 292 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 296 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.E[ 300 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.E[ 304 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 44 ]), &(acadoWorkspace.E[ 308 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.d[ 48 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.evGx[ 192 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 312 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.E[ 316 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.E[ 320 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 324 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.E[ 328 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.E[ 332 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 336 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.E[ 340 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.E[ 344 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 348 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.E[ 352 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.E[ 356 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 360 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.d[ 52 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.evGx[ 208 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 364 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.E[ 368 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.E[ 372 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 376 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.E[ 380 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.E[ 384 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 388 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.E[ 392 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.E[ 396 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 400 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.E[ 404 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.E[ 408 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 412 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 52 ]), &(acadoWorkspace.E[ 416 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.d[ 56 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.evGx[ 224 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.E[ 420 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.E[ 424 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 428 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.E[ 432 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.E[ 436 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 440 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.E[ 444 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.E[ 448 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 452 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.E[ 456 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.E[ 460 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 464 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.E[ 468 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.E[ 472 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 56 ]), &(acadoWorkspace.E[ 476 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.d[ 60 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.evGx[ 240 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 480 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.E[ 484 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.E[ 488 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 492 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.E[ 496 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.E[ 500 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 504 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.E[ 508 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.E[ 512 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 516 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.E[ 520 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.E[ 524 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 528 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.E[ 532 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E[ 536 ]) ); + +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 ]) ); @@ -609,52 +917,218 @@ acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(ac 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.Q1[ 320 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 512 ]), &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 528 ]), &(acadoWorkspace.evGx[ 512 ]), &(acadoWorkspace.QGx[ 512 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 544 ]), &(acadoWorkspace.evGx[ 528 ]), &(acadoWorkspace.QGx[ 528 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 560 ]), &(acadoWorkspace.evGx[ 544 ]), &(acadoWorkspace.QGx[ 544 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.evGx[ 560 ]), &(acadoWorkspace.QGx[ 560 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 592 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 608 ]), &(acadoWorkspace.evGx[ 592 ]), &(acadoWorkspace.QGx[ 592 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 624 ]), &(acadoWorkspace.evGx[ 608 ]), &(acadoWorkspace.QGx[ 608 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 640 ]), &(acadoWorkspace.evGx[ 624 ]), &(acadoWorkspace.QGx[ 624 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 656 ]), &(acadoWorkspace.evGx[ 640 ]), &(acadoWorkspace.QGx[ 640 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 672 ]), &(acadoWorkspace.evGx[ 656 ]), &(acadoWorkspace.QGx[ 656 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 688 ]), &(acadoWorkspace.evGx[ 672 ]), &(acadoWorkspace.QGx[ 672 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 704 ]), &(acadoWorkspace.evGx[ 688 ]), &(acadoWorkspace.QGx[ 688 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.evGx[ 704 ]), &(acadoWorkspace.QGx[ 704 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 736 ]), &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 752 ]), &(acadoWorkspace.evGx[ 736 ]), &(acadoWorkspace.QGx[ 736 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 768 ]), &(acadoWorkspace.evGx[ 752 ]), &(acadoWorkspace.QGx[ 752 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 784 ]), &(acadoWorkspace.evGx[ 768 ]), &(acadoWorkspace.QGx[ 768 ]) ); -acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 784 ]), &(acadoWorkspace.QGx[ 784 ]) ); - -for (lRun1 = 0; lRun1 < 49; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( &(acadoWorkspace.Q1[ lRun1 * 16 + 16 ]), &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QE[ lRun3 * 4 ]) ); -} -} +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) ); -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QE[ lRun3 * 4 ]) ); -} +acado_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) ); +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_zeroBlockH00( ); acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); @@ -677,84 +1151,2551 @@ 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_multCTQC( &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 512 ]), &(acadoWorkspace.QGx[ 512 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 528 ]), &(acadoWorkspace.QGx[ 528 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 544 ]), &(acadoWorkspace.QGx[ 544 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 560 ]), &(acadoWorkspace.QGx[ 560 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 592 ]), &(acadoWorkspace.QGx[ 592 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 608 ]), &(acadoWorkspace.QGx[ 608 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 624 ]), &(acadoWorkspace.QGx[ 624 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 640 ]), &(acadoWorkspace.QGx[ 640 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 656 ]), &(acadoWorkspace.QGx[ 656 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 672 ]), &(acadoWorkspace.QGx[ 672 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 688 ]), &(acadoWorkspace.QGx[ 688 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 704 ]), &(acadoWorkspace.QGx[ 704 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 736 ]), &(acadoWorkspace.QGx[ 736 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 752 ]), &(acadoWorkspace.QGx[ 752 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 768 ]), &(acadoWorkspace.QGx[ 768 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 784 ]), &(acadoWorkspace.QGx[ 784 ]) ); - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -acado_zeroBlockH10( &(acadoWorkspace.H10[ lRun1 * 4 ]) ); -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_multQETGx( &(acadoWorkspace.QE[ lRun3 * 4 ]), &(acadoWorkspace.evGx[ lRun2 * 16 ]), &(acadoWorkspace.H10[ lRun1 * 4 ]) ); -} -} -for (lRun1 = 0;lRun1 < 4; ++lRun1) -for (lRun2 = 0;lRun2 < 50; ++lRun2) -acadoWorkspace.H[(lRun1 * 54) + (lRun2 + 4)] = acadoWorkspace.H10[(lRun2 * 4) + (lRun1)]; +acado_zeroBlockH10( acadoWorkspace.H10 ); +acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.H10 ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 4 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 4 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 8 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 8 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 12 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 16 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 16 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 20 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 20 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 24 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 28 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 28 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 32 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 32 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 36 ]) ); +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 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 40 ]) ); +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 ]) ); -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -acado_setBlockH11_R1( lRun1, lRun1, &(acadoWorkspace.R1[ lRun1 ]) ); -lRun2 = lRun1; -for (lRun3 = lRun1; lRun3 < 50; ++lRun3) -{ -lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); -lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); -acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.QE[ lRun5 * 4 ]) ); -} -for (lRun2 = lRun1 + 1; lRun2 < 50; ++lRun2) -{ -acado_zeroBlockH11( lRun1, lRun2 ); -for (lRun3 = lRun2; lRun3 < 50; ++lRun3) -{ -lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); -lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); -acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 4 ]), &(acadoWorkspace.QE[ lRun5 * 4 ]) ); -} -} -} +acadoWorkspace.H[4] = acadoWorkspace.H10[0]; +acadoWorkspace.H[5] = acadoWorkspace.H10[4]; +acadoWorkspace.H[6] = acadoWorkspace.H10[8]; +acadoWorkspace.H[7] = acadoWorkspace.H10[12]; +acadoWorkspace.H[8] = acadoWorkspace.H10[16]; +acadoWorkspace.H[9] = acadoWorkspace.H10[20]; +acadoWorkspace.H[10] = acadoWorkspace.H10[24]; +acadoWorkspace.H[11] = acadoWorkspace.H10[28]; +acadoWorkspace.H[12] = acadoWorkspace.H10[32]; +acadoWorkspace.H[13] = acadoWorkspace.H10[36]; +acadoWorkspace.H[14] = acadoWorkspace.H10[40]; +acadoWorkspace.H[15] = acadoWorkspace.H10[44]; +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]; -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1; ++lRun2) -{ -acado_copyHTH( lRun1, lRun2 ); -} -} +acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); +acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +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 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 268 ]) ); +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 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) ); +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 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) ); +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 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +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 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) ); +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 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); -for (lRun1 = 0;lRun1 < 50; ++lRun1) -for (lRun2 = 0;lRun2 < 4; ++lRun2) -acadoWorkspace.H[(lRun1 * 54 + 216) + (lRun2)] = acadoWorkspace.H10[(lRun1 * 4) + (lRun2)]; +acado_zeroBlockH11( 4, 10 ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 260 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) ); +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 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +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 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) ); +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 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) ); +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 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +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 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) ); +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 ]) ); +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 ]) ); +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 ); +acado_copyHTH( 2, 0 ); +acado_copyHTH( 2, 1 ); +acado_copyHTH( 3, 0 ); +acado_copyHTH( 3, 1 ); +acado_copyHTH( 3, 2 ); +acado_copyHTH( 4, 0 ); +acado_copyHTH( 4, 1 ); +acado_copyHTH( 4, 2 ); +acado_copyHTH( 4, 3 ); +acado_copyHTH( 5, 0 ); +acado_copyHTH( 5, 1 ); +acado_copyHTH( 5, 2 ); +acado_copyHTH( 5, 3 ); +acado_copyHTH( 5, 4 ); +acado_copyHTH( 6, 0 ); +acado_copyHTH( 6, 1 ); +acado_copyHTH( 6, 2 ); +acado_copyHTH( 6, 3 ); +acado_copyHTH( 6, 4 ); +acado_copyHTH( 6, 5 ); +acado_copyHTH( 7, 0 ); +acado_copyHTH( 7, 1 ); +acado_copyHTH( 7, 2 ); +acado_copyHTH( 7, 3 ); +acado_copyHTH( 7, 4 ); +acado_copyHTH( 7, 5 ); +acado_copyHTH( 7, 6 ); +acado_copyHTH( 8, 0 ); +acado_copyHTH( 8, 1 ); +acado_copyHTH( 8, 2 ); +acado_copyHTH( 8, 3 ); +acado_copyHTH( 8, 4 ); +acado_copyHTH( 8, 5 ); +acado_copyHTH( 8, 6 ); +acado_copyHTH( 8, 7 ); +acado_copyHTH( 9, 0 ); +acado_copyHTH( 9, 1 ); +acado_copyHTH( 9, 2 ); +acado_copyHTH( 9, 3 ); +acado_copyHTH( 9, 4 ); +acado_copyHTH( 9, 5 ); +acado_copyHTH( 9, 6 ); +acado_copyHTH( 9, 7 ); +acado_copyHTH( 9, 8 ); +acado_copyHTH( 10, 0 ); +acado_copyHTH( 10, 1 ); +acado_copyHTH( 10, 2 ); +acado_copyHTH( 10, 3 ); +acado_copyHTH( 10, 4 ); +acado_copyHTH( 10, 5 ); +acado_copyHTH( 10, 6 ); +acado_copyHTH( 10, 7 ); +acado_copyHTH( 10, 8 ); +acado_copyHTH( 10, 9 ); +acado_copyHTH( 11, 0 ); +acado_copyHTH( 11, 1 ); +acado_copyHTH( 11, 2 ); +acado_copyHTH( 11, 3 ); +acado_copyHTH( 11, 4 ); +acado_copyHTH( 11, 5 ); +acado_copyHTH( 11, 6 ); +acado_copyHTH( 11, 7 ); +acado_copyHTH( 11, 8 ); +acado_copyHTH( 11, 9 ); +acado_copyHTH( 11, 10 ); +acado_copyHTH( 12, 0 ); +acado_copyHTH( 12, 1 ); +acado_copyHTH( 12, 2 ); +acado_copyHTH( 12, 3 ); +acado_copyHTH( 12, 4 ); +acado_copyHTH( 12, 5 ); +acado_copyHTH( 12, 6 ); +acado_copyHTH( 12, 7 ); +acado_copyHTH( 12, 8 ); +acado_copyHTH( 12, 9 ); +acado_copyHTH( 12, 10 ); +acado_copyHTH( 12, 11 ); +acado_copyHTH( 13, 0 ); +acado_copyHTH( 13, 1 ); +acado_copyHTH( 13, 2 ); +acado_copyHTH( 13, 3 ); +acado_copyHTH( 13, 4 ); +acado_copyHTH( 13, 5 ); +acado_copyHTH( 13, 6 ); +acado_copyHTH( 13, 7 ); +acado_copyHTH( 13, 8 ); +acado_copyHTH( 13, 9 ); +acado_copyHTH( 13, 10 ); +acado_copyHTH( 13, 11 ); +acado_copyHTH( 13, 12 ); +acado_copyHTH( 14, 0 ); +acado_copyHTH( 14, 1 ); +acado_copyHTH( 14, 2 ); +acado_copyHTH( 14, 3 ); +acado_copyHTH( 14, 4 ); +acado_copyHTH( 14, 5 ); +acado_copyHTH( 14, 6 ); +acado_copyHTH( 14, 7 ); +acado_copyHTH( 14, 8 ); +acado_copyHTH( 14, 9 ); +acado_copyHTH( 14, 10 ); +acado_copyHTH( 14, 11 ); +acado_copyHTH( 14, 12 ); +acado_copyHTH( 14, 13 ); +acado_copyHTH( 15, 0 ); +acado_copyHTH( 15, 1 ); +acado_copyHTH( 15, 2 ); +acado_copyHTH( 15, 3 ); +acado_copyHTH( 15, 4 ); +acado_copyHTH( 15, 5 ); +acado_copyHTH( 15, 6 ); +acado_copyHTH( 15, 7 ); +acado_copyHTH( 15, 8 ); +acado_copyHTH( 15, 9 ); +acado_copyHTH( 15, 10 ); +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]; acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd ); acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) ); @@ -775,37 +3716,7 @@ acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWo 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_multQ1d( &(acadoWorkspace.Q1[ 320 ]), &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.d[ 80 ]), &(acadoWorkspace.Qd[ 80 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.Qd[ 84 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.d[ 88 ]), &(acadoWorkspace.Qd[ 88 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.d[ 92 ]), &(acadoWorkspace.Qd[ 92 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.Qd[ 96 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.d[ 100 ]), &(acadoWorkspace.Qd[ 100 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.d[ 104 ]), &(acadoWorkspace.Qd[ 104 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.Qd[ 108 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.d[ 112 ]), &(acadoWorkspace.Qd[ 112 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.d[ 116 ]), &(acadoWorkspace.Qd[ 116 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.d[ 120 ]), &(acadoWorkspace.Qd[ 120 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 512 ]), &(acadoWorkspace.d[ 124 ]), &(acadoWorkspace.Qd[ 124 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 528 ]), &(acadoWorkspace.d[ 128 ]), &(acadoWorkspace.Qd[ 128 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 544 ]), &(acadoWorkspace.d[ 132 ]), &(acadoWorkspace.Qd[ 132 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 560 ]), &(acadoWorkspace.d[ 136 ]), &(acadoWorkspace.Qd[ 136 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.d[ 140 ]), &(acadoWorkspace.Qd[ 140 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 592 ]), &(acadoWorkspace.d[ 144 ]), &(acadoWorkspace.Qd[ 144 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 608 ]), &(acadoWorkspace.d[ 148 ]), &(acadoWorkspace.Qd[ 148 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 624 ]), &(acadoWorkspace.d[ 152 ]), &(acadoWorkspace.Qd[ 152 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 640 ]), &(acadoWorkspace.d[ 156 ]), &(acadoWorkspace.Qd[ 156 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 656 ]), &(acadoWorkspace.d[ 160 ]), &(acadoWorkspace.Qd[ 160 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 672 ]), &(acadoWorkspace.d[ 164 ]), &(acadoWorkspace.Qd[ 164 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 688 ]), &(acadoWorkspace.d[ 168 ]), &(acadoWorkspace.Qd[ 168 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 704 ]), &(acadoWorkspace.d[ 172 ]), &(acadoWorkspace.Qd[ 172 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.d[ 176 ]), &(acadoWorkspace.Qd[ 176 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 736 ]), &(acadoWorkspace.d[ 180 ]), &(acadoWorkspace.Qd[ 180 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 752 ]), &(acadoWorkspace.d[ 184 ]), &(acadoWorkspace.Qd[ 184 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 768 ]), &(acadoWorkspace.d[ 188 ]), &(acadoWorkspace.Qd[ 188 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 784 ]), &(acadoWorkspace.d[ 192 ]), &(acadoWorkspace.Qd[ 192 ]) ); -acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 196 ]), &(acadoWorkspace.Qd[ 196 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) ); acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g ); @@ -827,44 +3738,216 @@ 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_macCTSlx( &(acadoWorkspace.evGx[ 320 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 336 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 352 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 368 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 384 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 400 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 416 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 448 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 464 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 480 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 496 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 512 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 528 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 544 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 560 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 592 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 608 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 624 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 640 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 656 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 672 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 688 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 704 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 720 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 736 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 752 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 768 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 784 ]), acadoWorkspace.g ); -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_macETSlu( &(acadoWorkspace.QE[ lRun3 * 4 ]), &(acadoWorkspace.g[ lRun1 + 4 ]) ); -} -} +acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 4 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.g[ 5 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.g[ 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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.g[ 8 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.g[ 9 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 10 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.g[ 11 ]) ); +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 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.g[ 12 ]) ); +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 ]) ); +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.0000000000000001e-01 - acadoVariables.u[0]; acadoWorkspace.lb[5] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[1]; acadoWorkspace.lb[6] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[2]; @@ -885,36 +3968,6 @@ acadoWorkspace.lb[20] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[16]; acadoWorkspace.lb[21] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[17]; acadoWorkspace.lb[22] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[18]; acadoWorkspace.lb[23] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[19]; -acadoWorkspace.lb[24] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[20]; -acadoWorkspace.lb[25] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[21]; -acadoWorkspace.lb[26] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[22]; -acadoWorkspace.lb[27] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[23]; -acadoWorkspace.lb[28] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[24]; -acadoWorkspace.lb[29] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[25]; -acadoWorkspace.lb[30] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[26]; -acadoWorkspace.lb[31] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[27]; -acadoWorkspace.lb[32] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[28]; -acadoWorkspace.lb[33] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[29]; -acadoWorkspace.lb[34] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[30]; -acadoWorkspace.lb[35] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[31]; -acadoWorkspace.lb[36] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[32]; -acadoWorkspace.lb[37] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[33]; -acadoWorkspace.lb[38] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[34]; -acadoWorkspace.lb[39] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[35]; -acadoWorkspace.lb[40] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[36]; -acadoWorkspace.lb[41] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[37]; -acadoWorkspace.lb[42] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[38]; -acadoWorkspace.lb[43] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[39]; -acadoWorkspace.lb[44] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[40]; -acadoWorkspace.lb[45] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[41]; -acadoWorkspace.lb[46] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[42]; -acadoWorkspace.lb[47] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[43]; -acadoWorkspace.lb[48] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[44]; -acadoWorkspace.lb[49] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[45]; -acadoWorkspace.lb[50] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[46]; -acadoWorkspace.lb[51] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[47]; -acadoWorkspace.lb[52] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[48]; -acadoWorkspace.lb[53] = (real_t)-1.0000000000000001e-01 - acadoVariables.u[49]; acadoWorkspace.ub[4] = (real_t)1.0000000000000001e-01 - acadoVariables.u[0]; acadoWorkspace.ub[5] = (real_t)1.0000000000000001e-01 - acadoVariables.u[1]; acadoWorkspace.ub[6] = (real_t)1.0000000000000001e-01 - acadoVariables.u[2]; @@ -935,49 +3988,19 @@ acadoWorkspace.ub[20] = (real_t)1.0000000000000001e-01 - acadoVariables.u[16]; acadoWorkspace.ub[21] = (real_t)1.0000000000000001e-01 - acadoVariables.u[17]; acadoWorkspace.ub[22] = (real_t)1.0000000000000001e-01 - acadoVariables.u[18]; acadoWorkspace.ub[23] = (real_t)1.0000000000000001e-01 - acadoVariables.u[19]; -acadoWorkspace.ub[24] = (real_t)1.0000000000000001e-01 - acadoVariables.u[20]; -acadoWorkspace.ub[25] = (real_t)1.0000000000000001e-01 - acadoVariables.u[21]; -acadoWorkspace.ub[26] = (real_t)1.0000000000000001e-01 - acadoVariables.u[22]; -acadoWorkspace.ub[27] = (real_t)1.0000000000000001e-01 - acadoVariables.u[23]; -acadoWorkspace.ub[28] = (real_t)1.0000000000000001e-01 - acadoVariables.u[24]; -acadoWorkspace.ub[29] = (real_t)1.0000000000000001e-01 - acadoVariables.u[25]; -acadoWorkspace.ub[30] = (real_t)1.0000000000000001e-01 - acadoVariables.u[26]; -acadoWorkspace.ub[31] = (real_t)1.0000000000000001e-01 - acadoVariables.u[27]; -acadoWorkspace.ub[32] = (real_t)1.0000000000000001e-01 - acadoVariables.u[28]; -acadoWorkspace.ub[33] = (real_t)1.0000000000000001e-01 - acadoVariables.u[29]; -acadoWorkspace.ub[34] = (real_t)1.0000000000000001e-01 - acadoVariables.u[30]; -acadoWorkspace.ub[35] = (real_t)1.0000000000000001e-01 - acadoVariables.u[31]; -acadoWorkspace.ub[36] = (real_t)1.0000000000000001e-01 - acadoVariables.u[32]; -acadoWorkspace.ub[37] = (real_t)1.0000000000000001e-01 - acadoVariables.u[33]; -acadoWorkspace.ub[38] = (real_t)1.0000000000000001e-01 - acadoVariables.u[34]; -acadoWorkspace.ub[39] = (real_t)1.0000000000000001e-01 - acadoVariables.u[35]; -acadoWorkspace.ub[40] = (real_t)1.0000000000000001e-01 - acadoVariables.u[36]; -acadoWorkspace.ub[41] = (real_t)1.0000000000000001e-01 - acadoVariables.u[37]; -acadoWorkspace.ub[42] = (real_t)1.0000000000000001e-01 - acadoVariables.u[38]; -acadoWorkspace.ub[43] = (real_t)1.0000000000000001e-01 - acadoVariables.u[39]; -acadoWorkspace.ub[44] = (real_t)1.0000000000000001e-01 - acadoVariables.u[40]; -acadoWorkspace.ub[45] = (real_t)1.0000000000000001e-01 - acadoVariables.u[41]; -acadoWorkspace.ub[46] = (real_t)1.0000000000000001e-01 - acadoVariables.u[42]; -acadoWorkspace.ub[47] = (real_t)1.0000000000000001e-01 - acadoVariables.u[43]; -acadoWorkspace.ub[48] = (real_t)1.0000000000000001e-01 - acadoVariables.u[44]; -acadoWorkspace.ub[49] = (real_t)1.0000000000000001e-01 - acadoVariables.u[45]; -acadoWorkspace.ub[50] = (real_t)1.0000000000000001e-01 - acadoVariables.u[46]; -acadoWorkspace.ub[51] = (real_t)1.0000000000000001e-01 - acadoVariables.u[47]; -acadoWorkspace.ub[52] = (real_t)1.0000000000000001e-01 - acadoVariables.u[48]; -acadoWorkspace.ub[53] = (real_t)1.0000000000000001e-01 - acadoVariables.u[49]; - -for (lRun1 = 0; lRun1 < 100; ++lRun1) + +for (lRun1 = 0; lRun1 < 40; ++lRun1) { lRun3 = xBoundIndices[ lRun1 ] - 4; lRun4 = ((lRun3) / (4)) + (1); -acadoWorkspace.A[lRun1 * 54] = acadoWorkspace.evGx[lRun3 * 4]; -acadoWorkspace.A[lRun1 * 54 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1]; -acadoWorkspace.A[lRun1 * 54 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2]; -acadoWorkspace.A[lRun1 * 54 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3]; +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]; for (lRun2 = 0; lRun2 < lRun4; ++lRun2) { lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4)); -acadoWorkspace.A[(lRun1 * 54) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; +acadoWorkspace.A[(lRun1 * 24) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; } } @@ -985,9 +4008,6 @@ acadoWorkspace.A[(lRun1 * 54) + (lRun2 + 4)] = acadoWorkspace.E[lRun5]; void acado_condenseFdb( ) { -int lRun1; -int lRun2; -int lRun3; real_t tmp; acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; @@ -995,9 +4015,106 @@ acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1]; acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2]; acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3]; -for (lRun2 = 0; lRun2 < 250; ++lRun2) -acadoWorkspace.Dy[lRun2] -= acadoVariables.y[lRun2]; - +acadoWorkspace.Dy[0] -= acadoVariables.y[0]; +acadoWorkspace.Dy[1] -= acadoVariables.y[1]; +acadoWorkspace.Dy[2] -= acadoVariables.y[2]; +acadoWorkspace.Dy[3] -= acadoVariables.y[3]; +acadoWorkspace.Dy[4] -= acadoVariables.y[4]; +acadoWorkspace.Dy[5] -= acadoVariables.y[5]; +acadoWorkspace.Dy[6] -= acadoVariables.y[6]; +acadoWorkspace.Dy[7] -= acadoVariables.y[7]; +acadoWorkspace.Dy[8] -= acadoVariables.y[8]; +acadoWorkspace.Dy[9] -= acadoVariables.y[9]; +acadoWorkspace.Dy[10] -= acadoVariables.y[10]; +acadoWorkspace.Dy[11] -= acadoVariables.y[11]; +acadoWorkspace.Dy[12] -= acadoVariables.y[12]; +acadoWorkspace.Dy[13] -= acadoVariables.y[13]; +acadoWorkspace.Dy[14] -= acadoVariables.y[14]; +acadoWorkspace.Dy[15] -= acadoVariables.y[15]; +acadoWorkspace.Dy[16] -= acadoVariables.y[16]; +acadoWorkspace.Dy[17] -= acadoVariables.y[17]; +acadoWorkspace.Dy[18] -= acadoVariables.y[18]; +acadoWorkspace.Dy[19] -= acadoVariables.y[19]; +acadoWorkspace.Dy[20] -= acadoVariables.y[20]; +acadoWorkspace.Dy[21] -= acadoVariables.y[21]; +acadoWorkspace.Dy[22] -= acadoVariables.y[22]; +acadoWorkspace.Dy[23] -= acadoVariables.y[23]; +acadoWorkspace.Dy[24] -= acadoVariables.y[24]; +acadoWorkspace.Dy[25] -= acadoVariables.y[25]; +acadoWorkspace.Dy[26] -= acadoVariables.y[26]; +acadoWorkspace.Dy[27] -= acadoVariables.y[27]; +acadoWorkspace.Dy[28] -= acadoVariables.y[28]; +acadoWorkspace.Dy[29] -= acadoVariables.y[29]; +acadoWorkspace.Dy[30] -= acadoVariables.y[30]; +acadoWorkspace.Dy[31] -= acadoVariables.y[31]; +acadoWorkspace.Dy[32] -= acadoVariables.y[32]; +acadoWorkspace.Dy[33] -= acadoVariables.y[33]; +acadoWorkspace.Dy[34] -= acadoVariables.y[34]; +acadoWorkspace.Dy[35] -= acadoVariables.y[35]; +acadoWorkspace.Dy[36] -= acadoVariables.y[36]; +acadoWorkspace.Dy[37] -= acadoVariables.y[37]; +acadoWorkspace.Dy[38] -= acadoVariables.y[38]; +acadoWorkspace.Dy[39] -= acadoVariables.y[39]; +acadoWorkspace.Dy[40] -= acadoVariables.y[40]; +acadoWorkspace.Dy[41] -= acadoVariables.y[41]; +acadoWorkspace.Dy[42] -= acadoVariables.y[42]; +acadoWorkspace.Dy[43] -= acadoVariables.y[43]; +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]; @@ -1023,36 +4140,6 @@ acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWo 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[ 100 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.g[ 24 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 105 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.g[ 25 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 110 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.g[ 26 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 115 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.g[ 27 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 120 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.g[ 28 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 125 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.g[ 29 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 130 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.g[ 30 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 135 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.g[ 31 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 140 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.g[ 32 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 145 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.g[ 33 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 150 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.g[ 34 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 155 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.g[ 35 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 160 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.g[ 36 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 165 ]), &(acadoWorkspace.Dy[ 165 ]), &(acadoWorkspace.g[ 37 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 170 ]), &(acadoWorkspace.Dy[ 170 ]), &(acadoWorkspace.g[ 38 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 175 ]), &(acadoWorkspace.Dy[ 175 ]), &(acadoWorkspace.g[ 39 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 180 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.g[ 40 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 185 ]), &(acadoWorkspace.Dy[ 185 ]), &(acadoWorkspace.g[ 41 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 190 ]), &(acadoWorkspace.Dy[ 190 ]), &(acadoWorkspace.g[ 42 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 195 ]), &(acadoWorkspace.Dy[ 195 ]), &(acadoWorkspace.g[ 43 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 200 ]), &(acadoWorkspace.Dy[ 200 ]), &(acadoWorkspace.g[ 44 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 205 ]), &(acadoWorkspace.Dy[ 205 ]), &(acadoWorkspace.g[ 45 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 210 ]), &(acadoWorkspace.Dy[ 210 ]), &(acadoWorkspace.g[ 46 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 215 ]), &(acadoWorkspace.Dy[ 215 ]), &(acadoWorkspace.g[ 47 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 220 ]), &(acadoWorkspace.Dy[ 220 ]), &(acadoWorkspace.g[ 48 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 225 ]), &(acadoWorkspace.Dy[ 225 ]), &(acadoWorkspace.g[ 49 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 230 ]), &(acadoWorkspace.Dy[ 230 ]), &(acadoWorkspace.g[ 50 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 235 ]), &(acadoWorkspace.Dy[ 235 ]), &(acadoWorkspace.g[ 51 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 240 ]), &(acadoWorkspace.Dy[ 240 ]), &(acadoWorkspace.g[ 52 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 245 ]), &(acadoWorkspace.Dy[ 245 ]), &(acadoWorkspace.g[ 53 ]) ); acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) ); @@ -1074,60 +4161,309 @@ acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoW acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) ); acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) ); acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 400 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.QDy[ 80 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 420 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.QDy[ 84 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 440 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.QDy[ 88 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 460 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.QDy[ 92 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 480 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.QDy[ 96 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 500 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.QDy[ 100 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 520 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.QDy[ 104 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 540 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.QDy[ 108 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 560 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.QDy[ 112 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 580 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.QDy[ 116 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 600 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.QDy[ 120 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 620 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.QDy[ 124 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 640 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.QDy[ 128 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 660 ]), &(acadoWorkspace.Dy[ 165 ]), &(acadoWorkspace.QDy[ 132 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 680 ]), &(acadoWorkspace.Dy[ 170 ]), &(acadoWorkspace.QDy[ 136 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 700 ]), &(acadoWorkspace.Dy[ 175 ]), &(acadoWorkspace.QDy[ 140 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 720 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.QDy[ 144 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 740 ]), &(acadoWorkspace.Dy[ 185 ]), &(acadoWorkspace.QDy[ 148 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 760 ]), &(acadoWorkspace.Dy[ 190 ]), &(acadoWorkspace.QDy[ 152 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 780 ]), &(acadoWorkspace.Dy[ 195 ]), &(acadoWorkspace.QDy[ 156 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 800 ]), &(acadoWorkspace.Dy[ 200 ]), &(acadoWorkspace.QDy[ 160 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 820 ]), &(acadoWorkspace.Dy[ 205 ]), &(acadoWorkspace.QDy[ 164 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 840 ]), &(acadoWorkspace.Dy[ 210 ]), &(acadoWorkspace.QDy[ 168 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 860 ]), &(acadoWorkspace.Dy[ 215 ]), &(acadoWorkspace.QDy[ 172 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 880 ]), &(acadoWorkspace.Dy[ 220 ]), &(acadoWorkspace.QDy[ 176 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 900 ]), &(acadoWorkspace.Dy[ 225 ]), &(acadoWorkspace.QDy[ 180 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 920 ]), &(acadoWorkspace.Dy[ 230 ]), &(acadoWorkspace.QDy[ 184 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 940 ]), &(acadoWorkspace.Dy[ 235 ]), &(acadoWorkspace.QDy[ 188 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 960 ]), &(acadoWorkspace.Dy[ 240 ]), &(acadoWorkspace.QDy[ 192 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 980 ]), &(acadoWorkspace.Dy[ 245 ]), &(acadoWorkspace.QDy[ 196 ]) ); - -acadoWorkspace.QDy[200] = + 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[201] = + 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[202] = + 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[203] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; - -for (lRun2 = 0; lRun2 < 200; ++lRun2) -acadoWorkspace.QDy[lRun2 + 4] += acadoWorkspace.Qd[lRun2]; - - -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.evGx[320]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[512]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[516]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[520]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[524]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[528]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[532]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[536]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[540]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[544]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[548]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[552]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[556]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[560]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[564]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[568]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[572]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[576]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[580]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[584]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[588]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[592]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[596]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[600]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[604]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[608]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[612]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[616]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[620]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[624]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[628]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[632]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[636]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[640]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[644]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[648]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[652]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[656]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[660]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[664]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[668]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[672]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[676]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[680]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[684]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[688]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[692]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[696]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[700]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[704]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[708]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[712]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[716]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[720]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[724]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[728]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[732]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[736]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[740]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[744]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[748]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[752]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[756]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[760]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[764]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[768]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[772]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[776]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[780]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[784]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[788]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[792]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[796]*acadoWorkspace.QDy[203]; -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.evGx[321]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[513]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[517]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[521]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[525]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[529]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[533]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[537]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[541]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[545]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[549]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[553]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[557]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[561]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[565]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[569]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[573]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[577]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[581]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[585]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[589]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[593]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[597]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[601]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[605]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[609]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[613]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[617]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[621]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[625]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[629]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[633]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[637]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[641]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[645]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[649]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[653]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[657]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[661]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[665]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[669]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[673]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[677]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[681]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[685]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[689]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[693]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[697]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[701]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[705]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[709]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[713]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[717]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[721]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[725]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[729]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[733]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[737]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[741]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[745]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[749]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[753]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[757]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[761]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[765]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[769]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[773]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[777]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[781]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[785]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[789]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[793]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[797]*acadoWorkspace.QDy[203]; -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.evGx[322]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[514]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[518]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[522]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[526]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[530]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[534]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[538]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[542]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[546]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[550]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[554]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[558]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[562]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[566]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[570]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[574]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[578]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[582]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[586]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[590]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[594]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[598]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[602]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[606]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[610]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[614]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[618]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[622]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[626]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[630]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[634]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[638]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[642]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[646]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[650]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[654]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[658]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[662]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[666]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[670]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[674]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[678]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[682]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[686]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[690]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[694]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[698]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[702]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[706]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[710]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[714]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[718]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[722]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[726]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[730]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[734]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[738]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[742]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[746]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[750]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[754]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[758]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[762]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[766]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[770]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[774]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[778]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[782]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[786]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[790]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[794]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[798]*acadoWorkspace.QDy[203]; -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.evGx[323]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[515]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[519]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[523]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[527]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[531]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[535]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[539]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[543]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[547]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[551]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[555]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[559]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[563]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[567]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[571]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[575]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[579]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[583]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[587]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[591]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[595]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[599]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[603]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[607]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[611]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[615]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[619]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[623]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[627]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[631]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[635]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[639]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[643]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[647]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[651]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[655]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[659]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[663]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[667]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[671]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[675]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[679]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[683]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[687]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[691]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[695]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[699]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[703]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[707]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[711]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[715]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[719]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[723]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[727]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[731]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[735]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[739]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[743]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[747]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[751]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[755]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[759]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[763]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[767]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[771]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[775]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[779]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[783]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[787]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[791]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[795]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[799]*acadoWorkspace.QDy[203]; - - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_multEQDy( &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.QDy[ lRun2 * 4 + 4 ]), &(acadoWorkspace.g[ lRun1 + 4 ]) ); -} -} + +acadoWorkspace.QDy[80] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[81] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[82] = + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[3]; +acadoWorkspace.QDy[83] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2] + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[3]; + +acadoWorkspace.QDy[4] += acadoWorkspace.Qd[0]; +acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1]; +acadoWorkspace.QDy[6] += acadoWorkspace.Qd[2]; +acadoWorkspace.QDy[7] += acadoWorkspace.Qd[3]; +acadoWorkspace.QDy[8] += acadoWorkspace.Qd[4]; +acadoWorkspace.QDy[9] += acadoWorkspace.Qd[5]; +acadoWorkspace.QDy[10] += acadoWorkspace.Qd[6]; +acadoWorkspace.QDy[11] += acadoWorkspace.Qd[7]; +acadoWorkspace.QDy[12] += acadoWorkspace.Qd[8]; +acadoWorkspace.QDy[13] += acadoWorkspace.Qd[9]; +acadoWorkspace.QDy[14] += acadoWorkspace.Qd[10]; +acadoWorkspace.QDy[15] += acadoWorkspace.Qd[11]; +acadoWorkspace.QDy[16] += acadoWorkspace.Qd[12]; +acadoWorkspace.QDy[17] += acadoWorkspace.Qd[13]; +acadoWorkspace.QDy[18] += acadoWorkspace.Qd[14]; +acadoWorkspace.QDy[19] += acadoWorkspace.Qd[15]; +acadoWorkspace.QDy[20] += acadoWorkspace.Qd[16]; +acadoWorkspace.QDy[21] += acadoWorkspace.Qd[17]; +acadoWorkspace.QDy[22] += acadoWorkspace.Qd[18]; +acadoWorkspace.QDy[23] += acadoWorkspace.Qd[19]; +acadoWorkspace.QDy[24] += acadoWorkspace.Qd[20]; +acadoWorkspace.QDy[25] += acadoWorkspace.Qd[21]; +acadoWorkspace.QDy[26] += acadoWorkspace.Qd[22]; +acadoWorkspace.QDy[27] += acadoWorkspace.Qd[23]; +acadoWorkspace.QDy[28] += acadoWorkspace.Qd[24]; +acadoWorkspace.QDy[29] += acadoWorkspace.Qd[25]; +acadoWorkspace.QDy[30] += acadoWorkspace.Qd[26]; +acadoWorkspace.QDy[31] += acadoWorkspace.Qd[27]; +acadoWorkspace.QDy[32] += acadoWorkspace.Qd[28]; +acadoWorkspace.QDy[33] += acadoWorkspace.Qd[29]; +acadoWorkspace.QDy[34] += acadoWorkspace.Qd[30]; +acadoWorkspace.QDy[35] += acadoWorkspace.Qd[31]; +acadoWorkspace.QDy[36] += acadoWorkspace.Qd[32]; +acadoWorkspace.QDy[37] += acadoWorkspace.Qd[33]; +acadoWorkspace.QDy[38] += acadoWorkspace.Qd[34]; +acadoWorkspace.QDy[39] += acadoWorkspace.Qd[35]; +acadoWorkspace.QDy[40] += acadoWorkspace.Qd[36]; +acadoWorkspace.QDy[41] += acadoWorkspace.Qd[37]; +acadoWorkspace.QDy[42] += acadoWorkspace.Qd[38]; +acadoWorkspace.QDy[43] += acadoWorkspace.Qd[39]; +acadoWorkspace.QDy[44] += acadoWorkspace.Qd[40]; +acadoWorkspace.QDy[45] += acadoWorkspace.Qd[41]; +acadoWorkspace.QDy[46] += acadoWorkspace.Qd[42]; +acadoWorkspace.QDy[47] += acadoWorkspace.Qd[43]; +acadoWorkspace.QDy[48] += acadoWorkspace.Qd[44]; +acadoWorkspace.QDy[49] += acadoWorkspace.Qd[45]; +acadoWorkspace.QDy[50] += acadoWorkspace.Qd[46]; +acadoWorkspace.QDy[51] += acadoWorkspace.Qd[47]; +acadoWorkspace.QDy[52] += acadoWorkspace.Qd[48]; +acadoWorkspace.QDy[53] += acadoWorkspace.Qd[49]; +acadoWorkspace.QDy[54] += acadoWorkspace.Qd[50]; +acadoWorkspace.QDy[55] += acadoWorkspace.Qd[51]; +acadoWorkspace.QDy[56] += acadoWorkspace.Qd[52]; +acadoWorkspace.QDy[57] += acadoWorkspace.Qd[53]; +acadoWorkspace.QDy[58] += acadoWorkspace.Qd[54]; +acadoWorkspace.QDy[59] += acadoWorkspace.Qd[55]; +acadoWorkspace.QDy[60] += acadoWorkspace.Qd[56]; +acadoWorkspace.QDy[61] += acadoWorkspace.Qd[57]; +acadoWorkspace.QDy[62] += acadoWorkspace.Qd[58]; +acadoWorkspace.QDy[63] += acadoWorkspace.Qd[59]; +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]; + + +acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 4 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 5 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 6 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 7 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 8 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 9 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 10 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 11 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 12 ]) ); +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 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 13 ]) ); +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]; @@ -1257,194 +4593,11 @@ acadoWorkspace.ubA[38] = (real_t)1.5707963268000000e+00 - tmp; tmp = acadoVariables.x[83] + acadoWorkspace.d[79]; acadoWorkspace.lbA[39] = (real_t)-4.3633231300000003e-01 - tmp; acadoWorkspace.ubA[39] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[86] + acadoWorkspace.d[82]; -acadoWorkspace.lbA[40] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[40] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[87] + acadoWorkspace.d[83]; -acadoWorkspace.lbA[41] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[41] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[90] + acadoWorkspace.d[86]; -acadoWorkspace.lbA[42] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[42] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[91] + acadoWorkspace.d[87]; -acadoWorkspace.lbA[43] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[43] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[94] + acadoWorkspace.d[90]; -acadoWorkspace.lbA[44] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[44] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[95] + acadoWorkspace.d[91]; -acadoWorkspace.lbA[45] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[45] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[98] + acadoWorkspace.d[94]; -acadoWorkspace.lbA[46] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[46] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[99] + acadoWorkspace.d[95]; -acadoWorkspace.lbA[47] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[47] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[102] + acadoWorkspace.d[98]; -acadoWorkspace.lbA[48] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[48] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[103] + acadoWorkspace.d[99]; -acadoWorkspace.lbA[49] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[49] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[106] + acadoWorkspace.d[102]; -acadoWorkspace.lbA[50] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[50] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[107] + acadoWorkspace.d[103]; -acadoWorkspace.lbA[51] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[51] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[110] + acadoWorkspace.d[106]; -acadoWorkspace.lbA[52] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[52] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[111] + acadoWorkspace.d[107]; -acadoWorkspace.lbA[53] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[53] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[114] + acadoWorkspace.d[110]; -acadoWorkspace.lbA[54] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[54] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[115] + acadoWorkspace.d[111]; -acadoWorkspace.lbA[55] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[55] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[118] + acadoWorkspace.d[114]; -acadoWorkspace.lbA[56] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[56] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[119] + acadoWorkspace.d[115]; -acadoWorkspace.lbA[57] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[57] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[122] + acadoWorkspace.d[118]; -acadoWorkspace.lbA[58] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[58] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[123] + acadoWorkspace.d[119]; -acadoWorkspace.lbA[59] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[59] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[126] + acadoWorkspace.d[122]; -acadoWorkspace.lbA[60] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[60] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[127] + acadoWorkspace.d[123]; -acadoWorkspace.lbA[61] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[61] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[130] + acadoWorkspace.d[126]; -acadoWorkspace.lbA[62] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[62] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[131] + acadoWorkspace.d[127]; -acadoWorkspace.lbA[63] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[63] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[134] + acadoWorkspace.d[130]; -acadoWorkspace.lbA[64] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[64] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[135] + acadoWorkspace.d[131]; -acadoWorkspace.lbA[65] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[65] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[138] + acadoWorkspace.d[134]; -acadoWorkspace.lbA[66] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[66] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[139] + acadoWorkspace.d[135]; -acadoWorkspace.lbA[67] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[67] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[142] + acadoWorkspace.d[138]; -acadoWorkspace.lbA[68] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[68] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[143] + acadoWorkspace.d[139]; -acadoWorkspace.lbA[69] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[69] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[146] + acadoWorkspace.d[142]; -acadoWorkspace.lbA[70] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[70] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[147] + acadoWorkspace.d[143]; -acadoWorkspace.lbA[71] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[71] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[150] + acadoWorkspace.d[146]; -acadoWorkspace.lbA[72] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[72] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[151] + acadoWorkspace.d[147]; -acadoWorkspace.lbA[73] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[73] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[154] + acadoWorkspace.d[150]; -acadoWorkspace.lbA[74] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[74] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[155] + acadoWorkspace.d[151]; -acadoWorkspace.lbA[75] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[75] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[158] + acadoWorkspace.d[154]; -acadoWorkspace.lbA[76] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[76] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[159] + acadoWorkspace.d[155]; -acadoWorkspace.lbA[77] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[77] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[162] + acadoWorkspace.d[158]; -acadoWorkspace.lbA[78] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[78] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[163] + acadoWorkspace.d[159]; -acadoWorkspace.lbA[79] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[79] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[166] + acadoWorkspace.d[162]; -acadoWorkspace.lbA[80] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[80] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[167] + acadoWorkspace.d[163]; -acadoWorkspace.lbA[81] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[81] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[170] + acadoWorkspace.d[166]; -acadoWorkspace.lbA[82] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[82] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[171] + acadoWorkspace.d[167]; -acadoWorkspace.lbA[83] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[83] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[174] + acadoWorkspace.d[170]; -acadoWorkspace.lbA[84] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[84] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[175] + acadoWorkspace.d[171]; -acadoWorkspace.lbA[85] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[85] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[178] + acadoWorkspace.d[174]; -acadoWorkspace.lbA[86] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[86] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[179] + acadoWorkspace.d[175]; -acadoWorkspace.lbA[87] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[87] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[182] + acadoWorkspace.d[178]; -acadoWorkspace.lbA[88] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[88] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[183] + acadoWorkspace.d[179]; -acadoWorkspace.lbA[89] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[89] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[186] + acadoWorkspace.d[182]; -acadoWorkspace.lbA[90] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[90] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[187] + acadoWorkspace.d[183]; -acadoWorkspace.lbA[91] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[91] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[190] + acadoWorkspace.d[186]; -acadoWorkspace.lbA[92] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[92] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[191] + acadoWorkspace.d[187]; -acadoWorkspace.lbA[93] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[93] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[194] + acadoWorkspace.d[190]; -acadoWorkspace.lbA[94] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[94] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[195] + acadoWorkspace.d[191]; -acadoWorkspace.lbA[95] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[95] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[198] + acadoWorkspace.d[194]; -acadoWorkspace.lbA[96] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[96] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[199] + acadoWorkspace.d[195]; -acadoWorkspace.lbA[97] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[97] = (real_t)4.3633231300000003e-01 - tmp; -tmp = acadoVariables.x[202] + acadoWorkspace.d[198]; -acadoWorkspace.lbA[98] = (real_t)-1.5707963268000000e+00 - tmp; -acadoWorkspace.ubA[98] = (real_t)1.5707963268000000e+00 - tmp; -tmp = acadoVariables.x[203] + acadoWorkspace.d[199]; -acadoWorkspace.lbA[99] = (real_t)-4.3633231300000003e-01 - tmp; -acadoWorkspace.ubA[99] = (real_t)4.3633231300000003e-01 - tmp; } void acado_expand( ) { -int lRun1; -int lRun2; -int lRun3; acadoVariables.x[0] += acadoWorkspace.x[0]; acadoVariables.x[1] += acadoWorkspace.x[1]; acadoVariables.x[2] += acadoWorkspace.x[2]; @@ -1470,36 +4623,6 @@ 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.u[20] += acadoWorkspace.x[24]; -acadoVariables.u[21] += acadoWorkspace.x[25]; -acadoVariables.u[22] += acadoWorkspace.x[26]; -acadoVariables.u[23] += acadoWorkspace.x[27]; -acadoVariables.u[24] += acadoWorkspace.x[28]; -acadoVariables.u[25] += acadoWorkspace.x[29]; -acadoVariables.u[26] += acadoWorkspace.x[30]; -acadoVariables.u[27] += acadoWorkspace.x[31]; -acadoVariables.u[28] += acadoWorkspace.x[32]; -acadoVariables.u[29] += acadoWorkspace.x[33]; -acadoVariables.u[30] += acadoWorkspace.x[34]; -acadoVariables.u[31] += acadoWorkspace.x[35]; -acadoVariables.u[32] += acadoWorkspace.x[36]; -acadoVariables.u[33] += acadoWorkspace.x[37]; -acadoVariables.u[34] += acadoWorkspace.x[38]; -acadoVariables.u[35] += acadoWorkspace.x[39]; -acadoVariables.u[36] += acadoWorkspace.x[40]; -acadoVariables.u[37] += acadoWorkspace.x[41]; -acadoVariables.u[38] += acadoWorkspace.x[42]; -acadoVariables.u[39] += acadoWorkspace.x[43]; -acadoVariables.u[40] += acadoWorkspace.x[44]; -acadoVariables.u[41] += acadoWorkspace.x[45]; -acadoVariables.u[42] += acadoWorkspace.x[46]; -acadoVariables.u[43] += acadoWorkspace.x[47]; -acadoVariables.u[44] += acadoWorkspace.x[48]; -acadoVariables.u[45] += acadoWorkspace.x[49]; -acadoVariables.u[46] += acadoWorkspace.x[50]; -acadoVariables.u[47] += acadoWorkspace.x[51]; -acadoVariables.u[48] += acadoWorkspace.x[52]; -acadoVariables.u[49] += acadoWorkspace.x[53]; 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]; @@ -1581,135 +4704,217 @@ acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWo 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]; -acadoVariables.x[84] += + acadoWorkspace.evGx[320]*acadoWorkspace.x[0] + acadoWorkspace.evGx[321]*acadoWorkspace.x[1] + acadoWorkspace.evGx[322]*acadoWorkspace.x[2] + acadoWorkspace.evGx[323]*acadoWorkspace.x[3] + acadoWorkspace.d[80]; -acadoVariables.x[85] += + acadoWorkspace.evGx[324]*acadoWorkspace.x[0] + acadoWorkspace.evGx[325]*acadoWorkspace.x[1] + acadoWorkspace.evGx[326]*acadoWorkspace.x[2] + acadoWorkspace.evGx[327]*acadoWorkspace.x[3] + acadoWorkspace.d[81]; -acadoVariables.x[86] += + acadoWorkspace.evGx[328]*acadoWorkspace.x[0] + acadoWorkspace.evGx[329]*acadoWorkspace.x[1] + acadoWorkspace.evGx[330]*acadoWorkspace.x[2] + acadoWorkspace.evGx[331]*acadoWorkspace.x[3] + acadoWorkspace.d[82]; -acadoVariables.x[87] += + acadoWorkspace.evGx[332]*acadoWorkspace.x[0] + acadoWorkspace.evGx[333]*acadoWorkspace.x[1] + acadoWorkspace.evGx[334]*acadoWorkspace.x[2] + acadoWorkspace.evGx[335]*acadoWorkspace.x[3] + acadoWorkspace.d[83]; -acadoVariables.x[88] += + acadoWorkspace.evGx[336]*acadoWorkspace.x[0] + acadoWorkspace.evGx[337]*acadoWorkspace.x[1] + acadoWorkspace.evGx[338]*acadoWorkspace.x[2] + acadoWorkspace.evGx[339]*acadoWorkspace.x[3] + acadoWorkspace.d[84]; -acadoVariables.x[89] += + acadoWorkspace.evGx[340]*acadoWorkspace.x[0] + acadoWorkspace.evGx[341]*acadoWorkspace.x[1] + acadoWorkspace.evGx[342]*acadoWorkspace.x[2] + acadoWorkspace.evGx[343]*acadoWorkspace.x[3] + acadoWorkspace.d[85]; -acadoVariables.x[90] += + acadoWorkspace.evGx[344]*acadoWorkspace.x[0] + acadoWorkspace.evGx[345]*acadoWorkspace.x[1] + acadoWorkspace.evGx[346]*acadoWorkspace.x[2] + acadoWorkspace.evGx[347]*acadoWorkspace.x[3] + acadoWorkspace.d[86]; -acadoVariables.x[91] += + acadoWorkspace.evGx[348]*acadoWorkspace.x[0] + acadoWorkspace.evGx[349]*acadoWorkspace.x[1] + acadoWorkspace.evGx[350]*acadoWorkspace.x[2] + acadoWorkspace.evGx[351]*acadoWorkspace.x[3] + acadoWorkspace.d[87]; -acadoVariables.x[92] += + acadoWorkspace.evGx[352]*acadoWorkspace.x[0] + acadoWorkspace.evGx[353]*acadoWorkspace.x[1] + acadoWorkspace.evGx[354]*acadoWorkspace.x[2] + acadoWorkspace.evGx[355]*acadoWorkspace.x[3] + acadoWorkspace.d[88]; -acadoVariables.x[93] += + acadoWorkspace.evGx[356]*acadoWorkspace.x[0] + acadoWorkspace.evGx[357]*acadoWorkspace.x[1] + acadoWorkspace.evGx[358]*acadoWorkspace.x[2] + acadoWorkspace.evGx[359]*acadoWorkspace.x[3] + acadoWorkspace.d[89]; -acadoVariables.x[94] += + acadoWorkspace.evGx[360]*acadoWorkspace.x[0] + acadoWorkspace.evGx[361]*acadoWorkspace.x[1] + acadoWorkspace.evGx[362]*acadoWorkspace.x[2] + acadoWorkspace.evGx[363]*acadoWorkspace.x[3] + acadoWorkspace.d[90]; -acadoVariables.x[95] += + acadoWorkspace.evGx[364]*acadoWorkspace.x[0] + acadoWorkspace.evGx[365]*acadoWorkspace.x[1] + acadoWorkspace.evGx[366]*acadoWorkspace.x[2] + acadoWorkspace.evGx[367]*acadoWorkspace.x[3] + acadoWorkspace.d[91]; -acadoVariables.x[96] += + acadoWorkspace.evGx[368]*acadoWorkspace.x[0] + acadoWorkspace.evGx[369]*acadoWorkspace.x[1] + acadoWorkspace.evGx[370]*acadoWorkspace.x[2] + acadoWorkspace.evGx[371]*acadoWorkspace.x[3] + acadoWorkspace.d[92]; -acadoVariables.x[97] += + acadoWorkspace.evGx[372]*acadoWorkspace.x[0] + acadoWorkspace.evGx[373]*acadoWorkspace.x[1] + acadoWorkspace.evGx[374]*acadoWorkspace.x[2] + acadoWorkspace.evGx[375]*acadoWorkspace.x[3] + acadoWorkspace.d[93]; -acadoVariables.x[98] += + acadoWorkspace.evGx[376]*acadoWorkspace.x[0] + acadoWorkspace.evGx[377]*acadoWorkspace.x[1] + acadoWorkspace.evGx[378]*acadoWorkspace.x[2] + acadoWorkspace.evGx[379]*acadoWorkspace.x[3] + acadoWorkspace.d[94]; -acadoVariables.x[99] += + acadoWorkspace.evGx[380]*acadoWorkspace.x[0] + acadoWorkspace.evGx[381]*acadoWorkspace.x[1] + acadoWorkspace.evGx[382]*acadoWorkspace.x[2] + acadoWorkspace.evGx[383]*acadoWorkspace.x[3] + acadoWorkspace.d[95]; -acadoVariables.x[100] += + acadoWorkspace.evGx[384]*acadoWorkspace.x[0] + acadoWorkspace.evGx[385]*acadoWorkspace.x[1] + acadoWorkspace.evGx[386]*acadoWorkspace.x[2] + acadoWorkspace.evGx[387]*acadoWorkspace.x[3] + acadoWorkspace.d[96]; -acadoVariables.x[101] += + acadoWorkspace.evGx[388]*acadoWorkspace.x[0] + acadoWorkspace.evGx[389]*acadoWorkspace.x[1] + acadoWorkspace.evGx[390]*acadoWorkspace.x[2] + acadoWorkspace.evGx[391]*acadoWorkspace.x[3] + acadoWorkspace.d[97]; -acadoVariables.x[102] += + acadoWorkspace.evGx[392]*acadoWorkspace.x[0] + acadoWorkspace.evGx[393]*acadoWorkspace.x[1] + acadoWorkspace.evGx[394]*acadoWorkspace.x[2] + acadoWorkspace.evGx[395]*acadoWorkspace.x[3] + acadoWorkspace.d[98]; -acadoVariables.x[103] += + acadoWorkspace.evGx[396]*acadoWorkspace.x[0] + acadoWorkspace.evGx[397]*acadoWorkspace.x[1] + acadoWorkspace.evGx[398]*acadoWorkspace.x[2] + acadoWorkspace.evGx[399]*acadoWorkspace.x[3] + acadoWorkspace.d[99]; -acadoVariables.x[104] += + acadoWorkspace.evGx[400]*acadoWorkspace.x[0] + acadoWorkspace.evGx[401]*acadoWorkspace.x[1] + acadoWorkspace.evGx[402]*acadoWorkspace.x[2] + acadoWorkspace.evGx[403]*acadoWorkspace.x[3] + acadoWorkspace.d[100]; -acadoVariables.x[105] += + acadoWorkspace.evGx[404]*acadoWorkspace.x[0] + acadoWorkspace.evGx[405]*acadoWorkspace.x[1] + acadoWorkspace.evGx[406]*acadoWorkspace.x[2] + acadoWorkspace.evGx[407]*acadoWorkspace.x[3] + acadoWorkspace.d[101]; -acadoVariables.x[106] += + acadoWorkspace.evGx[408]*acadoWorkspace.x[0] + acadoWorkspace.evGx[409]*acadoWorkspace.x[1] + acadoWorkspace.evGx[410]*acadoWorkspace.x[2] + acadoWorkspace.evGx[411]*acadoWorkspace.x[3] + acadoWorkspace.d[102]; -acadoVariables.x[107] += + acadoWorkspace.evGx[412]*acadoWorkspace.x[0] + acadoWorkspace.evGx[413]*acadoWorkspace.x[1] + acadoWorkspace.evGx[414]*acadoWorkspace.x[2] + acadoWorkspace.evGx[415]*acadoWorkspace.x[3] + acadoWorkspace.d[103]; -acadoVariables.x[108] += + acadoWorkspace.evGx[416]*acadoWorkspace.x[0] + acadoWorkspace.evGx[417]*acadoWorkspace.x[1] + acadoWorkspace.evGx[418]*acadoWorkspace.x[2] + acadoWorkspace.evGx[419]*acadoWorkspace.x[3] + acadoWorkspace.d[104]; -acadoVariables.x[109] += + acadoWorkspace.evGx[420]*acadoWorkspace.x[0] + acadoWorkspace.evGx[421]*acadoWorkspace.x[1] + acadoWorkspace.evGx[422]*acadoWorkspace.x[2] + acadoWorkspace.evGx[423]*acadoWorkspace.x[3] + acadoWorkspace.d[105]; -acadoVariables.x[110] += + acadoWorkspace.evGx[424]*acadoWorkspace.x[0] + acadoWorkspace.evGx[425]*acadoWorkspace.x[1] + acadoWorkspace.evGx[426]*acadoWorkspace.x[2] + acadoWorkspace.evGx[427]*acadoWorkspace.x[3] + acadoWorkspace.d[106]; -acadoVariables.x[111] += + acadoWorkspace.evGx[428]*acadoWorkspace.x[0] + acadoWorkspace.evGx[429]*acadoWorkspace.x[1] + acadoWorkspace.evGx[430]*acadoWorkspace.x[2] + acadoWorkspace.evGx[431]*acadoWorkspace.x[3] + acadoWorkspace.d[107]; -acadoVariables.x[112] += + acadoWorkspace.evGx[432]*acadoWorkspace.x[0] + acadoWorkspace.evGx[433]*acadoWorkspace.x[1] + acadoWorkspace.evGx[434]*acadoWorkspace.x[2] + acadoWorkspace.evGx[435]*acadoWorkspace.x[3] + acadoWorkspace.d[108]; -acadoVariables.x[113] += + acadoWorkspace.evGx[436]*acadoWorkspace.x[0] + acadoWorkspace.evGx[437]*acadoWorkspace.x[1] + acadoWorkspace.evGx[438]*acadoWorkspace.x[2] + acadoWorkspace.evGx[439]*acadoWorkspace.x[3] + acadoWorkspace.d[109]; -acadoVariables.x[114] += + acadoWorkspace.evGx[440]*acadoWorkspace.x[0] + acadoWorkspace.evGx[441]*acadoWorkspace.x[1] + acadoWorkspace.evGx[442]*acadoWorkspace.x[2] + acadoWorkspace.evGx[443]*acadoWorkspace.x[3] + acadoWorkspace.d[110]; -acadoVariables.x[115] += + acadoWorkspace.evGx[444]*acadoWorkspace.x[0] + acadoWorkspace.evGx[445]*acadoWorkspace.x[1] + acadoWorkspace.evGx[446]*acadoWorkspace.x[2] + acadoWorkspace.evGx[447]*acadoWorkspace.x[3] + acadoWorkspace.d[111]; -acadoVariables.x[116] += + acadoWorkspace.evGx[448]*acadoWorkspace.x[0] + acadoWorkspace.evGx[449]*acadoWorkspace.x[1] + acadoWorkspace.evGx[450]*acadoWorkspace.x[2] + acadoWorkspace.evGx[451]*acadoWorkspace.x[3] + acadoWorkspace.d[112]; -acadoVariables.x[117] += + acadoWorkspace.evGx[452]*acadoWorkspace.x[0] + acadoWorkspace.evGx[453]*acadoWorkspace.x[1] + acadoWorkspace.evGx[454]*acadoWorkspace.x[2] + acadoWorkspace.evGx[455]*acadoWorkspace.x[3] + acadoWorkspace.d[113]; -acadoVariables.x[118] += + acadoWorkspace.evGx[456]*acadoWorkspace.x[0] + acadoWorkspace.evGx[457]*acadoWorkspace.x[1] + acadoWorkspace.evGx[458]*acadoWorkspace.x[2] + acadoWorkspace.evGx[459]*acadoWorkspace.x[3] + acadoWorkspace.d[114]; -acadoVariables.x[119] += + acadoWorkspace.evGx[460]*acadoWorkspace.x[0] + acadoWorkspace.evGx[461]*acadoWorkspace.x[1] + acadoWorkspace.evGx[462]*acadoWorkspace.x[2] + acadoWorkspace.evGx[463]*acadoWorkspace.x[3] + acadoWorkspace.d[115]; -acadoVariables.x[120] += + acadoWorkspace.evGx[464]*acadoWorkspace.x[0] + acadoWorkspace.evGx[465]*acadoWorkspace.x[1] + acadoWorkspace.evGx[466]*acadoWorkspace.x[2] + acadoWorkspace.evGx[467]*acadoWorkspace.x[3] + acadoWorkspace.d[116]; -acadoVariables.x[121] += + acadoWorkspace.evGx[468]*acadoWorkspace.x[0] + acadoWorkspace.evGx[469]*acadoWorkspace.x[1] + acadoWorkspace.evGx[470]*acadoWorkspace.x[2] + acadoWorkspace.evGx[471]*acadoWorkspace.x[3] + acadoWorkspace.d[117]; -acadoVariables.x[122] += + acadoWorkspace.evGx[472]*acadoWorkspace.x[0] + acadoWorkspace.evGx[473]*acadoWorkspace.x[1] + acadoWorkspace.evGx[474]*acadoWorkspace.x[2] + acadoWorkspace.evGx[475]*acadoWorkspace.x[3] + acadoWorkspace.d[118]; -acadoVariables.x[123] += + acadoWorkspace.evGx[476]*acadoWorkspace.x[0] + acadoWorkspace.evGx[477]*acadoWorkspace.x[1] + acadoWorkspace.evGx[478]*acadoWorkspace.x[2] + acadoWorkspace.evGx[479]*acadoWorkspace.x[3] + acadoWorkspace.d[119]; -acadoVariables.x[124] += + acadoWorkspace.evGx[480]*acadoWorkspace.x[0] + acadoWorkspace.evGx[481]*acadoWorkspace.x[1] + acadoWorkspace.evGx[482]*acadoWorkspace.x[2] + acadoWorkspace.evGx[483]*acadoWorkspace.x[3] + acadoWorkspace.d[120]; -acadoVariables.x[125] += + acadoWorkspace.evGx[484]*acadoWorkspace.x[0] + acadoWorkspace.evGx[485]*acadoWorkspace.x[1] + acadoWorkspace.evGx[486]*acadoWorkspace.x[2] + acadoWorkspace.evGx[487]*acadoWorkspace.x[3] + acadoWorkspace.d[121]; -acadoVariables.x[126] += + acadoWorkspace.evGx[488]*acadoWorkspace.x[0] + acadoWorkspace.evGx[489]*acadoWorkspace.x[1] + acadoWorkspace.evGx[490]*acadoWorkspace.x[2] + acadoWorkspace.evGx[491]*acadoWorkspace.x[3] + acadoWorkspace.d[122]; -acadoVariables.x[127] += + acadoWorkspace.evGx[492]*acadoWorkspace.x[0] + acadoWorkspace.evGx[493]*acadoWorkspace.x[1] + acadoWorkspace.evGx[494]*acadoWorkspace.x[2] + acadoWorkspace.evGx[495]*acadoWorkspace.x[3] + acadoWorkspace.d[123]; -acadoVariables.x[128] += + acadoWorkspace.evGx[496]*acadoWorkspace.x[0] + acadoWorkspace.evGx[497]*acadoWorkspace.x[1] + acadoWorkspace.evGx[498]*acadoWorkspace.x[2] + acadoWorkspace.evGx[499]*acadoWorkspace.x[3] + acadoWorkspace.d[124]; -acadoVariables.x[129] += + acadoWorkspace.evGx[500]*acadoWorkspace.x[0] + acadoWorkspace.evGx[501]*acadoWorkspace.x[1] + acadoWorkspace.evGx[502]*acadoWorkspace.x[2] + acadoWorkspace.evGx[503]*acadoWorkspace.x[3] + acadoWorkspace.d[125]; -acadoVariables.x[130] += + acadoWorkspace.evGx[504]*acadoWorkspace.x[0] + acadoWorkspace.evGx[505]*acadoWorkspace.x[1] + acadoWorkspace.evGx[506]*acadoWorkspace.x[2] + acadoWorkspace.evGx[507]*acadoWorkspace.x[3] + acadoWorkspace.d[126]; -acadoVariables.x[131] += + acadoWorkspace.evGx[508]*acadoWorkspace.x[0] + acadoWorkspace.evGx[509]*acadoWorkspace.x[1] + acadoWorkspace.evGx[510]*acadoWorkspace.x[2] + acadoWorkspace.evGx[511]*acadoWorkspace.x[3] + acadoWorkspace.d[127]; -acadoVariables.x[132] += + acadoWorkspace.evGx[512]*acadoWorkspace.x[0] + acadoWorkspace.evGx[513]*acadoWorkspace.x[1] + acadoWorkspace.evGx[514]*acadoWorkspace.x[2] + acadoWorkspace.evGx[515]*acadoWorkspace.x[3] + acadoWorkspace.d[128]; -acadoVariables.x[133] += + acadoWorkspace.evGx[516]*acadoWorkspace.x[0] + acadoWorkspace.evGx[517]*acadoWorkspace.x[1] + acadoWorkspace.evGx[518]*acadoWorkspace.x[2] + acadoWorkspace.evGx[519]*acadoWorkspace.x[3] + acadoWorkspace.d[129]; -acadoVariables.x[134] += + acadoWorkspace.evGx[520]*acadoWorkspace.x[0] + acadoWorkspace.evGx[521]*acadoWorkspace.x[1] + acadoWorkspace.evGx[522]*acadoWorkspace.x[2] + acadoWorkspace.evGx[523]*acadoWorkspace.x[3] + acadoWorkspace.d[130]; -acadoVariables.x[135] += + acadoWorkspace.evGx[524]*acadoWorkspace.x[0] + acadoWorkspace.evGx[525]*acadoWorkspace.x[1] + acadoWorkspace.evGx[526]*acadoWorkspace.x[2] + acadoWorkspace.evGx[527]*acadoWorkspace.x[3] + acadoWorkspace.d[131]; -acadoVariables.x[136] += + acadoWorkspace.evGx[528]*acadoWorkspace.x[0] + acadoWorkspace.evGx[529]*acadoWorkspace.x[1] + acadoWorkspace.evGx[530]*acadoWorkspace.x[2] + acadoWorkspace.evGx[531]*acadoWorkspace.x[3] + acadoWorkspace.d[132]; -acadoVariables.x[137] += + acadoWorkspace.evGx[532]*acadoWorkspace.x[0] + acadoWorkspace.evGx[533]*acadoWorkspace.x[1] + acadoWorkspace.evGx[534]*acadoWorkspace.x[2] + acadoWorkspace.evGx[535]*acadoWorkspace.x[3] + acadoWorkspace.d[133]; -acadoVariables.x[138] += + acadoWorkspace.evGx[536]*acadoWorkspace.x[0] + acadoWorkspace.evGx[537]*acadoWorkspace.x[1] + acadoWorkspace.evGx[538]*acadoWorkspace.x[2] + acadoWorkspace.evGx[539]*acadoWorkspace.x[3] + acadoWorkspace.d[134]; -acadoVariables.x[139] += + acadoWorkspace.evGx[540]*acadoWorkspace.x[0] + acadoWorkspace.evGx[541]*acadoWorkspace.x[1] + acadoWorkspace.evGx[542]*acadoWorkspace.x[2] + acadoWorkspace.evGx[543]*acadoWorkspace.x[3] + acadoWorkspace.d[135]; -acadoVariables.x[140] += + acadoWorkspace.evGx[544]*acadoWorkspace.x[0] + acadoWorkspace.evGx[545]*acadoWorkspace.x[1] + acadoWorkspace.evGx[546]*acadoWorkspace.x[2] + acadoWorkspace.evGx[547]*acadoWorkspace.x[3] + acadoWorkspace.d[136]; -acadoVariables.x[141] += + acadoWorkspace.evGx[548]*acadoWorkspace.x[0] + acadoWorkspace.evGx[549]*acadoWorkspace.x[1] + acadoWorkspace.evGx[550]*acadoWorkspace.x[2] + acadoWorkspace.evGx[551]*acadoWorkspace.x[3] + acadoWorkspace.d[137]; -acadoVariables.x[142] += + acadoWorkspace.evGx[552]*acadoWorkspace.x[0] + acadoWorkspace.evGx[553]*acadoWorkspace.x[1] + acadoWorkspace.evGx[554]*acadoWorkspace.x[2] + acadoWorkspace.evGx[555]*acadoWorkspace.x[3] + acadoWorkspace.d[138]; -acadoVariables.x[143] += + acadoWorkspace.evGx[556]*acadoWorkspace.x[0] + acadoWorkspace.evGx[557]*acadoWorkspace.x[1] + acadoWorkspace.evGx[558]*acadoWorkspace.x[2] + acadoWorkspace.evGx[559]*acadoWorkspace.x[3] + acadoWorkspace.d[139]; -acadoVariables.x[144] += + acadoWorkspace.evGx[560]*acadoWorkspace.x[0] + acadoWorkspace.evGx[561]*acadoWorkspace.x[1] + acadoWorkspace.evGx[562]*acadoWorkspace.x[2] + acadoWorkspace.evGx[563]*acadoWorkspace.x[3] + acadoWorkspace.d[140]; -acadoVariables.x[145] += + acadoWorkspace.evGx[564]*acadoWorkspace.x[0] + acadoWorkspace.evGx[565]*acadoWorkspace.x[1] + acadoWorkspace.evGx[566]*acadoWorkspace.x[2] + acadoWorkspace.evGx[567]*acadoWorkspace.x[3] + acadoWorkspace.d[141]; -acadoVariables.x[146] += + acadoWorkspace.evGx[568]*acadoWorkspace.x[0] + acadoWorkspace.evGx[569]*acadoWorkspace.x[1] + acadoWorkspace.evGx[570]*acadoWorkspace.x[2] + acadoWorkspace.evGx[571]*acadoWorkspace.x[3] + acadoWorkspace.d[142]; -acadoVariables.x[147] += + acadoWorkspace.evGx[572]*acadoWorkspace.x[0] + acadoWorkspace.evGx[573]*acadoWorkspace.x[1] + acadoWorkspace.evGx[574]*acadoWorkspace.x[2] + acadoWorkspace.evGx[575]*acadoWorkspace.x[3] + acadoWorkspace.d[143]; -acadoVariables.x[148] += + acadoWorkspace.evGx[576]*acadoWorkspace.x[0] + acadoWorkspace.evGx[577]*acadoWorkspace.x[1] + acadoWorkspace.evGx[578]*acadoWorkspace.x[2] + acadoWorkspace.evGx[579]*acadoWorkspace.x[3] + acadoWorkspace.d[144]; -acadoVariables.x[149] += + acadoWorkspace.evGx[580]*acadoWorkspace.x[0] + acadoWorkspace.evGx[581]*acadoWorkspace.x[1] + acadoWorkspace.evGx[582]*acadoWorkspace.x[2] + acadoWorkspace.evGx[583]*acadoWorkspace.x[3] + acadoWorkspace.d[145]; -acadoVariables.x[150] += + acadoWorkspace.evGx[584]*acadoWorkspace.x[0] + acadoWorkspace.evGx[585]*acadoWorkspace.x[1] + acadoWorkspace.evGx[586]*acadoWorkspace.x[2] + acadoWorkspace.evGx[587]*acadoWorkspace.x[3] + acadoWorkspace.d[146]; -acadoVariables.x[151] += + acadoWorkspace.evGx[588]*acadoWorkspace.x[0] + acadoWorkspace.evGx[589]*acadoWorkspace.x[1] + acadoWorkspace.evGx[590]*acadoWorkspace.x[2] + acadoWorkspace.evGx[591]*acadoWorkspace.x[3] + acadoWorkspace.d[147]; -acadoVariables.x[152] += + acadoWorkspace.evGx[592]*acadoWorkspace.x[0] + acadoWorkspace.evGx[593]*acadoWorkspace.x[1] + acadoWorkspace.evGx[594]*acadoWorkspace.x[2] + acadoWorkspace.evGx[595]*acadoWorkspace.x[3] + acadoWorkspace.d[148]; -acadoVariables.x[153] += + acadoWorkspace.evGx[596]*acadoWorkspace.x[0] + acadoWorkspace.evGx[597]*acadoWorkspace.x[1] + acadoWorkspace.evGx[598]*acadoWorkspace.x[2] + acadoWorkspace.evGx[599]*acadoWorkspace.x[3] + acadoWorkspace.d[149]; -acadoVariables.x[154] += + acadoWorkspace.evGx[600]*acadoWorkspace.x[0] + acadoWorkspace.evGx[601]*acadoWorkspace.x[1] + acadoWorkspace.evGx[602]*acadoWorkspace.x[2] + acadoWorkspace.evGx[603]*acadoWorkspace.x[3] + acadoWorkspace.d[150]; -acadoVariables.x[155] += + acadoWorkspace.evGx[604]*acadoWorkspace.x[0] + acadoWorkspace.evGx[605]*acadoWorkspace.x[1] + acadoWorkspace.evGx[606]*acadoWorkspace.x[2] + acadoWorkspace.evGx[607]*acadoWorkspace.x[3] + acadoWorkspace.d[151]; -acadoVariables.x[156] += + acadoWorkspace.evGx[608]*acadoWorkspace.x[0] + acadoWorkspace.evGx[609]*acadoWorkspace.x[1] + acadoWorkspace.evGx[610]*acadoWorkspace.x[2] + acadoWorkspace.evGx[611]*acadoWorkspace.x[3] + acadoWorkspace.d[152]; -acadoVariables.x[157] += + acadoWorkspace.evGx[612]*acadoWorkspace.x[0] + acadoWorkspace.evGx[613]*acadoWorkspace.x[1] + acadoWorkspace.evGx[614]*acadoWorkspace.x[2] + acadoWorkspace.evGx[615]*acadoWorkspace.x[3] + acadoWorkspace.d[153]; -acadoVariables.x[158] += + acadoWorkspace.evGx[616]*acadoWorkspace.x[0] + acadoWorkspace.evGx[617]*acadoWorkspace.x[1] + acadoWorkspace.evGx[618]*acadoWorkspace.x[2] + acadoWorkspace.evGx[619]*acadoWorkspace.x[3] + acadoWorkspace.d[154]; -acadoVariables.x[159] += + acadoWorkspace.evGx[620]*acadoWorkspace.x[0] + acadoWorkspace.evGx[621]*acadoWorkspace.x[1] + acadoWorkspace.evGx[622]*acadoWorkspace.x[2] + acadoWorkspace.evGx[623]*acadoWorkspace.x[3] + acadoWorkspace.d[155]; -acadoVariables.x[160] += + acadoWorkspace.evGx[624]*acadoWorkspace.x[0] + acadoWorkspace.evGx[625]*acadoWorkspace.x[1] + acadoWorkspace.evGx[626]*acadoWorkspace.x[2] + acadoWorkspace.evGx[627]*acadoWorkspace.x[3] + acadoWorkspace.d[156]; -acadoVariables.x[161] += + acadoWorkspace.evGx[628]*acadoWorkspace.x[0] + acadoWorkspace.evGx[629]*acadoWorkspace.x[1] + acadoWorkspace.evGx[630]*acadoWorkspace.x[2] + acadoWorkspace.evGx[631]*acadoWorkspace.x[3] + acadoWorkspace.d[157]; -acadoVariables.x[162] += + acadoWorkspace.evGx[632]*acadoWorkspace.x[0] + acadoWorkspace.evGx[633]*acadoWorkspace.x[1] + acadoWorkspace.evGx[634]*acadoWorkspace.x[2] + acadoWorkspace.evGx[635]*acadoWorkspace.x[3] + acadoWorkspace.d[158]; -acadoVariables.x[163] += + acadoWorkspace.evGx[636]*acadoWorkspace.x[0] + acadoWorkspace.evGx[637]*acadoWorkspace.x[1] + acadoWorkspace.evGx[638]*acadoWorkspace.x[2] + acadoWorkspace.evGx[639]*acadoWorkspace.x[3] + acadoWorkspace.d[159]; -acadoVariables.x[164] += + acadoWorkspace.evGx[640]*acadoWorkspace.x[0] + acadoWorkspace.evGx[641]*acadoWorkspace.x[1] + acadoWorkspace.evGx[642]*acadoWorkspace.x[2] + acadoWorkspace.evGx[643]*acadoWorkspace.x[3] + acadoWorkspace.d[160]; -acadoVariables.x[165] += + acadoWorkspace.evGx[644]*acadoWorkspace.x[0] + acadoWorkspace.evGx[645]*acadoWorkspace.x[1] + acadoWorkspace.evGx[646]*acadoWorkspace.x[2] + acadoWorkspace.evGx[647]*acadoWorkspace.x[3] + acadoWorkspace.d[161]; -acadoVariables.x[166] += + acadoWorkspace.evGx[648]*acadoWorkspace.x[0] + acadoWorkspace.evGx[649]*acadoWorkspace.x[1] + acadoWorkspace.evGx[650]*acadoWorkspace.x[2] + acadoWorkspace.evGx[651]*acadoWorkspace.x[3] + acadoWorkspace.d[162]; -acadoVariables.x[167] += + acadoWorkspace.evGx[652]*acadoWorkspace.x[0] + acadoWorkspace.evGx[653]*acadoWorkspace.x[1] + acadoWorkspace.evGx[654]*acadoWorkspace.x[2] + acadoWorkspace.evGx[655]*acadoWorkspace.x[3] + acadoWorkspace.d[163]; -acadoVariables.x[168] += + acadoWorkspace.evGx[656]*acadoWorkspace.x[0] + acadoWorkspace.evGx[657]*acadoWorkspace.x[1] + acadoWorkspace.evGx[658]*acadoWorkspace.x[2] + acadoWorkspace.evGx[659]*acadoWorkspace.x[3] + acadoWorkspace.d[164]; -acadoVariables.x[169] += + acadoWorkspace.evGx[660]*acadoWorkspace.x[0] + acadoWorkspace.evGx[661]*acadoWorkspace.x[1] + acadoWorkspace.evGx[662]*acadoWorkspace.x[2] + acadoWorkspace.evGx[663]*acadoWorkspace.x[3] + acadoWorkspace.d[165]; -acadoVariables.x[170] += + acadoWorkspace.evGx[664]*acadoWorkspace.x[0] + acadoWorkspace.evGx[665]*acadoWorkspace.x[1] + acadoWorkspace.evGx[666]*acadoWorkspace.x[2] + acadoWorkspace.evGx[667]*acadoWorkspace.x[3] + acadoWorkspace.d[166]; -acadoVariables.x[171] += + acadoWorkspace.evGx[668]*acadoWorkspace.x[0] + acadoWorkspace.evGx[669]*acadoWorkspace.x[1] + acadoWorkspace.evGx[670]*acadoWorkspace.x[2] + acadoWorkspace.evGx[671]*acadoWorkspace.x[3] + acadoWorkspace.d[167]; -acadoVariables.x[172] += + acadoWorkspace.evGx[672]*acadoWorkspace.x[0] + acadoWorkspace.evGx[673]*acadoWorkspace.x[1] + acadoWorkspace.evGx[674]*acadoWorkspace.x[2] + acadoWorkspace.evGx[675]*acadoWorkspace.x[3] + acadoWorkspace.d[168]; -acadoVariables.x[173] += + acadoWorkspace.evGx[676]*acadoWorkspace.x[0] + acadoWorkspace.evGx[677]*acadoWorkspace.x[1] + acadoWorkspace.evGx[678]*acadoWorkspace.x[2] + acadoWorkspace.evGx[679]*acadoWorkspace.x[3] + acadoWorkspace.d[169]; -acadoVariables.x[174] += + acadoWorkspace.evGx[680]*acadoWorkspace.x[0] + acadoWorkspace.evGx[681]*acadoWorkspace.x[1] + acadoWorkspace.evGx[682]*acadoWorkspace.x[2] + acadoWorkspace.evGx[683]*acadoWorkspace.x[3] + acadoWorkspace.d[170]; -acadoVariables.x[175] += + acadoWorkspace.evGx[684]*acadoWorkspace.x[0] + acadoWorkspace.evGx[685]*acadoWorkspace.x[1] + acadoWorkspace.evGx[686]*acadoWorkspace.x[2] + acadoWorkspace.evGx[687]*acadoWorkspace.x[3] + acadoWorkspace.d[171]; -acadoVariables.x[176] += + acadoWorkspace.evGx[688]*acadoWorkspace.x[0] + acadoWorkspace.evGx[689]*acadoWorkspace.x[1] + acadoWorkspace.evGx[690]*acadoWorkspace.x[2] + acadoWorkspace.evGx[691]*acadoWorkspace.x[3] + acadoWorkspace.d[172]; -acadoVariables.x[177] += + acadoWorkspace.evGx[692]*acadoWorkspace.x[0] + acadoWorkspace.evGx[693]*acadoWorkspace.x[1] + acadoWorkspace.evGx[694]*acadoWorkspace.x[2] + acadoWorkspace.evGx[695]*acadoWorkspace.x[3] + acadoWorkspace.d[173]; -acadoVariables.x[178] += + acadoWorkspace.evGx[696]*acadoWorkspace.x[0] + acadoWorkspace.evGx[697]*acadoWorkspace.x[1] + acadoWorkspace.evGx[698]*acadoWorkspace.x[2] + acadoWorkspace.evGx[699]*acadoWorkspace.x[3] + acadoWorkspace.d[174]; -acadoVariables.x[179] += + acadoWorkspace.evGx[700]*acadoWorkspace.x[0] + acadoWorkspace.evGx[701]*acadoWorkspace.x[1] + acadoWorkspace.evGx[702]*acadoWorkspace.x[2] + acadoWorkspace.evGx[703]*acadoWorkspace.x[3] + acadoWorkspace.d[175]; -acadoVariables.x[180] += + acadoWorkspace.evGx[704]*acadoWorkspace.x[0] + acadoWorkspace.evGx[705]*acadoWorkspace.x[1] + acadoWorkspace.evGx[706]*acadoWorkspace.x[2] + acadoWorkspace.evGx[707]*acadoWorkspace.x[3] + acadoWorkspace.d[176]; -acadoVariables.x[181] += + acadoWorkspace.evGx[708]*acadoWorkspace.x[0] + acadoWorkspace.evGx[709]*acadoWorkspace.x[1] + acadoWorkspace.evGx[710]*acadoWorkspace.x[2] + acadoWorkspace.evGx[711]*acadoWorkspace.x[3] + acadoWorkspace.d[177]; -acadoVariables.x[182] += + acadoWorkspace.evGx[712]*acadoWorkspace.x[0] + acadoWorkspace.evGx[713]*acadoWorkspace.x[1] + acadoWorkspace.evGx[714]*acadoWorkspace.x[2] + acadoWorkspace.evGx[715]*acadoWorkspace.x[3] + acadoWorkspace.d[178]; -acadoVariables.x[183] += + acadoWorkspace.evGx[716]*acadoWorkspace.x[0] + acadoWorkspace.evGx[717]*acadoWorkspace.x[1] + acadoWorkspace.evGx[718]*acadoWorkspace.x[2] + acadoWorkspace.evGx[719]*acadoWorkspace.x[3] + acadoWorkspace.d[179]; -acadoVariables.x[184] += + acadoWorkspace.evGx[720]*acadoWorkspace.x[0] + acadoWorkspace.evGx[721]*acadoWorkspace.x[1] + acadoWorkspace.evGx[722]*acadoWorkspace.x[2] + acadoWorkspace.evGx[723]*acadoWorkspace.x[3] + acadoWorkspace.d[180]; -acadoVariables.x[185] += + acadoWorkspace.evGx[724]*acadoWorkspace.x[0] + acadoWorkspace.evGx[725]*acadoWorkspace.x[1] + acadoWorkspace.evGx[726]*acadoWorkspace.x[2] + acadoWorkspace.evGx[727]*acadoWorkspace.x[3] + acadoWorkspace.d[181]; -acadoVariables.x[186] += + acadoWorkspace.evGx[728]*acadoWorkspace.x[0] + acadoWorkspace.evGx[729]*acadoWorkspace.x[1] + acadoWorkspace.evGx[730]*acadoWorkspace.x[2] + acadoWorkspace.evGx[731]*acadoWorkspace.x[3] + acadoWorkspace.d[182]; -acadoVariables.x[187] += + acadoWorkspace.evGx[732]*acadoWorkspace.x[0] + acadoWorkspace.evGx[733]*acadoWorkspace.x[1] + acadoWorkspace.evGx[734]*acadoWorkspace.x[2] + acadoWorkspace.evGx[735]*acadoWorkspace.x[3] + acadoWorkspace.d[183]; -acadoVariables.x[188] += + acadoWorkspace.evGx[736]*acadoWorkspace.x[0] + acadoWorkspace.evGx[737]*acadoWorkspace.x[1] + acadoWorkspace.evGx[738]*acadoWorkspace.x[2] + acadoWorkspace.evGx[739]*acadoWorkspace.x[3] + acadoWorkspace.d[184]; -acadoVariables.x[189] += + acadoWorkspace.evGx[740]*acadoWorkspace.x[0] + acadoWorkspace.evGx[741]*acadoWorkspace.x[1] + acadoWorkspace.evGx[742]*acadoWorkspace.x[2] + acadoWorkspace.evGx[743]*acadoWorkspace.x[3] + acadoWorkspace.d[185]; -acadoVariables.x[190] += + acadoWorkspace.evGx[744]*acadoWorkspace.x[0] + acadoWorkspace.evGx[745]*acadoWorkspace.x[1] + acadoWorkspace.evGx[746]*acadoWorkspace.x[2] + acadoWorkspace.evGx[747]*acadoWorkspace.x[3] + acadoWorkspace.d[186]; -acadoVariables.x[191] += + acadoWorkspace.evGx[748]*acadoWorkspace.x[0] + acadoWorkspace.evGx[749]*acadoWorkspace.x[1] + acadoWorkspace.evGx[750]*acadoWorkspace.x[2] + acadoWorkspace.evGx[751]*acadoWorkspace.x[3] + acadoWorkspace.d[187]; -acadoVariables.x[192] += + acadoWorkspace.evGx[752]*acadoWorkspace.x[0] + acadoWorkspace.evGx[753]*acadoWorkspace.x[1] + acadoWorkspace.evGx[754]*acadoWorkspace.x[2] + acadoWorkspace.evGx[755]*acadoWorkspace.x[3] + acadoWorkspace.d[188]; -acadoVariables.x[193] += + acadoWorkspace.evGx[756]*acadoWorkspace.x[0] + acadoWorkspace.evGx[757]*acadoWorkspace.x[1] + acadoWorkspace.evGx[758]*acadoWorkspace.x[2] + acadoWorkspace.evGx[759]*acadoWorkspace.x[3] + acadoWorkspace.d[189]; -acadoVariables.x[194] += + acadoWorkspace.evGx[760]*acadoWorkspace.x[0] + acadoWorkspace.evGx[761]*acadoWorkspace.x[1] + acadoWorkspace.evGx[762]*acadoWorkspace.x[2] + acadoWorkspace.evGx[763]*acadoWorkspace.x[3] + acadoWorkspace.d[190]; -acadoVariables.x[195] += + acadoWorkspace.evGx[764]*acadoWorkspace.x[0] + acadoWorkspace.evGx[765]*acadoWorkspace.x[1] + acadoWorkspace.evGx[766]*acadoWorkspace.x[2] + acadoWorkspace.evGx[767]*acadoWorkspace.x[3] + acadoWorkspace.d[191]; -acadoVariables.x[196] += + acadoWorkspace.evGx[768]*acadoWorkspace.x[0] + acadoWorkspace.evGx[769]*acadoWorkspace.x[1] + acadoWorkspace.evGx[770]*acadoWorkspace.x[2] + acadoWorkspace.evGx[771]*acadoWorkspace.x[3] + acadoWorkspace.d[192]; -acadoVariables.x[197] += + acadoWorkspace.evGx[772]*acadoWorkspace.x[0] + acadoWorkspace.evGx[773]*acadoWorkspace.x[1] + acadoWorkspace.evGx[774]*acadoWorkspace.x[2] + acadoWorkspace.evGx[775]*acadoWorkspace.x[3] + acadoWorkspace.d[193]; -acadoVariables.x[198] += + acadoWorkspace.evGx[776]*acadoWorkspace.x[0] + acadoWorkspace.evGx[777]*acadoWorkspace.x[1] + acadoWorkspace.evGx[778]*acadoWorkspace.x[2] + acadoWorkspace.evGx[779]*acadoWorkspace.x[3] + acadoWorkspace.d[194]; -acadoVariables.x[199] += + acadoWorkspace.evGx[780]*acadoWorkspace.x[0] + acadoWorkspace.evGx[781]*acadoWorkspace.x[1] + acadoWorkspace.evGx[782]*acadoWorkspace.x[2] + acadoWorkspace.evGx[783]*acadoWorkspace.x[3] + acadoWorkspace.d[195]; -acadoVariables.x[200] += + acadoWorkspace.evGx[784]*acadoWorkspace.x[0] + acadoWorkspace.evGx[785]*acadoWorkspace.x[1] + acadoWorkspace.evGx[786]*acadoWorkspace.x[2] + acadoWorkspace.evGx[787]*acadoWorkspace.x[3] + acadoWorkspace.d[196]; -acadoVariables.x[201] += + acadoWorkspace.evGx[788]*acadoWorkspace.x[0] + acadoWorkspace.evGx[789]*acadoWorkspace.x[1] + acadoWorkspace.evGx[790]*acadoWorkspace.x[2] + acadoWorkspace.evGx[791]*acadoWorkspace.x[3] + acadoWorkspace.d[197]; -acadoVariables.x[202] += + acadoWorkspace.evGx[792]*acadoWorkspace.x[0] + acadoWorkspace.evGx[793]*acadoWorkspace.x[1] + acadoWorkspace.evGx[794]*acadoWorkspace.x[2] + acadoWorkspace.evGx[795]*acadoWorkspace.x[3] + acadoWorkspace.d[198]; -acadoVariables.x[203] += + acadoWorkspace.evGx[796]*acadoWorkspace.x[0] + acadoWorkspace.evGx[797]*acadoWorkspace.x[1] + acadoWorkspace.evGx[798]*acadoWorkspace.x[2] + acadoWorkspace.evGx[799]*acadoWorkspace.x[3] + acadoWorkspace.d[199]; - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multEDu( &(acadoWorkspace.E[ lRun3 * 4 ]), &(acadoWorkspace.x[ lRun2 + 4 ]), &(acadoVariables.x[ lRun1 * 4 + 4 ]) ); -} -} + +acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 4 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 8 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 8 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 16 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 20 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 28 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 32 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 40 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 44 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 52 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 56 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 64 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 64 ]) ); +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( ) @@ -1750,7 +4955,7 @@ return ret; void acado_initializeNodesByForwardSimulation( ) { int index; -for (index = 0; index < 50; ++index) +for (index = 0; index < 20; ++index) { acadoWorkspace.state[0] = acadoVariables.x[index * 4]; acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1]; @@ -1776,7 +4981,7 @@ acadoWorkspace.state[40] = acadoVariables.od[index * 18 + 15]; acadoWorkspace.state[41] = acadoVariables.od[index * 18 + 16]; acadoWorkspace.state[42] = acadoVariables.od[index * 18 + 17]; -acado_integrate(acadoWorkspace.state, index == 0); +acado_integrate(acadoWorkspace.state, index == 0, index); acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0]; acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1]; @@ -1788,7 +4993,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 < 50; ++index) +for (index = 0; index < 20; ++index) { acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4]; acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5]; @@ -1798,64 +5003,64 @@ acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7]; if (strategy == 1 && xEnd != 0) { -acadoVariables.x[200] = xEnd[0]; -acadoVariables.x[201] = xEnd[1]; -acadoVariables.x[202] = xEnd[2]; -acadoVariables.x[203] = xEnd[3]; +acadoVariables.x[80] = xEnd[0]; +acadoVariables.x[81] = xEnd[1]; +acadoVariables.x[82] = xEnd[2]; +acadoVariables.x[83] = xEnd[3]; } else if (strategy == 2) { -acadoWorkspace.state[0] = acadoVariables.x[200]; -acadoWorkspace.state[1] = acadoVariables.x[201]; -acadoWorkspace.state[2] = acadoVariables.x[202]; -acadoWorkspace.state[3] = acadoVariables.x[203]; +acadoWorkspace.state[0] = acadoVariables.x[80]; +acadoWorkspace.state[1] = acadoVariables.x[81]; +acadoWorkspace.state[2] = acadoVariables.x[82]; +acadoWorkspace.state[3] = acadoVariables.x[83]; if (uEnd != 0) { acadoWorkspace.state[24] = uEnd[0]; } else { -acadoWorkspace.state[24] = acadoVariables.u[49]; +acadoWorkspace.state[24] = acadoVariables.u[19]; } -acadoWorkspace.state[25] = acadoVariables.od[900]; -acadoWorkspace.state[26] = acadoVariables.od[901]; -acadoWorkspace.state[27] = acadoVariables.od[902]; -acadoWorkspace.state[28] = acadoVariables.od[903]; -acadoWorkspace.state[29] = acadoVariables.od[904]; -acadoWorkspace.state[30] = acadoVariables.od[905]; -acadoWorkspace.state[31] = acadoVariables.od[906]; -acadoWorkspace.state[32] = acadoVariables.od[907]; -acadoWorkspace.state[33] = acadoVariables.od[908]; -acadoWorkspace.state[34] = acadoVariables.od[909]; -acadoWorkspace.state[35] = acadoVariables.od[910]; -acadoWorkspace.state[36] = acadoVariables.od[911]; -acadoWorkspace.state[37] = acadoVariables.od[912]; -acadoWorkspace.state[38] = acadoVariables.od[913]; -acadoWorkspace.state[39] = acadoVariables.od[914]; -acadoWorkspace.state[40] = acadoVariables.od[915]; -acadoWorkspace.state[41] = acadoVariables.od[916]; -acadoWorkspace.state[42] = acadoVariables.od[917]; - -acado_integrate(acadoWorkspace.state, 1); - -acadoVariables.x[200] = acadoWorkspace.state[0]; -acadoVariables.x[201] = acadoWorkspace.state[1]; -acadoVariables.x[202] = acadoWorkspace.state[2]; -acadoVariables.x[203] = acadoWorkspace.state[3]; +acadoWorkspace.state[25] = acadoVariables.od[360]; +acadoWorkspace.state[26] = acadoVariables.od[361]; +acadoWorkspace.state[27] = acadoVariables.od[362]; +acadoWorkspace.state[28] = acadoVariables.od[363]; +acadoWorkspace.state[29] = acadoVariables.od[364]; +acadoWorkspace.state[30] = acadoVariables.od[365]; +acadoWorkspace.state[31] = acadoVariables.od[366]; +acadoWorkspace.state[32] = acadoVariables.od[367]; +acadoWorkspace.state[33] = acadoVariables.od[368]; +acadoWorkspace.state[34] = acadoVariables.od[369]; +acadoWorkspace.state[35] = acadoVariables.od[370]; +acadoWorkspace.state[36] = acadoVariables.od[371]; +acadoWorkspace.state[37] = acadoVariables.od[372]; +acadoWorkspace.state[38] = acadoVariables.od[373]; +acadoWorkspace.state[39] = acadoVariables.od[374]; +acadoWorkspace.state[40] = acadoVariables.od[375]; +acadoWorkspace.state[41] = acadoVariables.od[376]; +acadoWorkspace.state[42] = acadoVariables.od[377]; + +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]; } } void acado_shiftControls( real_t* const uEnd ) { int index; -for (index = 0; index < 49; ++index) +for (index = 0; index < 19; ++index) { acadoVariables.u[index] = acadoVariables.u[index + 1]; } if (uEnd != 0) { -acadoVariables.u[49] = uEnd[0]; +acadoVariables.u[19] = uEnd[0]; } } @@ -1866,9 +5071,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] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25] + acadoWorkspace.g[26]*acadoWorkspace.x[26] + acadoWorkspace.g[27]*acadoWorkspace.x[27] + acadoWorkspace.g[28]*acadoWorkspace.x[28] + acadoWorkspace.g[29]*acadoWorkspace.x[29] + acadoWorkspace.g[30]*acadoWorkspace.x[30] + acadoWorkspace.g[31]*acadoWorkspace.x[31] + acadoWorkspace.g[32]*acadoWorkspace.x[32] + acadoWorkspace.g[33]*acadoWorkspace.x[33] + acadoWorkspace.g[34]*acadoWorkspace.x[34] + acadoWorkspace.g[35]*acadoWorkspace.x[35] + acadoWorkspace.g[36]*acadoWorkspace.x[36] + acadoWorkspace.g[37]*acadoWorkspace.x[37] + acadoWorkspace.g[38]*acadoWorkspace.x[38] + acadoWorkspace.g[39]*acadoWorkspace.x[39] + acadoWorkspace.g[40]*acadoWorkspace.x[40] + acadoWorkspace.g[41]*acadoWorkspace.x[41] + acadoWorkspace.g[42]*acadoWorkspace.x[42] + acadoWorkspace.g[43]*acadoWorkspace.x[43] + acadoWorkspace.g[44]*acadoWorkspace.x[44] + acadoWorkspace.g[45]*acadoWorkspace.x[45] + acadoWorkspace.g[46]*acadoWorkspace.x[46] + acadoWorkspace.g[47]*acadoWorkspace.x[47] + acadoWorkspace.g[48]*acadoWorkspace.x[48] + acadoWorkspace.g[49]*acadoWorkspace.x[49] + acadoWorkspace.g[50]*acadoWorkspace.x[50] + acadoWorkspace.g[51]*acadoWorkspace.x[51] + acadoWorkspace.g[52]*acadoWorkspace.x[52] + acadoWorkspace.g[53]*acadoWorkspace.x[53]; +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 = fabs( kkt ); -for (index = 0; index < 54; ++index) +for (index = 0; index < 24; ++index) { prd = acadoWorkspace.y[index]; if (prd > 1e-12) @@ -1876,9 +5081,9 @@ kkt += fabs(acadoWorkspace.lb[index] * prd); else if (prd < -1e-12) kkt += fabs(acadoWorkspace.ub[index] * prd); } -for (index = 0; index < 100; ++index) +for (index = 0; index < 40; ++index) { -prd = acadoWorkspace.y[index + 54]; +prd = acadoWorkspace.y[index + 24]; if (prd > 1e-12) kkt += fabs(acadoWorkspace.lbA[index] * prd); else if (prd < -1e-12) @@ -1898,7 +5103,7 @@ real_t tmpDy[ 5 ]; /** Row vector of size: 4 */ real_t tmpDyN[ 4 ]; -for (lRun1 = 0; lRun1 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4]; acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1]; @@ -1931,48 +5136,48 @@ acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariable 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.objValueIn[0] = acadoVariables.x[200]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[201]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[202]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[203]; -acadoWorkspace.objValueIn[4] = acadoVariables.od[900]; -acadoWorkspace.objValueIn[5] = acadoVariables.od[901]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[902]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[903]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[904]; -acadoWorkspace.objValueIn[9] = acadoVariables.od[905]; -acadoWorkspace.objValueIn[10] = acadoVariables.od[906]; -acadoWorkspace.objValueIn[11] = acadoVariables.od[907]; -acadoWorkspace.objValueIn[12] = acadoVariables.od[908]; -acadoWorkspace.objValueIn[13] = acadoVariables.od[909]; -acadoWorkspace.objValueIn[14] = acadoVariables.od[910]; -acadoWorkspace.objValueIn[15] = acadoVariables.od[911]; -acadoWorkspace.objValueIn[16] = acadoVariables.od[912]; -acadoWorkspace.objValueIn[17] = acadoVariables.od[913]; -acadoWorkspace.objValueIn[18] = acadoVariables.od[914]; -acadoWorkspace.objValueIn[19] = acadoVariables.od[915]; -acadoWorkspace.objValueIn[20] = acadoVariables.od[916]; -acadoWorkspace.objValueIn[21] = acadoVariables.od[917]; +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[360]; +acadoWorkspace.objValueIn[5] = acadoVariables.od[361]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[362]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[363]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[364]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[365]; +acadoWorkspace.objValueIn[10] = acadoVariables.od[366]; +acadoWorkspace.objValueIn[11] = acadoVariables.od[367]; +acadoWorkspace.objValueIn[12] = acadoVariables.od[368]; +acadoWorkspace.objValueIn[13] = acadoVariables.od[369]; +acadoWorkspace.objValueIn[14] = acadoVariables.od[370]; +acadoWorkspace.objValueIn[15] = acadoVariables.od[371]; +acadoWorkspace.objValueIn[16] = acadoVariables.od[372]; +acadoWorkspace.objValueIn[17] = acadoVariables.od[373]; +acadoWorkspace.objValueIn[18] = acadoVariables.od[374]; +acadoWorkspace.objValueIn[19] = acadoVariables.od[375]; +acadoWorkspace.objValueIn[20] = acadoVariables.od[376]; +acadoWorkspace.objValueIn[21] = acadoVariables.od[377]; 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 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { -tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]; -tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5 + 1]; -tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5 + 2]; -tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5 + 3]; -tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5 + 4]; +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]; } -tmpDyN[0] = + acadoWorkspace.DyN[0]; -tmpDyN[1] = + acadoWorkspace.DyN[1]; -tmpDyN[2] = + acadoWorkspace.DyN[2]; -tmpDyN[3] = + acadoWorkspace.DyN[3]; +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]; objVal *= 0.5; diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f54bf25c06847b..0257898f692ecd 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,7 +4,7 @@ from selfdrive.controls.lib.pid import PIController STOPPING_EGO_SPEED = 0.5 -MIN_CAN_SPEED = 0.3 +MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 STARTING_TARGET_SPEED = 0.5 BRAKE_THRESHOLD_TO_PID = 0.2 @@ -101,7 +101,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ elif self.long_control_state == LongCtrlState.pid: prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 - self.v_pid = max(v_target, MIN_CAN_SPEED) + self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = - brake_max deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index 6bf14e6c60376a..264a2ae0339313 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -44,11 +44,8 @@ int main( ) h << a_ego * (1.0 + v_ego / 10.0); h << j_ego * (1.0 + v_ego / 10.0); - DMatrix Q(4,4); - Q(0,0) = 5.0; - Q(1,1) = 0.1; - Q(2,2) = 10.0; - Q(3,3) = 20.0; + // Weights are defined in mpc. + BMatrix Q(4,4); Q.setAll(true); // Terminal cost Function hN; @@ -56,16 +53,24 @@ int main( ) hN << (d_l - desired) / (0.1 * v_ego + 0.5); hN << a_ego * (1.0 + v_ego / 10.0); - DMatrix QN(3,3); - QN(0,0) = 5.0; - QN(1,1) = 0.1; - QN(2,2) = 10.0; + // Weights are defined in mpc. + BMatrix QN(3,3); QN.setAll(true); + + // Non uniform time grid + // First 5 timesteps are 0.2, after that it's 0.6 + 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 = samplingTime * controlHorizon; + const double tEnd = 10.0; - OCP ocp( tStart, tEnd, controlHorizon ); + OCP ocp( tStart, tEnd, numSteps); ocp.subjectTo(f); ocp.minimizeLSQ(Q, h); @@ -78,8 +83,9 @@ int main( ) 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( NUM_INTEGRATOR_STEPS, controlHorizon); mpc.set( MAX_NUM_QP_ITERATIONS, 500); + mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES); mpc.set( SPARSE_QP_SOLUTION, CONDENSING ); mpc.set( QP_SOLVER, QP_QPOASES ); diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 600224b33e2d7e..8020b63c9b5673 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -1,10 +1,11 @@ import os +import sys import subprocess from cffi import FFI mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -subprocess.check_output(["make", "-j4"], cwd=mpc_dir) +subprocess.check_call(["make", "-j4"], stdout=sys.stderr, cwd=mpc_dir) def _get_libmpc(mpc_id): @@ -18,13 +19,13 @@ def _get_libmpc(mpc_id): typedef struct { - double x_ego[50]; - double v_ego[50]; - double a_ego[50]; - double j_ego[50]; - double x_l[50]; - double v_l[50]; - double a_l[50]; + double x_ego[20]; + double v_ego[20]; + double a_ego[20]; + double j_ego[20]; + double x_l[20]; + double v_l[20]; + double a_l[20]; } log_t; void init(); diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc.c b/selfdrive/controls/lib/longitudinal_mpc/mpc.c index c153ae725d61e1..4e2c13c1a1c7e4 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc.c @@ -45,6 +45,22 @@ void init(){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + // Set weights + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = 3; + } + acadoVariables.W[16 * i + 0] = 5.0 * f; // exponential cost for danger zone + acadoVariables.W[16 * i + 5] = 0.1 * f; // desired distance + acadoVariables.W[16 * i + 10] = 10.0 * f; // acceleration + acadoVariables.W[16 * i + 15] = 20.0 * f; // jerk + } + acadoVariables.WN[0] = 5.0; // exponential cost for danger zone + acadoVariables.WN[4] = 0.1; // desired distance + acadoVariables.WN[8] = 10.0; // acceleration + } void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l){ @@ -58,6 +74,9 @@ void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, doub double dt = 0.2; for (i = 0; i < N + 1; ++i){ + if (i > 4){ + dt = 0.6; + } acadoVariables.x[i*NX] = x_ego; acadoVariables.x[i*NX+1] = v_ego; acadoVariables.x[i*NX+2] = a_ego; diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h index 8528e462f91e43..2c0ab64ef141c7 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_common.h @@ -62,7 +62,7 @@ extern "C" /** Indicator for fixed initial state. */ #define ACADO_INITIAL_STATE_FIXED 1 /** Number of control/estimation intervals. */ -#define ACADO_N 50 +#define ACADO_N 20 /** Number of online data values. */ #define ACADO_NOD 1 /** Number of path constraints. */ @@ -80,9 +80,7 @@ extern "C" /** Number of references/measurements on the last (N + 1)st node. */ #define ACADO_NYN 3 /** Total number of QP optimization variables. */ -#define ACADO_QP_NV 56 -/** Number of integration steps per shooting interval. */ -#define ACADO_RK_NIS 1 +#define ACADO_QP_NV 26 /** Number of Runge-Kutta stages per integration step. */ #define ACADO_RK_NSTAGES 4 /** Providing interface for arrival cost. */ @@ -90,7 +88,7 @@ extern "C" /** Indicator for usage of non-hard-coded linear terms in the objective. */ #define ACADO_USE_LINEAR_TERMS 0 /** Indicator for type of fixed weighting matrices. */ -#define ACADO_WEIGHTING_MATRICES_TYPE 0 +#define ACADO_WEIGHTING_MATRICES_TYPE 2 /* @@ -104,36 +102,42 @@ extern "C" typedef struct ACADOvariables_ { int dummy; -/** Matrix of size: 51 x 6 (row major format) +/** Matrix of size: 21 x 6 (row major format) * - * Matrix containing 51 differential variable vectors. + * Matrix containing 21 differential variable vectors. */ -real_t x[ 306 ]; +real_t x[ 126 ]; -/** Column vector of size: 50 +/** Column vector of size: 20 * - * Matrix containing 50 control variable vectors. + * Matrix containing 20 control variable vectors. */ -real_t u[ 50 ]; +real_t u[ 20 ]; -/** Column vector of size: 51 +/** Column vector of size: 21 * - * Matrix containing 51 online data vectors. + * Matrix containing 21 online data vectors. */ -real_t od[ 51 ]; +real_t od[ 21 ]; -/** Column vector of size: 200 +/** Column vector of size: 80 * - * Matrix containing 50 reference/measurement vectors of size 4 for first 50 nodes. + * Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes. */ -real_t y[ 200 ]; +real_t y[ 80 ]; /** Column vector of size: 3 * - * Reference/measurement vector for the 51. node. + * Reference/measurement vector for the 21. node. */ real_t yN[ 3 ]; +/** Matrix of size: 80 x 4 (row major format) */ +real_t W[ 320 ]; + +/** Matrix of size: 3 x 3 (row major format) */ +real_t WN[ 9 ]; + /** Column vector of size: 6 * * Current state feedback vector. @@ -162,20 +166,20 @@ real_t rk_kkk[ 192 ]; /** Row vector of size: 50 */ real_t state[ 50 ]; -/** Column vector of size: 300 */ -real_t d[ 300 ]; +/** Column vector of size: 120 */ +real_t d[ 120 ]; -/** Column vector of size: 200 */ -real_t Dy[ 200 ]; +/** Column vector of size: 80 */ +real_t Dy[ 80 ]; /** Column vector of size: 3 */ real_t DyN[ 3 ]; -/** Matrix of size: 300 x 6 (row major format) */ -real_t evGx[ 1800 ]; +/** Matrix of size: 120 x 6 (row major format) */ +real_t evGx[ 720 ]; -/** Column vector of size: 300 */ -real_t evGu[ 300 ]; +/** Column vector of size: 120 */ +real_t evGu[ 120 ]; /** Column vector of size: 32 */ real_t objAuxVar[ 32 ]; @@ -186,20 +190,20 @@ real_t objValueIn[ 8 ]; /** Row vector of size: 32 */ real_t objValueOut[ 32 ]; -/** Matrix of size: 300 x 6 (row major format) */ -real_t Q1[ 1800 ]; +/** Matrix of size: 120 x 6 (row major format) */ +real_t Q1[ 720 ]; -/** Matrix of size: 300 x 4 (row major format) */ -real_t Q2[ 1200 ]; +/** Matrix of size: 120 x 4 (row major format) */ +real_t Q2[ 480 ]; -/** Column vector of size: 50 */ -real_t R1[ 50 ]; +/** Column vector of size: 20 */ +real_t R1[ 20 ]; -/** Matrix of size: 50 x 4 (row major format) */ -real_t R2[ 200 ]; +/** Matrix of size: 20 x 4 (row major format) */ +real_t R2[ 80 ]; -/** Column vector of size: 300 */ -real_t S1[ 300 ]; +/** Column vector of size: 120 */ +real_t S1[ 120 ]; /** Matrix of size: 6 x 6 (row major format) */ real_t QN1[ 36 ]; @@ -213,50 +217,50 @@ real_t Dx0[ 6 ]; /** Matrix of size: 6 x 6 (row major format) */ real_t T[ 36 ]; -/** Column vector of size: 7650 */ -real_t E[ 7650 ]; +/** Column vector of size: 1260 */ +real_t E[ 1260 ]; -/** Column vector of size: 7650 */ -real_t QE[ 7650 ]; +/** Column vector of size: 1260 */ +real_t QE[ 1260 ]; -/** Matrix of size: 300 x 6 (row major format) */ -real_t QGx[ 1800 ]; +/** Matrix of size: 120 x 6 (row major format) */ +real_t QGx[ 720 ]; -/** Column vector of size: 300 */ -real_t Qd[ 300 ]; +/** Column vector of size: 120 */ +real_t Qd[ 120 ]; -/** Column vector of size: 306 */ -real_t QDy[ 306 ]; +/** Column vector of size: 126 */ +real_t QDy[ 126 ]; -/** Matrix of size: 50 x 6 (row major format) */ -real_t H10[ 300 ]; +/** Matrix of size: 20 x 6 (row major format) */ +real_t H10[ 120 ]; -/** Matrix of size: 56 x 56 (row major format) */ -real_t H[ 3136 ]; +/** Matrix of size: 26 x 26 (row major format) */ +real_t H[ 676 ]; -/** Matrix of size: 50 x 56 (row major format) */ -real_t A[ 2800 ]; +/** Matrix of size: 20 x 26 (row major format) */ +real_t A[ 520 ]; -/** Column vector of size: 56 */ -real_t g[ 56 ]; +/** Column vector of size: 26 */ +real_t g[ 26 ]; -/** Column vector of size: 56 */ -real_t lb[ 56 ]; +/** Column vector of size: 26 */ +real_t lb[ 26 ]; -/** Column vector of size: 56 */ -real_t ub[ 56 ]; +/** Column vector of size: 26 */ +real_t ub[ 26 ]; -/** Column vector of size: 50 */ -real_t lbA[ 50 ]; +/** Column vector of size: 20 */ +real_t lbA[ 20 ]; -/** Column vector of size: 50 */ -real_t ubA[ 50 ]; +/** Column vector of size: 20 */ +real_t ubA[ 20 ]; -/** Column vector of size: 56 */ -real_t x[ 56 ]; +/** Column vector of size: 26 */ +real_t x[ 26 ]; -/** Column vector of size: 106 */ -real_t y[ 106 ]; +/** Column vector of size: 46 */ +real_t y[ 46 ]; } ACADOworkspace; @@ -270,10 +274,11 @@ real_t y[ 106 ]; * * \param rk_eta Working array to pass the input values and return the results. * \param resetIntegrator The internal memory of the integrator can be reset. + * \param rk_index Number of the shooting interval. * * \return Status code of the integrator. */ -int acado_integrate( real_t* const rk_eta, int resetIntegrator ); +int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index ); /** Export of an ACADO symbolic function. * @@ -306,7 +311,7 @@ void acado_initializeNodesByForwardSimulation( ); /** Shift differential variables vector by one interval. * - * \param strategy Shifting strategy: 1. Initialize node 51 with xEnd. 2. Initialize node 51 by forward simulation. + * \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 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/longitudinal_mpc/mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c index 986d046a52c994..584d8fb857f530 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_integrator.c @@ -78,11 +78,13 @@ out[47] = (((real_t)(0.0000000000000000e+00)-od[0])*xd[47]); } /* Fixed step size:0.2 */ -int acado_integrate( real_t* const rk_eta, int resetIntegrator ) +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 numInts = numSteps[rk_index]; acadoWorkspace.rk_ttt = 0.0000000000000000e+00; rk_eta[6] = 1.0000000000000000e+00; rk_eta[7] = 0.0000000000000000e+00; @@ -131,6 +133,7 @@ acadoWorkspace.rk_xxx[49] = rk_eta[49]; for (run1 = 0; run1 < 1; ++run1) { +for(run1 = 0; run1 < numInts; run1++ ) { acadoWorkspace.rk_xxx[0] = + rk_eta[0]; acadoWorkspace.rk_xxx[1] = + rk_eta[1]; acadoWorkspace.rk_xxx[2] = + rk_eta[2]; @@ -180,203 +183,204 @@ acadoWorkspace.rk_xxx[45] = + rk_eta[45]; acadoWorkspace.rk_xxx[46] = + rk_eta[46]; acadoWorkspace.rk_xxx[47] = + rk_eta[47]; acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk ); -acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[0] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[1] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[2] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[3] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[4] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[5] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[6] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[7] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[8] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[9] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[10] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[11] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[12] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[13] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[14] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[15] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[16] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[17] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[18] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[19] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[20] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[21] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[22] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[23] + rk_eta[23]; -acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[24] + rk_eta[24]; -acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[25] + rk_eta[25]; -acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[26] + rk_eta[26]; -acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[27] + rk_eta[27]; -acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[28] + rk_eta[28]; -acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[29] + rk_eta[29]; -acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[30] + rk_eta[30]; -acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[31] + rk_eta[31]; -acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[32] + rk_eta[32]; -acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[33] + rk_eta[33]; -acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[34] + rk_eta[34]; -acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[35] + rk_eta[35]; -acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[36] + rk_eta[36]; -acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[37] + rk_eta[37]; -acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[38] + rk_eta[38]; -acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[39] + rk_eta[39]; -acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[40] + rk_eta[40]; -acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[41] + rk_eta[41]; -acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[42] + rk_eta[42]; -acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[43] + rk_eta[43]; -acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[44] + rk_eta[44]; -acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[45] + rk_eta[45]; -acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[46] + rk_eta[46]; -acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[47] + rk_eta[47]; +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23]; +acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[24]; +acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[25]; +acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[26]; +acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[27]; +acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[28]; +acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[29]; +acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[30]; +acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[31]; +acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[32]; +acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[33]; +acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[34]; +acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[35]; +acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[36]; +acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[37]; +acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[38]; +acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[39]; +acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[40]; +acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[41]; +acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[42]; +acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[43]; +acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[44]; +acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[45]; +acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[46]; +acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[47] + rk_eta[47]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[48] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[49] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[50] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[51] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[52] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[53] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[54] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[55] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[56] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[57] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[58] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[59] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[60] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[61] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[62] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[63] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[64] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[65] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[66] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[67] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[68] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[69] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[70] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[71] + rk_eta[23]; -acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[72] + rk_eta[24]; -acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[73] + rk_eta[25]; -acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[74] + rk_eta[26]; -acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[75] + rk_eta[27]; -acadoWorkspace.rk_xxx[28] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[76] + rk_eta[28]; -acadoWorkspace.rk_xxx[29] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[77] + rk_eta[29]; -acadoWorkspace.rk_xxx[30] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[78] + rk_eta[30]; -acadoWorkspace.rk_xxx[31] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[79] + rk_eta[31]; -acadoWorkspace.rk_xxx[32] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[80] + rk_eta[32]; -acadoWorkspace.rk_xxx[33] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[81] + rk_eta[33]; -acadoWorkspace.rk_xxx[34] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[82] + rk_eta[34]; -acadoWorkspace.rk_xxx[35] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[83] + rk_eta[35]; -acadoWorkspace.rk_xxx[36] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[84] + rk_eta[36]; -acadoWorkspace.rk_xxx[37] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[85] + rk_eta[37]; -acadoWorkspace.rk_xxx[38] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[86] + rk_eta[38]; -acadoWorkspace.rk_xxx[39] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[87] + rk_eta[39]; -acadoWorkspace.rk_xxx[40] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[88] + rk_eta[40]; -acadoWorkspace.rk_xxx[41] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[89] + rk_eta[41]; -acadoWorkspace.rk_xxx[42] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[90] + rk_eta[42]; -acadoWorkspace.rk_xxx[43] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[91] + rk_eta[43]; -acadoWorkspace.rk_xxx[44] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[92] + rk_eta[44]; -acadoWorkspace.rk_xxx[45] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[93] + rk_eta[45]; -acadoWorkspace.rk_xxx[46] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[94] + rk_eta[46]; -acadoWorkspace.rk_xxx[47] = + (real_t)1.0000000000000001e-01*acadoWorkspace.rk_kkk[95] + rk_eta[47]; +acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23]; +acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[72] + rk_eta[24]; +acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[73] + rk_eta[25]; +acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[74] + rk_eta[26]; +acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[75] + rk_eta[27]; +acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[76] + rk_eta[28]; +acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[77] + rk_eta[29]; +acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[78] + rk_eta[30]; +acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[79] + rk_eta[31]; +acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[80] + rk_eta[32]; +acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[81] + rk_eta[33]; +acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[82] + rk_eta[34]; +acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[83] + rk_eta[35]; +acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[84] + rk_eta[36]; +acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[85] + rk_eta[37]; +acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[86] + rk_eta[38]; +acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[87] + rk_eta[39]; +acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[88] + rk_eta[40]; +acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[89] + rk_eta[41]; +acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[90] + rk_eta[42]; +acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[91] + rk_eta[43]; +acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[92] + rk_eta[44]; +acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[93] + rk_eta[45]; +acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[94] + rk_eta[46]; +acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[95] + rk_eta[47]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) ); -acadoWorkspace.rk_xxx[0] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0]; -acadoWorkspace.rk_xxx[1] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1]; -acadoWorkspace.rk_xxx[2] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2]; -acadoWorkspace.rk_xxx[3] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3]; -acadoWorkspace.rk_xxx[4] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4]; -acadoWorkspace.rk_xxx[5] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5]; -acadoWorkspace.rk_xxx[6] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6]; -acadoWorkspace.rk_xxx[7] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7]; -acadoWorkspace.rk_xxx[8] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8]; -acadoWorkspace.rk_xxx[9] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9]; -acadoWorkspace.rk_xxx[10] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10]; -acadoWorkspace.rk_xxx[11] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11]; -acadoWorkspace.rk_xxx[12] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12]; -acadoWorkspace.rk_xxx[13] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13]; -acadoWorkspace.rk_xxx[14] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14]; -acadoWorkspace.rk_xxx[15] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15]; -acadoWorkspace.rk_xxx[16] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16]; -acadoWorkspace.rk_xxx[17] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17]; -acadoWorkspace.rk_xxx[18] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18]; -acadoWorkspace.rk_xxx[19] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19]; -acadoWorkspace.rk_xxx[20] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20]; -acadoWorkspace.rk_xxx[21] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21]; -acadoWorkspace.rk_xxx[22] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22]; -acadoWorkspace.rk_xxx[23] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23]; -acadoWorkspace.rk_xxx[24] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24]; -acadoWorkspace.rk_xxx[25] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25]; -acadoWorkspace.rk_xxx[26] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26]; -acadoWorkspace.rk_xxx[27] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27]; -acadoWorkspace.rk_xxx[28] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28]; -acadoWorkspace.rk_xxx[29] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29]; -acadoWorkspace.rk_xxx[30] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30]; -acadoWorkspace.rk_xxx[31] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31]; -acadoWorkspace.rk_xxx[32] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32]; -acadoWorkspace.rk_xxx[33] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33]; -acadoWorkspace.rk_xxx[34] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34]; -acadoWorkspace.rk_xxx[35] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35]; -acadoWorkspace.rk_xxx[36] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36]; -acadoWorkspace.rk_xxx[37] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37]; -acadoWorkspace.rk_xxx[38] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38]; -acadoWorkspace.rk_xxx[39] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39]; -acadoWorkspace.rk_xxx[40] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40]; -acadoWorkspace.rk_xxx[41] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41]; -acadoWorkspace.rk_xxx[42] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42]; -acadoWorkspace.rk_xxx[43] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43]; -acadoWorkspace.rk_xxx[44] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44]; -acadoWorkspace.rk_xxx[45] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45]; -acadoWorkspace.rk_xxx[46] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46]; -acadoWorkspace.rk_xxx[47] = + (real_t)2.0000000000000001e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47]; +acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0]; +acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1]; +acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2]; +acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3]; +acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4]; +acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5]; +acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6]; +acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7]; +acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8]; +acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9]; +acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10]; +acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11]; +acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12]; +acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13]; +acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14]; +acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15]; +acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16]; +acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17]; +acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18]; +acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19]; +acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20]; +acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21]; +acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22]; +acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23]; +acadoWorkspace.rk_xxx[24] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24]; +acadoWorkspace.rk_xxx[25] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25]; +acadoWorkspace.rk_xxx[26] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26]; +acadoWorkspace.rk_xxx[27] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27]; +acadoWorkspace.rk_xxx[28] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28]; +acadoWorkspace.rk_xxx[29] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29]; +acadoWorkspace.rk_xxx[30] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30]; +acadoWorkspace.rk_xxx[31] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31]; +acadoWorkspace.rk_xxx[32] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32]; +acadoWorkspace.rk_xxx[33] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33]; +acadoWorkspace.rk_xxx[34] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34]; +acadoWorkspace.rk_xxx[35] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35]; +acadoWorkspace.rk_xxx[36] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36]; +acadoWorkspace.rk_xxx[37] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37]; +acadoWorkspace.rk_xxx[38] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38]; +acadoWorkspace.rk_xxx[39] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39]; +acadoWorkspace.rk_xxx[40] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40]; +acadoWorkspace.rk_xxx[41] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41]; +acadoWorkspace.rk_xxx[42] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42]; +acadoWorkspace.rk_xxx[43] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43]; +acadoWorkspace.rk_xxx[44] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44]; +acadoWorkspace.rk_xxx[45] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45]; +acadoWorkspace.rk_xxx[46] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46]; +acadoWorkspace.rk_xxx[47] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47]; acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) ); -rk_eta[0] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[144]; -rk_eta[1] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[145]; -rk_eta[2] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[146]; -rk_eta[3] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[147]; -rk_eta[4] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[148]; -rk_eta[5] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[149]; -rk_eta[6] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[150]; -rk_eta[7] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[151]; -rk_eta[8] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[152]; -rk_eta[9] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[153]; -rk_eta[10] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[154]; -rk_eta[11] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[155]; -rk_eta[12] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[156]; -rk_eta[13] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[157]; -rk_eta[14] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[158]; -rk_eta[15] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[159]; -rk_eta[16] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[160]; -rk_eta[17] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[161]; -rk_eta[18] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[162]; -rk_eta[19] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[163]; -rk_eta[20] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[164]; -rk_eta[21] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[165]; -rk_eta[22] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[166]; -rk_eta[23] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[167]; -rk_eta[24] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[168]; -rk_eta[25] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[169]; -rk_eta[26] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[170]; -rk_eta[27] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[171]; -rk_eta[28] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[172]; -rk_eta[29] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[173]; -rk_eta[30] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[174]; -rk_eta[31] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[175]; -rk_eta[32] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[176]; -rk_eta[33] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[177]; -rk_eta[34] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[178]; -rk_eta[35] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[179]; -rk_eta[36] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[180]; -rk_eta[37] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[181]; -rk_eta[38] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[182]; -rk_eta[39] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[183]; -rk_eta[40] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[184]; -rk_eta[41] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[185]; -rk_eta[42] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[186]; -rk_eta[43] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[187]; -rk_eta[44] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[188]; -rk_eta[45] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[189]; -rk_eta[46] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[190]; -rk_eta[47] += + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666666e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333333e-02*acadoWorkspace.rk_kkk[191]; +rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[144]; +rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[145]; +rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[146]; +rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[147]; +rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[148]; +rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[149]; +rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[150]; +rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[151]; +rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[152]; +rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[153]; +rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[154]; +rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[155]; +rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[156]; +rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[157]; +rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[158]; +rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[159]; +rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[160]; +rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[161]; +rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[162]; +rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[163]; +rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[164]; +rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[165]; +rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[166]; +rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[167]; +rk_eta[24] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[168]; +rk_eta[25] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[169]; +rk_eta[26] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[170]; +rk_eta[27] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[171]; +rk_eta[28] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[172]; +rk_eta[29] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[173]; +rk_eta[30] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[174]; +rk_eta[31] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[175]; +rk_eta[32] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[176]; +rk_eta[33] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[177]; +rk_eta[34] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[178]; +rk_eta[35] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[179]; +rk_eta[36] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[180]; +rk_eta[37] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[181]; +rk_eta[38] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[182]; +rk_eta[39] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[183]; +rk_eta[40] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[184]; +rk_eta[41] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[185]; +rk_eta[42] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[186]; +rk_eta[43] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[187]; +rk_eta[44] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[188]; +rk_eta[45] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[189]; +rk_eta[46] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[190]; +rk_eta[47] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[191]; acadoWorkspace.rk_ttt += 1.0000000000000000e+00; } +} error = 0; return error; } diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp index 4fa43e054cca2b..a4d9f8cde2777f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.cpp @@ -40,7 +40,7 @@ int acado_solve( void ) { acado_nWSR = QPOASES_NWSRMAX; - QProblem qp(56, 50); + QProblem qp(26, 20); 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/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp index a52e9b641daed9..3b3c75aa111c72 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_qpoases_interface.hpp @@ -37,9 +37,9 @@ */ /** Maximum number of optimization variables. */ -#define QPOASES_NVMAX 56 +#define QPOASES_NVMAX 26 /** Maximum number of constraints. */ -#define QPOASES_NCMAX 50 +#define QPOASES_NCMAX 20 /** Maximum number of working set recalculations. */ #define QPOASES_NWSRMAX 500 /** Print level for qpOASES. */ diff --git a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c index e4b2cbcbd83631..94aef8c05c87ae 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/mpc_export/acado_solver.c @@ -35,7 +35,7 @@ int ret; int lRun1; ret = 0; -for (lRun1 = 0; lRun1 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 6]; acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 6 + 1]; @@ -47,7 +47,7 @@ acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[lRun1]; acadoWorkspace.state[49] = acadoVariables.od[lRun1]; -ret = acado_integrate(acadoWorkspace.state, 1); +ret = acado_integrate(acadoWorkspace.state, 1, lRun1); acadoWorkspace.d[lRun1 * 6] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 6 + 6]; acadoWorkspace.d[lRun1 * 6 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 6 + 7]; @@ -242,32 +242,32 @@ out[19] = (real_t)(0.0000000000000000e+00); out[20] = (real_t)(0.0000000000000000e+00); } -void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpQ1, real_t* const tmpQ2 ) +void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 ) { -tmpQ2[0] = + tmpFx[0]*(real_t)5.0000000000000000e+00; -tmpQ2[1] = + tmpFx[6]*(real_t)1.0000000000000001e-01; -tmpQ2[2] = + tmpFx[12]*(real_t)1.0000000000000000e+01; -tmpQ2[3] = + tmpFx[18]*(real_t)2.0000000000000000e+01; -tmpQ2[4] = + tmpFx[1]*(real_t)5.0000000000000000e+00; -tmpQ2[5] = + tmpFx[7]*(real_t)1.0000000000000001e-01; -tmpQ2[6] = + tmpFx[13]*(real_t)1.0000000000000000e+01; -tmpQ2[7] = + tmpFx[19]*(real_t)2.0000000000000000e+01; -tmpQ2[8] = + tmpFx[2]*(real_t)5.0000000000000000e+00; -tmpQ2[9] = + tmpFx[8]*(real_t)1.0000000000000001e-01; -tmpQ2[10] = + tmpFx[14]*(real_t)1.0000000000000000e+01; -tmpQ2[11] = + tmpFx[20]*(real_t)2.0000000000000000e+01; -tmpQ2[12] = + tmpFx[3]*(real_t)5.0000000000000000e+00; -tmpQ2[13] = + tmpFx[9]*(real_t)1.0000000000000001e-01; -tmpQ2[14] = + tmpFx[15]*(real_t)1.0000000000000000e+01; -tmpQ2[15] = + tmpFx[21]*(real_t)2.0000000000000000e+01; -tmpQ2[16] = + tmpFx[4]*(real_t)5.0000000000000000e+00; -tmpQ2[17] = + tmpFx[10]*(real_t)1.0000000000000001e-01; -tmpQ2[18] = + tmpFx[16]*(real_t)1.0000000000000000e+01; -tmpQ2[19] = + tmpFx[22]*(real_t)2.0000000000000000e+01; -tmpQ2[20] = + tmpFx[5]*(real_t)5.0000000000000000e+00; -tmpQ2[21] = + tmpFx[11]*(real_t)1.0000000000000001e-01; -tmpQ2[22] = + tmpFx[17]*(real_t)1.0000000000000000e+01; -tmpQ2[23] = + tmpFx[23]*(real_t)2.0000000000000000e+01; +tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[6]*tmpObjS[4] + tmpFx[12]*tmpObjS[8] + tmpFx[18]*tmpObjS[12]; +tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[6]*tmpObjS[5] + tmpFx[12]*tmpObjS[9] + tmpFx[18]*tmpObjS[13]; +tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[6]*tmpObjS[6] + tmpFx[12]*tmpObjS[10] + tmpFx[18]*tmpObjS[14]; +tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[6]*tmpObjS[7] + tmpFx[12]*tmpObjS[11] + tmpFx[18]*tmpObjS[15]; +tmpQ2[4] = + tmpFx[1]*tmpObjS[0] + tmpFx[7]*tmpObjS[4] + tmpFx[13]*tmpObjS[8] + tmpFx[19]*tmpObjS[12]; +tmpQ2[5] = + tmpFx[1]*tmpObjS[1] + tmpFx[7]*tmpObjS[5] + tmpFx[13]*tmpObjS[9] + tmpFx[19]*tmpObjS[13]; +tmpQ2[6] = + tmpFx[1]*tmpObjS[2] + tmpFx[7]*tmpObjS[6] + tmpFx[13]*tmpObjS[10] + tmpFx[19]*tmpObjS[14]; +tmpQ2[7] = + tmpFx[1]*tmpObjS[3] + tmpFx[7]*tmpObjS[7] + tmpFx[13]*tmpObjS[11] + tmpFx[19]*tmpObjS[15]; +tmpQ2[8] = + tmpFx[2]*tmpObjS[0] + tmpFx[8]*tmpObjS[4] + tmpFx[14]*tmpObjS[8] + tmpFx[20]*tmpObjS[12]; +tmpQ2[9] = + tmpFx[2]*tmpObjS[1] + tmpFx[8]*tmpObjS[5] + tmpFx[14]*tmpObjS[9] + tmpFx[20]*tmpObjS[13]; +tmpQ2[10] = + tmpFx[2]*tmpObjS[2] + tmpFx[8]*tmpObjS[6] + tmpFx[14]*tmpObjS[10] + tmpFx[20]*tmpObjS[14]; +tmpQ2[11] = + tmpFx[2]*tmpObjS[3] + tmpFx[8]*tmpObjS[7] + tmpFx[14]*tmpObjS[11] + tmpFx[20]*tmpObjS[15]; +tmpQ2[12] = + tmpFx[3]*tmpObjS[0] + tmpFx[9]*tmpObjS[4] + tmpFx[15]*tmpObjS[8] + tmpFx[21]*tmpObjS[12]; +tmpQ2[13] = + tmpFx[3]*tmpObjS[1] + tmpFx[9]*tmpObjS[5] + tmpFx[15]*tmpObjS[9] + tmpFx[21]*tmpObjS[13]; +tmpQ2[14] = + tmpFx[3]*tmpObjS[2] + tmpFx[9]*tmpObjS[6] + tmpFx[15]*tmpObjS[10] + tmpFx[21]*tmpObjS[14]; +tmpQ2[15] = + tmpFx[3]*tmpObjS[3] + tmpFx[9]*tmpObjS[7] + tmpFx[15]*tmpObjS[11] + tmpFx[21]*tmpObjS[15]; +tmpQ2[16] = + tmpFx[4]*tmpObjS[0] + tmpFx[10]*tmpObjS[4] + tmpFx[16]*tmpObjS[8] + tmpFx[22]*tmpObjS[12]; +tmpQ2[17] = + tmpFx[4]*tmpObjS[1] + tmpFx[10]*tmpObjS[5] + tmpFx[16]*tmpObjS[9] + tmpFx[22]*tmpObjS[13]; +tmpQ2[18] = + tmpFx[4]*tmpObjS[2] + tmpFx[10]*tmpObjS[6] + tmpFx[16]*tmpObjS[10] + tmpFx[22]*tmpObjS[14]; +tmpQ2[19] = + tmpFx[4]*tmpObjS[3] + tmpFx[10]*tmpObjS[7] + tmpFx[16]*tmpObjS[11] + tmpFx[22]*tmpObjS[15]; +tmpQ2[20] = + tmpFx[5]*tmpObjS[0] + tmpFx[11]*tmpObjS[4] + tmpFx[17]*tmpObjS[8] + tmpFx[23]*tmpObjS[12]; +tmpQ2[21] = + tmpFx[5]*tmpObjS[1] + tmpFx[11]*tmpObjS[5] + tmpFx[17]*tmpObjS[9] + tmpFx[23]*tmpObjS[13]; +tmpQ2[22] = + tmpFx[5]*tmpObjS[2] + tmpFx[11]*tmpObjS[6] + tmpFx[17]*tmpObjS[10] + tmpFx[23]*tmpObjS[14]; +tmpQ2[23] = + tmpFx[5]*tmpObjS[3] + tmpFx[11]*tmpObjS[7] + tmpFx[17]*tmpObjS[11] + tmpFx[23]*tmpObjS[15]; tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[12] + tmpQ2[3]*tmpFx[18]; tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[13] + tmpQ2[3]*tmpFx[19]; tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[8] + tmpQ2[2]*tmpFx[14] + tmpQ2[3]*tmpFx[20]; @@ -306,35 +306,35 @@ tmpQ1[34] = + tmpQ2[20]*tmpFx[4] + tmpQ2[21]*tmpFx[10] + tmpQ2[22]*tmpFx[16] + t tmpQ1[35] = + tmpQ2[20]*tmpFx[5] + tmpQ2[21]*tmpFx[11] + tmpQ2[22]*tmpFx[17] + tmpQ2[23]*tmpFx[23]; } -void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpR1, real_t* const tmpR2 ) +void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 ) { -tmpR2[0] = + tmpFu[0]*(real_t)5.0000000000000000e+00; -tmpR2[1] = + tmpFu[1]*(real_t)1.0000000000000001e-01; -tmpR2[2] = + tmpFu[2]*(real_t)1.0000000000000000e+01; -tmpR2[3] = + tmpFu[3]*(real_t)2.0000000000000000e+01; +tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[4] + tmpFu[2]*tmpObjS[8] + tmpFu[3]*tmpObjS[12]; +tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[9] + tmpFu[3]*tmpObjS[13]; +tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[14]; +tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[15]; tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3]; } -void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpQN1, real_t* const tmpQN2 ) +void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 ) { -tmpQN2[0] = + tmpFx[0]*(real_t)5.0000000000000000e+00; -tmpQN2[1] = + tmpFx[6]*(real_t)1.0000000000000001e-01; -tmpQN2[2] = + tmpFx[12]*(real_t)1.0000000000000000e+01; -tmpQN2[3] = + tmpFx[1]*(real_t)5.0000000000000000e+00; -tmpQN2[4] = + tmpFx[7]*(real_t)1.0000000000000001e-01; -tmpQN2[5] = + tmpFx[13]*(real_t)1.0000000000000000e+01; -tmpQN2[6] = + tmpFx[2]*(real_t)5.0000000000000000e+00; -tmpQN2[7] = + tmpFx[8]*(real_t)1.0000000000000001e-01; -tmpQN2[8] = + tmpFx[14]*(real_t)1.0000000000000000e+01; -tmpQN2[9] = + tmpFx[3]*(real_t)5.0000000000000000e+00; -tmpQN2[10] = + tmpFx[9]*(real_t)1.0000000000000001e-01; -tmpQN2[11] = + tmpFx[15]*(real_t)1.0000000000000000e+01; -tmpQN2[12] = + tmpFx[4]*(real_t)5.0000000000000000e+00; -tmpQN2[13] = + tmpFx[10]*(real_t)1.0000000000000001e-01; -tmpQN2[14] = + tmpFx[16]*(real_t)1.0000000000000000e+01; -tmpQN2[15] = + tmpFx[5]*(real_t)5.0000000000000000e+00; -tmpQN2[16] = + tmpFx[11]*(real_t)1.0000000000000001e-01; -tmpQN2[17] = + tmpFx[17]*(real_t)1.0000000000000000e+01; +tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[3] + tmpFx[12]*tmpObjSEndTerm[6]; +tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[4] + tmpFx[12]*tmpObjSEndTerm[7]; +tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[6]*tmpObjSEndTerm[5] + tmpFx[12]*tmpObjSEndTerm[8]; +tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[3] + tmpFx[13]*tmpObjSEndTerm[6]; +tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[4] + tmpFx[13]*tmpObjSEndTerm[7]; +tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[7]*tmpObjSEndTerm[5] + tmpFx[13]*tmpObjSEndTerm[8]; +tmpQN2[6] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[8]*tmpObjSEndTerm[3] + tmpFx[14]*tmpObjSEndTerm[6]; +tmpQN2[7] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[8]*tmpObjSEndTerm[4] + tmpFx[14]*tmpObjSEndTerm[7]; +tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[8]*tmpObjSEndTerm[5] + tmpFx[14]*tmpObjSEndTerm[8]; +tmpQN2[9] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[9]*tmpObjSEndTerm[3] + tmpFx[15]*tmpObjSEndTerm[6]; +tmpQN2[10] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[9]*tmpObjSEndTerm[4] + tmpFx[15]*tmpObjSEndTerm[7]; +tmpQN2[11] = + tmpFx[3]*tmpObjSEndTerm[2] + tmpFx[9]*tmpObjSEndTerm[5] + tmpFx[15]*tmpObjSEndTerm[8]; +tmpQN2[12] = + tmpFx[4]*tmpObjSEndTerm[0] + tmpFx[10]*tmpObjSEndTerm[3] + tmpFx[16]*tmpObjSEndTerm[6]; +tmpQN2[13] = + tmpFx[4]*tmpObjSEndTerm[1] + tmpFx[10]*tmpObjSEndTerm[4] + tmpFx[16]*tmpObjSEndTerm[7]; +tmpQN2[14] = + tmpFx[4]*tmpObjSEndTerm[2] + tmpFx[10]*tmpObjSEndTerm[5] + tmpFx[16]*tmpObjSEndTerm[8]; +tmpQN2[15] = + tmpFx[5]*tmpObjSEndTerm[0] + tmpFx[11]*tmpObjSEndTerm[3] + tmpFx[17]*tmpObjSEndTerm[6]; +tmpQN2[16] = + tmpFx[5]*tmpObjSEndTerm[1] + tmpFx[11]*tmpObjSEndTerm[4] + tmpFx[17]*tmpObjSEndTerm[7]; +tmpQN2[17] = + tmpFx[5]*tmpObjSEndTerm[2] + tmpFx[11]*tmpObjSEndTerm[5] + tmpFx[17]*tmpObjSEndTerm[8]; tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[12]; tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[13]; tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[8] + tmpQN2[2]*tmpFx[14]; @@ -376,7 +376,7 @@ tmpQN1[35] = + tmpQN2[15]*tmpFx[5] + tmpQN2[16]*tmpFx[11] + tmpQN2[17]*tmpFx[17] void acado_evaluateObjective( ) { int runObj; -for (runObj = 0; runObj < 50; ++runObj) +for (runObj = 0; runObj < 20; ++runObj) { acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 6]; acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 6 + 1]; @@ -393,25 +393,25 @@ acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; acadoWorkspace.Dy[runObj * 4 + 3] = acadoWorkspace.objValueOut[3]; -acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoWorkspace.Q1[ runObj * 36 ]), &(acadoWorkspace.Q2[ runObj * 24 ]) ); +acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.Q1[ runObj * 36 ]), &(acadoWorkspace.Q2[ runObj * 24 ]) ); -acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 28 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) ); +acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 28 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) ); } -acadoWorkspace.objValueIn[0] = acadoVariables.x[300]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[301]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[302]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[303]; -acadoWorkspace.objValueIn[4] = acadoVariables.x[304]; -acadoWorkspace.objValueIn[5] = acadoVariables.x[305]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[120]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[121]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; +acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; +acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[20]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2]; -acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 3 ]), acadoWorkspace.QN1, acadoWorkspace.QN2 ); +acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 3 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 ); } @@ -527,22 +527,22 @@ Gu2[5] = Gu1[5]; void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 ) { -acadoWorkspace.H[(iRow * 56 + 336) + (iCol + 6)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3] + Gu1[4]*Gu2[4] + Gu1[5]*Gu2[5]; +acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3] + Gu1[4]*Gu2[4] + Gu1[5]*Gu2[5]; } void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 ) { -acadoWorkspace.H[(iRow * 56 + 336) + (iCol + 6)] = R11[0]; +acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = R11[0]; } void acado_zeroBlockH11( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 56 + 336) + (iCol + 6)] = 0.0000000000000000e+00; +acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = 0.0000000000000000e+00; } void acado_copyHTH( int iRow, int iCol ) { -acadoWorkspace.H[(iRow * 56 + 336) + (iCol + 6)] = acadoWorkspace.H[(iCol * 56 + 336) + (iRow + 6)]; +acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = acadoWorkspace.H[(iCol * 26 + 156) + (iRow + 6)]; } void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew ) @@ -618,36 +618,36 @@ acadoWorkspace.H[2] = 0.0000000000000000e+00; acadoWorkspace.H[3] = 0.0000000000000000e+00; acadoWorkspace.H[4] = 0.0000000000000000e+00; acadoWorkspace.H[5] = 0.0000000000000000e+00; +acadoWorkspace.H[26] = 0.0000000000000000e+00; +acadoWorkspace.H[27] = 0.0000000000000000e+00; +acadoWorkspace.H[28] = 0.0000000000000000e+00; +acadoWorkspace.H[29] = 0.0000000000000000e+00; +acadoWorkspace.H[30] = 0.0000000000000000e+00; +acadoWorkspace.H[31] = 0.0000000000000000e+00; +acadoWorkspace.H[52] = 0.0000000000000000e+00; +acadoWorkspace.H[53] = 0.0000000000000000e+00; +acadoWorkspace.H[54] = 0.0000000000000000e+00; +acadoWorkspace.H[55] = 0.0000000000000000e+00; acadoWorkspace.H[56] = 0.0000000000000000e+00; acadoWorkspace.H[57] = 0.0000000000000000e+00; -acadoWorkspace.H[58] = 0.0000000000000000e+00; -acadoWorkspace.H[59] = 0.0000000000000000e+00; -acadoWorkspace.H[60] = 0.0000000000000000e+00; -acadoWorkspace.H[61] = 0.0000000000000000e+00; -acadoWorkspace.H[112] = 0.0000000000000000e+00; -acadoWorkspace.H[113] = 0.0000000000000000e+00; -acadoWorkspace.H[114] = 0.0000000000000000e+00; -acadoWorkspace.H[115] = 0.0000000000000000e+00; -acadoWorkspace.H[116] = 0.0000000000000000e+00; -acadoWorkspace.H[117] = 0.0000000000000000e+00; -acadoWorkspace.H[168] = 0.0000000000000000e+00; -acadoWorkspace.H[169] = 0.0000000000000000e+00; -acadoWorkspace.H[170] = 0.0000000000000000e+00; -acadoWorkspace.H[171] = 0.0000000000000000e+00; -acadoWorkspace.H[172] = 0.0000000000000000e+00; -acadoWorkspace.H[173] = 0.0000000000000000e+00; -acadoWorkspace.H[224] = 0.0000000000000000e+00; -acadoWorkspace.H[225] = 0.0000000000000000e+00; -acadoWorkspace.H[226] = 0.0000000000000000e+00; -acadoWorkspace.H[227] = 0.0000000000000000e+00; -acadoWorkspace.H[228] = 0.0000000000000000e+00; -acadoWorkspace.H[229] = 0.0000000000000000e+00; -acadoWorkspace.H[280] = 0.0000000000000000e+00; -acadoWorkspace.H[281] = 0.0000000000000000e+00; -acadoWorkspace.H[282] = 0.0000000000000000e+00; -acadoWorkspace.H[283] = 0.0000000000000000e+00; -acadoWorkspace.H[284] = 0.0000000000000000e+00; -acadoWorkspace.H[285] = 0.0000000000000000e+00; +acadoWorkspace.H[78] = 0.0000000000000000e+00; +acadoWorkspace.H[79] = 0.0000000000000000e+00; +acadoWorkspace.H[80] = 0.0000000000000000e+00; +acadoWorkspace.H[81] = 0.0000000000000000e+00; +acadoWorkspace.H[82] = 0.0000000000000000e+00; +acadoWorkspace.H[83] = 0.0000000000000000e+00; +acadoWorkspace.H[104] = 0.0000000000000000e+00; +acadoWorkspace.H[105] = 0.0000000000000000e+00; +acadoWorkspace.H[106] = 0.0000000000000000e+00; +acadoWorkspace.H[107] = 0.0000000000000000e+00; +acadoWorkspace.H[108] = 0.0000000000000000e+00; +acadoWorkspace.H[109] = 0.0000000000000000e+00; +acadoWorkspace.H[130] = 0.0000000000000000e+00; +acadoWorkspace.H[131] = 0.0000000000000000e+00; +acadoWorkspace.H[132] = 0.0000000000000000e+00; +acadoWorkspace.H[133] = 0.0000000000000000e+00; +acadoWorkspace.H[134] = 0.0000000000000000e+00; +acadoWorkspace.H[135] = 0.0000000000000000e+00; } void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 ) @@ -658,36 +658,36 @@ acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[6]*Gx2[8] + Gx1[12]*Gx2[14] + Gx1[1 acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[6]*Gx2[9] + Gx1[12]*Gx2[15] + Gx1[18]*Gx2[21] + Gx1[24]*Gx2[27] + Gx1[30]*Gx2[33]; acadoWorkspace.H[4] += + Gx1[0]*Gx2[4] + Gx1[6]*Gx2[10] + Gx1[12]*Gx2[16] + Gx1[18]*Gx2[22] + Gx1[24]*Gx2[28] + Gx1[30]*Gx2[34]; acadoWorkspace.H[5] += + Gx1[0]*Gx2[5] + Gx1[6]*Gx2[11] + Gx1[12]*Gx2[17] + Gx1[18]*Gx2[23] + Gx1[24]*Gx2[29] + Gx1[30]*Gx2[35]; -acadoWorkspace.H[56] += + Gx1[1]*Gx2[0] + Gx1[7]*Gx2[6] + Gx1[13]*Gx2[12] + Gx1[19]*Gx2[18] + Gx1[25]*Gx2[24] + Gx1[31]*Gx2[30]; -acadoWorkspace.H[57] += + Gx1[1]*Gx2[1] + Gx1[7]*Gx2[7] + Gx1[13]*Gx2[13] + Gx1[19]*Gx2[19] + Gx1[25]*Gx2[25] + Gx1[31]*Gx2[31]; -acadoWorkspace.H[58] += + Gx1[1]*Gx2[2] + Gx1[7]*Gx2[8] + Gx1[13]*Gx2[14] + Gx1[19]*Gx2[20] + Gx1[25]*Gx2[26] + Gx1[31]*Gx2[32]; -acadoWorkspace.H[59] += + Gx1[1]*Gx2[3] + Gx1[7]*Gx2[9] + Gx1[13]*Gx2[15] + Gx1[19]*Gx2[21] + Gx1[25]*Gx2[27] + Gx1[31]*Gx2[33]; -acadoWorkspace.H[60] += + Gx1[1]*Gx2[4] + Gx1[7]*Gx2[10] + Gx1[13]*Gx2[16] + Gx1[19]*Gx2[22] + Gx1[25]*Gx2[28] + Gx1[31]*Gx2[34]; -acadoWorkspace.H[61] += + Gx1[1]*Gx2[5] + Gx1[7]*Gx2[11] + Gx1[13]*Gx2[17] + Gx1[19]*Gx2[23] + Gx1[25]*Gx2[29] + Gx1[31]*Gx2[35]; -acadoWorkspace.H[112] += + Gx1[2]*Gx2[0] + Gx1[8]*Gx2[6] + Gx1[14]*Gx2[12] + Gx1[20]*Gx2[18] + Gx1[26]*Gx2[24] + Gx1[32]*Gx2[30]; -acadoWorkspace.H[113] += + Gx1[2]*Gx2[1] + Gx1[8]*Gx2[7] + Gx1[14]*Gx2[13] + Gx1[20]*Gx2[19] + Gx1[26]*Gx2[25] + Gx1[32]*Gx2[31]; -acadoWorkspace.H[114] += + Gx1[2]*Gx2[2] + Gx1[8]*Gx2[8] + Gx1[14]*Gx2[14] + Gx1[20]*Gx2[20] + Gx1[26]*Gx2[26] + Gx1[32]*Gx2[32]; -acadoWorkspace.H[115] += + Gx1[2]*Gx2[3] + Gx1[8]*Gx2[9] + Gx1[14]*Gx2[15] + Gx1[20]*Gx2[21] + Gx1[26]*Gx2[27] + Gx1[32]*Gx2[33]; -acadoWorkspace.H[116] += + Gx1[2]*Gx2[4] + Gx1[8]*Gx2[10] + Gx1[14]*Gx2[16] + Gx1[20]*Gx2[22] + Gx1[26]*Gx2[28] + Gx1[32]*Gx2[34]; -acadoWorkspace.H[117] += + Gx1[2]*Gx2[5] + Gx1[8]*Gx2[11] + Gx1[14]*Gx2[17] + Gx1[20]*Gx2[23] + Gx1[26]*Gx2[29] + Gx1[32]*Gx2[35]; -acadoWorkspace.H[168] += + Gx1[3]*Gx2[0] + Gx1[9]*Gx2[6] + Gx1[15]*Gx2[12] + Gx1[21]*Gx2[18] + Gx1[27]*Gx2[24] + Gx1[33]*Gx2[30]; -acadoWorkspace.H[169] += + Gx1[3]*Gx2[1] + Gx1[9]*Gx2[7] + Gx1[15]*Gx2[13] + Gx1[21]*Gx2[19] + Gx1[27]*Gx2[25] + Gx1[33]*Gx2[31]; -acadoWorkspace.H[170] += + Gx1[3]*Gx2[2] + Gx1[9]*Gx2[8] + Gx1[15]*Gx2[14] + Gx1[21]*Gx2[20] + Gx1[27]*Gx2[26] + Gx1[33]*Gx2[32]; -acadoWorkspace.H[171] += + Gx1[3]*Gx2[3] + Gx1[9]*Gx2[9] + Gx1[15]*Gx2[15] + Gx1[21]*Gx2[21] + Gx1[27]*Gx2[27] + Gx1[33]*Gx2[33]; -acadoWorkspace.H[172] += + Gx1[3]*Gx2[4] + Gx1[9]*Gx2[10] + Gx1[15]*Gx2[16] + Gx1[21]*Gx2[22] + Gx1[27]*Gx2[28] + Gx1[33]*Gx2[34]; -acadoWorkspace.H[173] += + Gx1[3]*Gx2[5] + Gx1[9]*Gx2[11] + Gx1[15]*Gx2[17] + Gx1[21]*Gx2[23] + Gx1[27]*Gx2[29] + Gx1[33]*Gx2[35]; -acadoWorkspace.H[224] += + Gx1[4]*Gx2[0] + Gx1[10]*Gx2[6] + Gx1[16]*Gx2[12] + Gx1[22]*Gx2[18] + Gx1[28]*Gx2[24] + Gx1[34]*Gx2[30]; -acadoWorkspace.H[225] += + Gx1[4]*Gx2[1] + Gx1[10]*Gx2[7] + Gx1[16]*Gx2[13] + Gx1[22]*Gx2[19] + Gx1[28]*Gx2[25] + Gx1[34]*Gx2[31]; -acadoWorkspace.H[226] += + Gx1[4]*Gx2[2] + Gx1[10]*Gx2[8] + Gx1[16]*Gx2[14] + Gx1[22]*Gx2[20] + Gx1[28]*Gx2[26] + Gx1[34]*Gx2[32]; -acadoWorkspace.H[227] += + Gx1[4]*Gx2[3] + Gx1[10]*Gx2[9] + Gx1[16]*Gx2[15] + Gx1[22]*Gx2[21] + Gx1[28]*Gx2[27] + Gx1[34]*Gx2[33]; -acadoWorkspace.H[228] += + Gx1[4]*Gx2[4] + Gx1[10]*Gx2[10] + Gx1[16]*Gx2[16] + Gx1[22]*Gx2[22] + Gx1[28]*Gx2[28] + Gx1[34]*Gx2[34]; -acadoWorkspace.H[229] += + Gx1[4]*Gx2[5] + Gx1[10]*Gx2[11] + Gx1[16]*Gx2[17] + Gx1[22]*Gx2[23] + Gx1[28]*Gx2[29] + Gx1[34]*Gx2[35]; -acadoWorkspace.H[280] += + Gx1[5]*Gx2[0] + Gx1[11]*Gx2[6] + Gx1[17]*Gx2[12] + Gx1[23]*Gx2[18] + Gx1[29]*Gx2[24] + Gx1[35]*Gx2[30]; -acadoWorkspace.H[281] += + Gx1[5]*Gx2[1] + Gx1[11]*Gx2[7] + Gx1[17]*Gx2[13] + Gx1[23]*Gx2[19] + Gx1[29]*Gx2[25] + Gx1[35]*Gx2[31]; -acadoWorkspace.H[282] += + Gx1[5]*Gx2[2] + Gx1[11]*Gx2[8] + Gx1[17]*Gx2[14] + Gx1[23]*Gx2[20] + Gx1[29]*Gx2[26] + Gx1[35]*Gx2[32]; -acadoWorkspace.H[283] += + Gx1[5]*Gx2[3] + Gx1[11]*Gx2[9] + Gx1[17]*Gx2[15] + Gx1[23]*Gx2[21] + Gx1[29]*Gx2[27] + Gx1[35]*Gx2[33]; -acadoWorkspace.H[284] += + Gx1[5]*Gx2[4] + Gx1[11]*Gx2[10] + Gx1[17]*Gx2[16] + Gx1[23]*Gx2[22] + Gx1[29]*Gx2[28] + Gx1[35]*Gx2[34]; -acadoWorkspace.H[285] += + Gx1[5]*Gx2[5] + Gx1[11]*Gx2[11] + Gx1[17]*Gx2[17] + Gx1[23]*Gx2[23] + Gx1[29]*Gx2[29] + Gx1[35]*Gx2[35]; +acadoWorkspace.H[26] += + Gx1[1]*Gx2[0] + Gx1[7]*Gx2[6] + Gx1[13]*Gx2[12] + Gx1[19]*Gx2[18] + Gx1[25]*Gx2[24] + Gx1[31]*Gx2[30]; +acadoWorkspace.H[27] += + Gx1[1]*Gx2[1] + Gx1[7]*Gx2[7] + Gx1[13]*Gx2[13] + Gx1[19]*Gx2[19] + Gx1[25]*Gx2[25] + Gx1[31]*Gx2[31]; +acadoWorkspace.H[28] += + Gx1[1]*Gx2[2] + Gx1[7]*Gx2[8] + Gx1[13]*Gx2[14] + Gx1[19]*Gx2[20] + Gx1[25]*Gx2[26] + Gx1[31]*Gx2[32]; +acadoWorkspace.H[29] += + Gx1[1]*Gx2[3] + Gx1[7]*Gx2[9] + Gx1[13]*Gx2[15] + Gx1[19]*Gx2[21] + Gx1[25]*Gx2[27] + Gx1[31]*Gx2[33]; +acadoWorkspace.H[30] += + Gx1[1]*Gx2[4] + Gx1[7]*Gx2[10] + Gx1[13]*Gx2[16] + Gx1[19]*Gx2[22] + Gx1[25]*Gx2[28] + Gx1[31]*Gx2[34]; +acadoWorkspace.H[31] += + Gx1[1]*Gx2[5] + Gx1[7]*Gx2[11] + Gx1[13]*Gx2[17] + Gx1[19]*Gx2[23] + Gx1[25]*Gx2[29] + Gx1[31]*Gx2[35]; +acadoWorkspace.H[52] += + Gx1[2]*Gx2[0] + Gx1[8]*Gx2[6] + Gx1[14]*Gx2[12] + Gx1[20]*Gx2[18] + Gx1[26]*Gx2[24] + Gx1[32]*Gx2[30]; +acadoWorkspace.H[53] += + Gx1[2]*Gx2[1] + Gx1[8]*Gx2[7] + Gx1[14]*Gx2[13] + Gx1[20]*Gx2[19] + Gx1[26]*Gx2[25] + Gx1[32]*Gx2[31]; +acadoWorkspace.H[54] += + Gx1[2]*Gx2[2] + Gx1[8]*Gx2[8] + Gx1[14]*Gx2[14] + Gx1[20]*Gx2[20] + Gx1[26]*Gx2[26] + Gx1[32]*Gx2[32]; +acadoWorkspace.H[55] += + Gx1[2]*Gx2[3] + Gx1[8]*Gx2[9] + Gx1[14]*Gx2[15] + Gx1[20]*Gx2[21] + Gx1[26]*Gx2[27] + Gx1[32]*Gx2[33]; +acadoWorkspace.H[56] += + Gx1[2]*Gx2[4] + Gx1[8]*Gx2[10] + Gx1[14]*Gx2[16] + Gx1[20]*Gx2[22] + Gx1[26]*Gx2[28] + Gx1[32]*Gx2[34]; +acadoWorkspace.H[57] += + Gx1[2]*Gx2[5] + Gx1[8]*Gx2[11] + Gx1[14]*Gx2[17] + Gx1[20]*Gx2[23] + Gx1[26]*Gx2[29] + Gx1[32]*Gx2[35]; +acadoWorkspace.H[78] += + Gx1[3]*Gx2[0] + Gx1[9]*Gx2[6] + Gx1[15]*Gx2[12] + Gx1[21]*Gx2[18] + Gx1[27]*Gx2[24] + Gx1[33]*Gx2[30]; +acadoWorkspace.H[79] += + Gx1[3]*Gx2[1] + Gx1[9]*Gx2[7] + Gx1[15]*Gx2[13] + Gx1[21]*Gx2[19] + Gx1[27]*Gx2[25] + Gx1[33]*Gx2[31]; +acadoWorkspace.H[80] += + Gx1[3]*Gx2[2] + Gx1[9]*Gx2[8] + Gx1[15]*Gx2[14] + Gx1[21]*Gx2[20] + Gx1[27]*Gx2[26] + Gx1[33]*Gx2[32]; +acadoWorkspace.H[81] += + Gx1[3]*Gx2[3] + Gx1[9]*Gx2[9] + Gx1[15]*Gx2[15] + Gx1[21]*Gx2[21] + Gx1[27]*Gx2[27] + Gx1[33]*Gx2[33]; +acadoWorkspace.H[82] += + Gx1[3]*Gx2[4] + Gx1[9]*Gx2[10] + Gx1[15]*Gx2[16] + Gx1[21]*Gx2[22] + Gx1[27]*Gx2[28] + Gx1[33]*Gx2[34]; +acadoWorkspace.H[83] += + Gx1[3]*Gx2[5] + Gx1[9]*Gx2[11] + Gx1[15]*Gx2[17] + Gx1[21]*Gx2[23] + Gx1[27]*Gx2[29] + Gx1[33]*Gx2[35]; +acadoWorkspace.H[104] += + Gx1[4]*Gx2[0] + Gx1[10]*Gx2[6] + Gx1[16]*Gx2[12] + Gx1[22]*Gx2[18] + Gx1[28]*Gx2[24] + Gx1[34]*Gx2[30]; +acadoWorkspace.H[105] += + Gx1[4]*Gx2[1] + Gx1[10]*Gx2[7] + Gx1[16]*Gx2[13] + Gx1[22]*Gx2[19] + Gx1[28]*Gx2[25] + Gx1[34]*Gx2[31]; +acadoWorkspace.H[106] += + Gx1[4]*Gx2[2] + Gx1[10]*Gx2[8] + Gx1[16]*Gx2[14] + Gx1[22]*Gx2[20] + Gx1[28]*Gx2[26] + Gx1[34]*Gx2[32]; +acadoWorkspace.H[107] += + Gx1[4]*Gx2[3] + Gx1[10]*Gx2[9] + Gx1[16]*Gx2[15] + Gx1[22]*Gx2[21] + Gx1[28]*Gx2[27] + Gx1[34]*Gx2[33]; +acadoWorkspace.H[108] += + Gx1[4]*Gx2[4] + Gx1[10]*Gx2[10] + Gx1[16]*Gx2[16] + Gx1[22]*Gx2[22] + Gx1[28]*Gx2[28] + Gx1[34]*Gx2[34]; +acadoWorkspace.H[109] += + Gx1[4]*Gx2[5] + Gx1[10]*Gx2[11] + Gx1[16]*Gx2[17] + Gx1[22]*Gx2[23] + Gx1[28]*Gx2[29] + Gx1[34]*Gx2[35]; +acadoWorkspace.H[130] += + Gx1[5]*Gx2[0] + Gx1[11]*Gx2[6] + Gx1[17]*Gx2[12] + Gx1[23]*Gx2[18] + Gx1[29]*Gx2[24] + Gx1[35]*Gx2[30]; +acadoWorkspace.H[131] += + Gx1[5]*Gx2[1] + Gx1[11]*Gx2[7] + Gx1[17]*Gx2[13] + Gx1[23]*Gx2[19] + Gx1[29]*Gx2[25] + Gx1[35]*Gx2[31]; +acadoWorkspace.H[132] += + Gx1[5]*Gx2[2] + Gx1[11]*Gx2[8] + Gx1[17]*Gx2[14] + Gx1[23]*Gx2[20] + Gx1[29]*Gx2[26] + Gx1[35]*Gx2[32]; +acadoWorkspace.H[133] += + Gx1[5]*Gx2[3] + Gx1[11]*Gx2[9] + Gx1[17]*Gx2[15] + Gx1[23]*Gx2[21] + Gx1[29]*Gx2[27] + Gx1[35]*Gx2[33]; +acadoWorkspace.H[134] += + Gx1[5]*Gx2[4] + Gx1[11]*Gx2[10] + Gx1[17]*Gx2[16] + Gx1[23]*Gx2[22] + Gx1[29]*Gx2[28] + Gx1[35]*Gx2[34]; +acadoWorkspace.H[135] += + Gx1[5]*Gx2[5] + Gx1[11]*Gx2[11] + Gx1[17]*Gx2[17] + Gx1[23]*Gx2[23] + Gx1[29]*Gx2[29] + Gx1[35]*Gx2[35]; } void acado_macCTSlx( real_t* const C0, real_t* const g0 ) @@ -719,24 +719,332 @@ int lRun2; int lRun3; int lRun4; int lRun5; -/** Row vector of size: 50 */ -static const int xBoundIndices[ 50 ] = -{ 7, 13, 19, 25, 31, 37, 43, 49, 55, 61, 67, 73, 79, 85, 91, 97, 103, 109, 115, 121, 127, 133, 139, 145, 151, 157, 163, 169, 175, 181, 187, 193, 199, 205, 211, 217, 223, 229, 235, 241, 247, 253, 259, 265, 271, 277, 283, 289, 295, 301 }; +/** Row vector of size: 20 */ +static const int xBoundIndices[ 20 ] = +{ 7, 13, 19, 25, 31, 37, 43, 49, 55, 61, 67, 73, 79, 85, 91, 97, 103, 109, 115, 121 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); -for (lRun1 = 1; lRun1 < 50; ++lRun1) -{ -acado_moveGxT( &(acadoWorkspace.evGx[ lRun1 * 36 ]), acadoWorkspace.T ); -acado_multGxd( &(acadoWorkspace.d[ lRun1 * 6-6 ]), &(acadoWorkspace.evGx[ lRun1 * 36 ]), &(acadoWorkspace.d[ lRun1 * 6 ]) ); -acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ lRun1 * 36-36 ]), &(acadoWorkspace.evGx[ lRun1 * 36 ]) ); -for (lRun2 = 0; lRun2 < lRun1; ++lRun2) -{ -lRun4 = (((lRun1) * (lRun1-1)) / (2)) + (lRun2); -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ lRun4 * 6 ]), &(acadoWorkspace.E[ lRun3 * 6 ]) ); -} -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_moveGuE( &(acadoWorkspace.evGu[ lRun1 * 6 ]), &(acadoWorkspace.E[ lRun3 * 6 ]) ); -} +acado_moveGxT( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.T ); +acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.d[ 6 ]) ); +acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 36 ]) ); + +acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 6 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 6 ]), &(acadoWorkspace.E[ 12 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.d[ 12 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.evGx[ 72 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.E[ 18 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 30 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.d[ 18 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.evGx[ 108 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.E[ 36 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 42 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.E[ 48 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 18 ]), &(acadoWorkspace.E[ 54 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 24 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.evGx[ 144 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 60 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.E[ 66 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 72 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.E[ 78 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 84 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 180 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.d[ 30 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 180 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 90 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.E[ 96 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 102 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.E[ 108 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 114 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 30 ]), &(acadoWorkspace.E[ 120 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 216 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.d[ 36 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.evGx[ 216 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.E[ 126 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 132 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.E[ 138 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 144 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.E[ 150 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 156 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 162 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 252 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.d[ 42 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.evGx[ 252 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.E[ 168 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 174 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.E[ 180 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 186 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.E[ 192 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 198 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.E[ 204 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 42 ]), &(acadoWorkspace.E[ 210 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.d[ 48 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.evGx[ 288 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 216 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.E[ 222 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 228 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.E[ 234 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 240 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.E[ 246 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 252 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.E[ 258 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 264 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 324 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.d[ 54 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.evGx[ 324 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 270 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.E[ 276 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 282 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.E[ 288 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 294 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.E[ 300 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 306 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.E[ 312 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 318 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 54 ]), &(acadoWorkspace.E[ 324 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 360 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.d[ 60 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.evGx[ 360 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.E[ 330 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 336 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.E[ 342 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 348 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.E[ 354 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 360 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.E[ 366 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 372 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.E[ 378 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 384 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 390 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 396 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.d[ 66 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.evGx[ 396 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.E[ 396 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 402 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.E[ 408 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 414 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.E[ 420 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 426 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.E[ 432 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 438 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.E[ 444 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 450 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.E[ 456 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 66 ]), &(acadoWorkspace.E[ 462 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 66 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.d[ 72 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.evGx[ 432 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 468 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.E[ 474 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 480 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.E[ 486 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 492 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.E[ 498 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 504 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.E[ 510 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 516 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.E[ 522 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 528 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.E[ 534 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 72 ]), &(acadoWorkspace.E[ 540 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 468 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.d[ 78 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.evGx[ 468 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 546 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.E[ 552 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 558 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.E[ 564 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 570 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.E[ 576 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 582 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.E[ 588 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 594 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.E[ 600 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 606 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.E[ 612 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 618 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 78 ]), &(acadoWorkspace.E[ 624 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 504 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 78 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.d[ 84 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.evGx[ 504 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.E[ 630 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 636 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.E[ 642 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 648 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.E[ 654 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.E[ 660 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.E[ 666 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.E[ 672 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.E[ 678 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.E[ 684 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.E[ 690 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.E[ 696 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.E[ 702 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.E[ 708 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 84 ]), &(acadoWorkspace.E[ 714 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 540 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.d[ 90 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.evGx[ 540 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.E[ 720 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.E[ 726 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.E[ 732 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.E[ 738 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.E[ 744 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.E[ 750 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.E[ 756 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.E[ 762 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.E[ 768 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.E[ 774 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.E[ 780 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.E[ 786 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.E[ 792 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.E[ 798 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.E[ 804 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 90 ]), &(acadoWorkspace.E[ 810 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 90 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.d[ 96 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.evGx[ 576 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.E[ 816 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.E[ 822 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.E[ 828 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.E[ 834 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.E[ 840 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.E[ 846 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.E[ 852 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.E[ 858 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.E[ 864 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.E[ 870 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.E[ 876 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.E[ 882 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.E[ 888 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.E[ 894 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.E[ 900 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.E[ 906 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 96 ]), &(acadoWorkspace.E[ 912 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.d[ 102 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.evGx[ 612 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.E[ 918 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.E[ 924 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.E[ 930 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.E[ 936 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.E[ 942 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.E[ 948 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.E[ 954 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.E[ 960 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.E[ 966 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.E[ 972 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.E[ 978 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.E[ 984 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.E[ 990 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.E[ 996 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.E[ 1002 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.E[ 1008 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.E[ 1014 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 102 ]), &(acadoWorkspace.E[ 1020 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 102 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.d[ 108 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.evGx[ 648 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.E[ 1026 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.E[ 1032 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.E[ 1038 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.E[ 1044 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.E[ 1050 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.E[ 1056 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.E[ 1062 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.E[ 1068 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.E[ 1074 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.E[ 1080 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.E[ 1086 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.E[ 1092 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.E[ 1098 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.E[ 1104 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.E[ 1110 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.E[ 1116 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.E[ 1122 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.E[ 1128 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 108 ]), &(acadoWorkspace.E[ 1134 ]) ); + +acado_moveGxT( &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.T ); +acado_multGxd( &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.d[ 114 ]) ); +acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.evGx[ 684 ]) ); + +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.E[ 1140 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.E[ 1146 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.E[ 1152 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.E[ 1158 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.E[ 1164 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.E[ 1170 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.E[ 1176 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.E[ 1182 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.E[ 1188 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.E[ 1194 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.E[ 1200 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.E[ 1206 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.E[ 1212 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.E[ 1218 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.E[ 1224 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.E[ 1230 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.E[ 1236 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.E[ 1242 ]) ); +acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.E[ 1248 ]) ); + +acado_moveGuE( &(acadoWorkspace.evGu[ 114 ]), &(acadoWorkspace.E[ 1254 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.evGx, acadoWorkspace.QGx ); acado_multGxGx( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) ); @@ -757,52 +1065,218 @@ acado_multGxGx( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.evGx[ 540 ]), &(ac acado_multGxGx( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.QGx[ 612 ]) ); acado_multGxGx( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.QGx[ 648 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.QGx[ 684 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 756 ]), &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 792 ]), &(acadoWorkspace.evGx[ 756 ]), &(acadoWorkspace.QGx[ 756 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 828 ]), &(acadoWorkspace.evGx[ 792 ]), &(acadoWorkspace.QGx[ 792 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 864 ]), &(acadoWorkspace.evGx[ 828 ]), &(acadoWorkspace.QGx[ 828 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 900 ]), &(acadoWorkspace.evGx[ 864 ]), &(acadoWorkspace.QGx[ 864 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 936 ]), &(acadoWorkspace.evGx[ 900 ]), &(acadoWorkspace.QGx[ 900 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 972 ]), &(acadoWorkspace.evGx[ 936 ]), &(acadoWorkspace.QGx[ 936 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1008 ]), &(acadoWorkspace.evGx[ 972 ]), &(acadoWorkspace.QGx[ 972 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1044 ]), &(acadoWorkspace.evGx[ 1008 ]), &(acadoWorkspace.QGx[ 1008 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1080 ]), &(acadoWorkspace.evGx[ 1044 ]), &(acadoWorkspace.QGx[ 1044 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1116 ]), &(acadoWorkspace.evGx[ 1080 ]), &(acadoWorkspace.QGx[ 1080 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1152 ]), &(acadoWorkspace.evGx[ 1116 ]), &(acadoWorkspace.QGx[ 1116 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1188 ]), &(acadoWorkspace.evGx[ 1152 ]), &(acadoWorkspace.QGx[ 1152 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1224 ]), &(acadoWorkspace.evGx[ 1188 ]), &(acadoWorkspace.QGx[ 1188 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1260 ]), &(acadoWorkspace.evGx[ 1224 ]), &(acadoWorkspace.QGx[ 1224 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1296 ]), &(acadoWorkspace.evGx[ 1260 ]), &(acadoWorkspace.QGx[ 1260 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1332 ]), &(acadoWorkspace.evGx[ 1296 ]), &(acadoWorkspace.QGx[ 1296 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1368 ]), &(acadoWorkspace.evGx[ 1332 ]), &(acadoWorkspace.QGx[ 1332 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1404 ]), &(acadoWorkspace.evGx[ 1368 ]), &(acadoWorkspace.QGx[ 1368 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1440 ]), &(acadoWorkspace.evGx[ 1404 ]), &(acadoWorkspace.QGx[ 1404 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1476 ]), &(acadoWorkspace.evGx[ 1440 ]), &(acadoWorkspace.QGx[ 1440 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1512 ]), &(acadoWorkspace.evGx[ 1476 ]), &(acadoWorkspace.QGx[ 1476 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1548 ]), &(acadoWorkspace.evGx[ 1512 ]), &(acadoWorkspace.QGx[ 1512 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1584 ]), &(acadoWorkspace.evGx[ 1548 ]), &(acadoWorkspace.QGx[ 1548 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1620 ]), &(acadoWorkspace.evGx[ 1584 ]), &(acadoWorkspace.QGx[ 1584 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1656 ]), &(acadoWorkspace.evGx[ 1620 ]), &(acadoWorkspace.QGx[ 1620 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1692 ]), &(acadoWorkspace.evGx[ 1656 ]), &(acadoWorkspace.QGx[ 1656 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1728 ]), &(acadoWorkspace.evGx[ 1692 ]), &(acadoWorkspace.QGx[ 1692 ]) ); -acado_multGxGx( &(acadoWorkspace.Q1[ 1764 ]), &(acadoWorkspace.evGx[ 1728 ]), &(acadoWorkspace.QGx[ 1728 ]) ); -acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 1764 ]), &(acadoWorkspace.QGx[ 1764 ]) ); - -for (lRun1 = 0; lRun1 < 49; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( &(acadoWorkspace.Q1[ lRun1 * 36 + 36 ]), &(acadoWorkspace.E[ lRun3 * 6 ]), &(acadoWorkspace.QE[ lRun3 * 6 ]) ); -} -} +acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.QGx[ 684 ]) ); -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ lRun3 * 6 ]), &(acadoWorkspace.QE[ lRun3 * 6 ]) ); -} +acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.E, acadoWorkspace.QE ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 630 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 642 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 726 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 738 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 822 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 834 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 918 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 924 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 930 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 936 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1026 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1032 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1038 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1044 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1140 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1146 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1152 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1158 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1164 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1170 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1176 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1182 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1188 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1194 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1200 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1206 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1212 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1218 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1224 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1230 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1236 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1242 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1248 ]) ); +acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QE[ 1254 ]) ); acado_zeroBlockH00( ); acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx ); @@ -825,84 +1299,2631 @@ acado_multCTQC( &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.QGx[ 612 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.QGx[ 648 ]) ); acado_multCTQC( &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.QGx[ 684 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 720 ]), &(acadoWorkspace.QGx[ 720 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 756 ]), &(acadoWorkspace.QGx[ 756 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 792 ]), &(acadoWorkspace.QGx[ 792 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 828 ]), &(acadoWorkspace.QGx[ 828 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 864 ]), &(acadoWorkspace.QGx[ 864 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 900 ]), &(acadoWorkspace.QGx[ 900 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 936 ]), &(acadoWorkspace.QGx[ 936 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 972 ]), &(acadoWorkspace.QGx[ 972 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1008 ]), &(acadoWorkspace.QGx[ 1008 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1044 ]), &(acadoWorkspace.QGx[ 1044 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1080 ]), &(acadoWorkspace.QGx[ 1080 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1116 ]), &(acadoWorkspace.QGx[ 1116 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1152 ]), &(acadoWorkspace.QGx[ 1152 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1188 ]), &(acadoWorkspace.QGx[ 1188 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1224 ]), &(acadoWorkspace.QGx[ 1224 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1260 ]), &(acadoWorkspace.QGx[ 1260 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1296 ]), &(acadoWorkspace.QGx[ 1296 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1332 ]), &(acadoWorkspace.QGx[ 1332 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1368 ]), &(acadoWorkspace.QGx[ 1368 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1404 ]), &(acadoWorkspace.QGx[ 1404 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1440 ]), &(acadoWorkspace.QGx[ 1440 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1476 ]), &(acadoWorkspace.QGx[ 1476 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1512 ]), &(acadoWorkspace.QGx[ 1512 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1548 ]), &(acadoWorkspace.QGx[ 1548 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1584 ]), &(acadoWorkspace.QGx[ 1584 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1620 ]), &(acadoWorkspace.QGx[ 1620 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1656 ]), &(acadoWorkspace.QGx[ 1656 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1692 ]), &(acadoWorkspace.QGx[ 1692 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1728 ]), &(acadoWorkspace.QGx[ 1728 ]) ); -acado_multCTQC( &(acadoWorkspace.evGx[ 1764 ]), &(acadoWorkspace.QGx[ 1764 ]) ); - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -acado_zeroBlockH10( &(acadoWorkspace.H10[ lRun1 * 6 ]) ); -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_multQETGx( &(acadoWorkspace.QE[ lRun3 * 6 ]), &(acadoWorkspace.evGx[ lRun2 * 36 ]), &(acadoWorkspace.H10[ lRun1 * 6 ]) ); -} -} -for (lRun1 = 0;lRun1 < 6; ++lRun1) -for (lRun2 = 0;lRun2 < 50; ++lRun2) -acadoWorkspace.H[(lRun1 * 56) + (lRun2 + 6)] = acadoWorkspace.H10[(lRun2 * 6) + (lRun1)]; +acado_zeroBlockH10( acadoWorkspace.H10 ); +acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.evGx[ 180 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.evGx[ 216 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 252 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.evGx[ 324 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.evGx[ 360 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 396 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.evGx[ 468 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 630 ]), &(acadoWorkspace.evGx[ 504 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.evGx[ 540 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 918 ]), &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 1026 ]), &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.H10 ); +acado_multQETGx( &(acadoWorkspace.QE[ 1140 ]), &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.H10 ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 726 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 822 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 924 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1032 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1146 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 6 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 642 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 930 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1038 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1152 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 12 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 738 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 834 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 936 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1044 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1158 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 18 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 654 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 840 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 942 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1050 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1164 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 24 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 750 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 846 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 948 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1056 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1170 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 30 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 666 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 852 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 954 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1062 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1176 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 36 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 762 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 858 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 960 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1068 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1182 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 42 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 678 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 864 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 966 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1074 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1188 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 48 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 774 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 870 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 972 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1080 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1194 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 54 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 690 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 876 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 978 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1086 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1200 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 60 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 786 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 882 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 984 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1092 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1206 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 66 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 702 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 888 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 990 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1098 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1212 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 72 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 798 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 894 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 996 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1104 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1218 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 78 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 714 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 900 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1002 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1110 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1224 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 84 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 90 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 810 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 90 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 906 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 90 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1008 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 90 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1116 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 90 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1230 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 90 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 96 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 912 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 96 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1014 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 96 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1122 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 96 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1236 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 96 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 102 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1020 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 102 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1128 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 102 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1242 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 102 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 108 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1134 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 108 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1248 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 108 ]) ); +acado_zeroBlockH10( &(acadoWorkspace.H10[ 114 ]) ); +acado_multQETGx( &(acadoWorkspace.QE[ 1254 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 114 ]) ); -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -acado_setBlockH11_R1( lRun1, lRun1, &(acadoWorkspace.R1[ lRun1 ]) ); -lRun2 = lRun1; -for (lRun3 = lRun1; lRun3 < 50; ++lRun3) -{ -lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); -lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); -acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 6 ]), &(acadoWorkspace.QE[ lRun5 * 6 ]) ); -} -for (lRun2 = lRun1 + 1; lRun2 < 50; ++lRun2) -{ -acado_zeroBlockH11( lRun1, lRun2 ); -for (lRun3 = lRun2; lRun3 < 50; ++lRun3) -{ -lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1); -lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2); -acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 6 ]), &(acadoWorkspace.QE[ lRun5 * 6 ]) ); -} -} -} +acadoWorkspace.H[6] = acadoWorkspace.H10[0]; +acadoWorkspace.H[7] = acadoWorkspace.H10[6]; +acadoWorkspace.H[8] = acadoWorkspace.H10[12]; +acadoWorkspace.H[9] = acadoWorkspace.H10[18]; +acadoWorkspace.H[10] = acadoWorkspace.H10[24]; +acadoWorkspace.H[11] = acadoWorkspace.H10[30]; +acadoWorkspace.H[12] = acadoWorkspace.H10[36]; +acadoWorkspace.H[13] = acadoWorkspace.H10[42]; +acadoWorkspace.H[14] = acadoWorkspace.H10[48]; +acadoWorkspace.H[15] = acadoWorkspace.H10[54]; +acadoWorkspace.H[16] = acadoWorkspace.H10[60]; +acadoWorkspace.H[17] = acadoWorkspace.H10[66]; +acadoWorkspace.H[18] = acadoWorkspace.H10[72]; +acadoWorkspace.H[19] = acadoWorkspace.H10[78]; +acadoWorkspace.H[20] = acadoWorkspace.H10[84]; +acadoWorkspace.H[21] = acadoWorkspace.H10[90]; +acadoWorkspace.H[22] = acadoWorkspace.H10[96]; +acadoWorkspace.H[23] = acadoWorkspace.H10[102]; +acadoWorkspace.H[24] = acadoWorkspace.H10[108]; +acadoWorkspace.H[25] = acadoWorkspace.H10[114]; +acadoWorkspace.H[32] = acadoWorkspace.H10[1]; +acadoWorkspace.H[33] = acadoWorkspace.H10[7]; +acadoWorkspace.H[34] = acadoWorkspace.H10[13]; +acadoWorkspace.H[35] = acadoWorkspace.H10[19]; +acadoWorkspace.H[36] = acadoWorkspace.H10[25]; +acadoWorkspace.H[37] = acadoWorkspace.H10[31]; +acadoWorkspace.H[38] = acadoWorkspace.H10[37]; +acadoWorkspace.H[39] = acadoWorkspace.H10[43]; +acadoWorkspace.H[40] = acadoWorkspace.H10[49]; +acadoWorkspace.H[41] = acadoWorkspace.H10[55]; +acadoWorkspace.H[42] = acadoWorkspace.H10[61]; +acadoWorkspace.H[43] = acadoWorkspace.H10[67]; +acadoWorkspace.H[44] = acadoWorkspace.H10[73]; +acadoWorkspace.H[45] = acadoWorkspace.H10[79]; +acadoWorkspace.H[46] = acadoWorkspace.H10[85]; +acadoWorkspace.H[47] = acadoWorkspace.H10[91]; +acadoWorkspace.H[48] = acadoWorkspace.H10[97]; +acadoWorkspace.H[49] = acadoWorkspace.H10[103]; +acadoWorkspace.H[50] = acadoWorkspace.H10[109]; +acadoWorkspace.H[51] = acadoWorkspace.H10[115]; +acadoWorkspace.H[58] = acadoWorkspace.H10[2]; +acadoWorkspace.H[59] = acadoWorkspace.H10[8]; +acadoWorkspace.H[60] = acadoWorkspace.H10[14]; +acadoWorkspace.H[61] = acadoWorkspace.H10[20]; +acadoWorkspace.H[62] = acadoWorkspace.H10[26]; +acadoWorkspace.H[63] = acadoWorkspace.H10[32]; +acadoWorkspace.H[64] = acadoWorkspace.H10[38]; +acadoWorkspace.H[65] = acadoWorkspace.H10[44]; +acadoWorkspace.H[66] = acadoWorkspace.H10[50]; +acadoWorkspace.H[67] = acadoWorkspace.H10[56]; +acadoWorkspace.H[68] = acadoWorkspace.H10[62]; +acadoWorkspace.H[69] = acadoWorkspace.H10[68]; +acadoWorkspace.H[70] = acadoWorkspace.H10[74]; +acadoWorkspace.H[71] = acadoWorkspace.H10[80]; +acadoWorkspace.H[72] = acadoWorkspace.H10[86]; +acadoWorkspace.H[73] = acadoWorkspace.H10[92]; +acadoWorkspace.H[74] = acadoWorkspace.H10[98]; +acadoWorkspace.H[75] = acadoWorkspace.H10[104]; +acadoWorkspace.H[76] = acadoWorkspace.H10[110]; +acadoWorkspace.H[77] = acadoWorkspace.H10[116]; +acadoWorkspace.H[84] = acadoWorkspace.H10[3]; +acadoWorkspace.H[85] = acadoWorkspace.H10[9]; +acadoWorkspace.H[86] = acadoWorkspace.H10[15]; +acadoWorkspace.H[87] = acadoWorkspace.H10[21]; +acadoWorkspace.H[88] = acadoWorkspace.H10[27]; +acadoWorkspace.H[89] = acadoWorkspace.H10[33]; +acadoWorkspace.H[90] = acadoWorkspace.H10[39]; +acadoWorkspace.H[91] = acadoWorkspace.H10[45]; +acadoWorkspace.H[92] = acadoWorkspace.H10[51]; +acadoWorkspace.H[93] = acadoWorkspace.H10[57]; +acadoWorkspace.H[94] = acadoWorkspace.H10[63]; +acadoWorkspace.H[95] = acadoWorkspace.H10[69]; +acadoWorkspace.H[96] = acadoWorkspace.H10[75]; +acadoWorkspace.H[97] = acadoWorkspace.H10[81]; +acadoWorkspace.H[98] = acadoWorkspace.H10[87]; +acadoWorkspace.H[99] = acadoWorkspace.H10[93]; +acadoWorkspace.H[100] = acadoWorkspace.H10[99]; +acadoWorkspace.H[101] = acadoWorkspace.H10[105]; +acadoWorkspace.H[102] = acadoWorkspace.H10[111]; +acadoWorkspace.H[103] = acadoWorkspace.H10[117]; +acadoWorkspace.H[110] = acadoWorkspace.H10[4]; +acadoWorkspace.H[111] = acadoWorkspace.H10[10]; +acadoWorkspace.H[112] = acadoWorkspace.H10[16]; +acadoWorkspace.H[113] = acadoWorkspace.H10[22]; +acadoWorkspace.H[114] = acadoWorkspace.H10[28]; +acadoWorkspace.H[115] = acadoWorkspace.H10[34]; +acadoWorkspace.H[116] = acadoWorkspace.H10[40]; +acadoWorkspace.H[117] = acadoWorkspace.H10[46]; +acadoWorkspace.H[118] = acadoWorkspace.H10[52]; +acadoWorkspace.H[119] = acadoWorkspace.H10[58]; +acadoWorkspace.H[120] = acadoWorkspace.H10[64]; +acadoWorkspace.H[121] = acadoWorkspace.H10[70]; +acadoWorkspace.H[122] = acadoWorkspace.H10[76]; +acadoWorkspace.H[123] = acadoWorkspace.H10[82]; +acadoWorkspace.H[124] = acadoWorkspace.H10[88]; +acadoWorkspace.H[125] = acadoWorkspace.H10[94]; +acadoWorkspace.H[126] = acadoWorkspace.H10[100]; +acadoWorkspace.H[127] = acadoWorkspace.H10[106]; +acadoWorkspace.H[128] = acadoWorkspace.H10[112]; +acadoWorkspace.H[129] = acadoWorkspace.H10[118]; +acadoWorkspace.H[136] = acadoWorkspace.H10[5]; +acadoWorkspace.H[137] = acadoWorkspace.H10[11]; +acadoWorkspace.H[138] = acadoWorkspace.H10[17]; +acadoWorkspace.H[139] = acadoWorkspace.H10[23]; +acadoWorkspace.H[140] = acadoWorkspace.H10[29]; +acadoWorkspace.H[141] = acadoWorkspace.H10[35]; +acadoWorkspace.H[142] = acadoWorkspace.H10[41]; +acadoWorkspace.H[143] = acadoWorkspace.H10[47]; +acadoWorkspace.H[144] = acadoWorkspace.H10[53]; +acadoWorkspace.H[145] = acadoWorkspace.H10[59]; +acadoWorkspace.H[146] = acadoWorkspace.H10[65]; +acadoWorkspace.H[147] = acadoWorkspace.H10[71]; +acadoWorkspace.H[148] = acadoWorkspace.H10[77]; +acadoWorkspace.H[149] = acadoWorkspace.H10[83]; +acadoWorkspace.H[150] = acadoWorkspace.H10[89]; +acadoWorkspace.H[151] = acadoWorkspace.H10[95]; +acadoWorkspace.H[152] = acadoWorkspace.H10[101]; +acadoWorkspace.H[153] = acadoWorkspace.H10[107]; +acadoWorkspace.H[154] = acadoWorkspace.H10[113]; +acadoWorkspace.H[155] = acadoWorkspace.H10[119]; -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1; ++lRun2) -{ -acado_copyHTH( lRun1, lRun2 ); -} -} +acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 ); +acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 630 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 918 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1026 ]) ); +acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1140 ]) ); + +acado_zeroBlockH11( 0, 1 ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 726 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 822 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 924 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1032 ]) ); +acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1146 ]) ); + +acado_zeroBlockH11( 0, 2 ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 642 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 828 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 930 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1038 ]) ); +acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1152 ]) ); + +acado_zeroBlockH11( 0, 3 ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 738 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 834 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 936 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1044 ]) ); +acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1158 ]) ); + +acado_zeroBlockH11( 0, 4 ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1164 ]) ); + +acado_zeroBlockH11( 0, 5 ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 0, 6 ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 0, 7 ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 0, 8 ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 0, 9 ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 0, 10 ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 0, 11 ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 0, 12 ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 0, 13 ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 0, 14 ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 0, 15 ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 0, 16 ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 0, 17 ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 0, 18 ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 0, 19 ); +acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 726 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 822 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 924 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1032 ]) ); +acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1146 ]) ); + +acado_zeroBlockH11( 1, 2 ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 642 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 828 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 930 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1038 ]) ); +acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1152 ]) ); + +acado_zeroBlockH11( 1, 3 ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 738 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 834 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 936 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1044 ]) ); +acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1158 ]) ); + +acado_zeroBlockH11( 1, 4 ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1164 ]) ); + +acado_zeroBlockH11( 1, 5 ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 1, 6 ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 1, 7 ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 1, 8 ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 1, 9 ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 1, 10 ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 1, 11 ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 1, 12 ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 1, 13 ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 1, 14 ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 1, 15 ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 1, 16 ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 1, 17 ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 1, 18 ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 1, 19 ); +acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 642 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 930 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1038 ]) ); +acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1152 ]) ); + +acado_zeroBlockH11( 2, 3 ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 738 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 834 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 936 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1044 ]) ); +acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1158 ]) ); + +acado_zeroBlockH11( 2, 4 ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1164 ]) ); + +acado_zeroBlockH11( 2, 5 ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 2, 6 ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 2, 7 ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 2, 8 ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 2, 9 ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 2, 10 ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 2, 11 ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 2, 12 ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 2, 13 ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 2, 14 ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 2, 15 ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 2, 16 ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 2, 17 ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 2, 18 ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 2, 19 ); +acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 738 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 834 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 936 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1044 ]) ); +acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1158 ]) ); + +acado_zeroBlockH11( 3, 4 ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1164 ]) ); + +acado_zeroBlockH11( 3, 5 ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 3, 6 ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 3, 7 ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 3, 8 ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 3, 9 ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 3, 10 ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 3, 11 ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 3, 12 ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 3, 13 ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 3, 14 ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 3, 15 ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 3, 16 ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 3, 17 ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 3, 18 ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 3, 19 ); +acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 654 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 840 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 942 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1050 ]) ); +acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1164 ]) ); + +acado_zeroBlockH11( 4, 5 ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 4, 6 ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 4, 7 ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 4, 8 ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1188 ]) ); -for (lRun1 = 0;lRun1 < 50; ++lRun1) -for (lRun2 = 0;lRun2 < 6; ++lRun2) -acadoWorkspace.H[(lRun1 * 56 + 336) + (lRun2)] = acadoWorkspace.H10[(lRun1 * 6) + (lRun2)]; +acado_zeroBlockH11( 4, 9 ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 4, 10 ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 4, 11 ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 4, 12 ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 4, 13 ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 4, 14 ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 4, 15 ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 4, 16 ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 4, 17 ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 4, 18 ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 4, 19 ); +acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 750 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 846 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 948 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1056 ]) ); +acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1170 ]) ); + +acado_zeroBlockH11( 5, 6 ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 5, 7 ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 5, 8 ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 5, 9 ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 5, 10 ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 5, 11 ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 5, 12 ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 5, 13 ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 5, 14 ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 5, 15 ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 5, 16 ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 5, 17 ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 5, 18 ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 5, 19 ); +acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 666 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 852 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 954 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1062 ]) ); +acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1176 ]) ); + +acado_zeroBlockH11( 6, 7 ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 6, 8 ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 6, 9 ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 6, 10 ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 6, 11 ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 6, 12 ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 6, 13 ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 6, 14 ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 6, 15 ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 6, 16 ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 6, 17 ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 6, 18 ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 6, 19 ); +acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 762 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 858 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 960 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1068 ]) ); +acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1182 ]) ); + +acado_zeroBlockH11( 7, 8 ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 7, 9 ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 7, 10 ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 7, 11 ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 7, 12 ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 7, 13 ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 7, 14 ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 7, 15 ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 7, 16 ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 7, 17 ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 7, 18 ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 7, 19 ); +acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 678 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 864 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 966 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1074 ]) ); +acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1188 ]) ); + +acado_zeroBlockH11( 8, 9 ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 8, 10 ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 8, 11 ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 8, 12 ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 8, 13 ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 8, 14 ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 8, 15 ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 8, 16 ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 8, 17 ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 8, 18 ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 8, 19 ); +acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 774 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 870 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 972 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1080 ]) ); +acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1194 ]) ); + +acado_zeroBlockH11( 9, 10 ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 9, 11 ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 9, 12 ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 9, 13 ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 9, 14 ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 9, 15 ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 9, 16 ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 9, 17 ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 9, 18 ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 9, 19 ); +acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 690 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 876 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 978 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1086 ]) ); +acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1200 ]) ); + +acado_zeroBlockH11( 10, 11 ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 10, 12 ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 10, 13 ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 10, 14 ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 10, 15 ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 10, 16 ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 10, 17 ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 10, 18 ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 10, 19 ); +acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 786 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 882 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 984 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1092 ]) ); +acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1206 ]) ); + +acado_zeroBlockH11( 11, 12 ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 11, 13 ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 11, 14 ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 11, 15 ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 11, 16 ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 11, 17 ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 11, 18 ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 11, 19 ); +acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 702 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 888 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 990 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1098 ]) ); +acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1212 ]) ); + +acado_zeroBlockH11( 12, 13 ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 12, 14 ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 12, 15 ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 12, 16 ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 12, 17 ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 12, 18 ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 12, 19 ); +acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 798 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 894 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 996 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1104 ]) ); +acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1218 ]) ); + +acado_zeroBlockH11( 13, 14 ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 13, 15 ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 13, 16 ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 13, 17 ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 13, 18 ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 13, 19 ); +acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QE[ 714 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 900 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1002 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1110 ]) ); +acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1224 ]) ); + +acado_zeroBlockH11( 14, 15 ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 14, 16 ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 14, 17 ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 14, 18 ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 14, 19 ); +acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QE[ 810 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 906 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1008 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1116 ]) ); +acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1230 ]) ); + +acado_zeroBlockH11( 15, 16 ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 15, 17 ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 15, 18 ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 15, 19 ); +acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QE[ 912 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1014 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1122 ]) ); +acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1236 ]) ); + +acado_zeroBlockH11( 16, 17 ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 16, 18 ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 16, 19 ); +acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QE[ 1020 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1128 ]) ); +acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1242 ]) ); + +acado_zeroBlockH11( 17, 18 ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 17, 19 ); +acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QE[ 1134 ]) ); +acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1248 ]) ); + +acado_zeroBlockH11( 18, 19 ); +acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1254 ]) ); + +acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) ); +acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QE[ 1254 ]) ); + + +acado_copyHTH( 1, 0 ); +acado_copyHTH( 2, 0 ); +acado_copyHTH( 2, 1 ); +acado_copyHTH( 3, 0 ); +acado_copyHTH( 3, 1 ); +acado_copyHTH( 3, 2 ); +acado_copyHTH( 4, 0 ); +acado_copyHTH( 4, 1 ); +acado_copyHTH( 4, 2 ); +acado_copyHTH( 4, 3 ); +acado_copyHTH( 5, 0 ); +acado_copyHTH( 5, 1 ); +acado_copyHTH( 5, 2 ); +acado_copyHTH( 5, 3 ); +acado_copyHTH( 5, 4 ); +acado_copyHTH( 6, 0 ); +acado_copyHTH( 6, 1 ); +acado_copyHTH( 6, 2 ); +acado_copyHTH( 6, 3 ); +acado_copyHTH( 6, 4 ); +acado_copyHTH( 6, 5 ); +acado_copyHTH( 7, 0 ); +acado_copyHTH( 7, 1 ); +acado_copyHTH( 7, 2 ); +acado_copyHTH( 7, 3 ); +acado_copyHTH( 7, 4 ); +acado_copyHTH( 7, 5 ); +acado_copyHTH( 7, 6 ); +acado_copyHTH( 8, 0 ); +acado_copyHTH( 8, 1 ); +acado_copyHTH( 8, 2 ); +acado_copyHTH( 8, 3 ); +acado_copyHTH( 8, 4 ); +acado_copyHTH( 8, 5 ); +acado_copyHTH( 8, 6 ); +acado_copyHTH( 8, 7 ); +acado_copyHTH( 9, 0 ); +acado_copyHTH( 9, 1 ); +acado_copyHTH( 9, 2 ); +acado_copyHTH( 9, 3 ); +acado_copyHTH( 9, 4 ); +acado_copyHTH( 9, 5 ); +acado_copyHTH( 9, 6 ); +acado_copyHTH( 9, 7 ); +acado_copyHTH( 9, 8 ); +acado_copyHTH( 10, 0 ); +acado_copyHTH( 10, 1 ); +acado_copyHTH( 10, 2 ); +acado_copyHTH( 10, 3 ); +acado_copyHTH( 10, 4 ); +acado_copyHTH( 10, 5 ); +acado_copyHTH( 10, 6 ); +acado_copyHTH( 10, 7 ); +acado_copyHTH( 10, 8 ); +acado_copyHTH( 10, 9 ); +acado_copyHTH( 11, 0 ); +acado_copyHTH( 11, 1 ); +acado_copyHTH( 11, 2 ); +acado_copyHTH( 11, 3 ); +acado_copyHTH( 11, 4 ); +acado_copyHTH( 11, 5 ); +acado_copyHTH( 11, 6 ); +acado_copyHTH( 11, 7 ); +acado_copyHTH( 11, 8 ); +acado_copyHTH( 11, 9 ); +acado_copyHTH( 11, 10 ); +acado_copyHTH( 12, 0 ); +acado_copyHTH( 12, 1 ); +acado_copyHTH( 12, 2 ); +acado_copyHTH( 12, 3 ); +acado_copyHTH( 12, 4 ); +acado_copyHTH( 12, 5 ); +acado_copyHTH( 12, 6 ); +acado_copyHTH( 12, 7 ); +acado_copyHTH( 12, 8 ); +acado_copyHTH( 12, 9 ); +acado_copyHTH( 12, 10 ); +acado_copyHTH( 12, 11 ); +acado_copyHTH( 13, 0 ); +acado_copyHTH( 13, 1 ); +acado_copyHTH( 13, 2 ); +acado_copyHTH( 13, 3 ); +acado_copyHTH( 13, 4 ); +acado_copyHTH( 13, 5 ); +acado_copyHTH( 13, 6 ); +acado_copyHTH( 13, 7 ); +acado_copyHTH( 13, 8 ); +acado_copyHTH( 13, 9 ); +acado_copyHTH( 13, 10 ); +acado_copyHTH( 13, 11 ); +acado_copyHTH( 13, 12 ); +acado_copyHTH( 14, 0 ); +acado_copyHTH( 14, 1 ); +acado_copyHTH( 14, 2 ); +acado_copyHTH( 14, 3 ); +acado_copyHTH( 14, 4 ); +acado_copyHTH( 14, 5 ); +acado_copyHTH( 14, 6 ); +acado_copyHTH( 14, 7 ); +acado_copyHTH( 14, 8 ); +acado_copyHTH( 14, 9 ); +acado_copyHTH( 14, 10 ); +acado_copyHTH( 14, 11 ); +acado_copyHTH( 14, 12 ); +acado_copyHTH( 14, 13 ); +acado_copyHTH( 15, 0 ); +acado_copyHTH( 15, 1 ); +acado_copyHTH( 15, 2 ); +acado_copyHTH( 15, 3 ); +acado_copyHTH( 15, 4 ); +acado_copyHTH( 15, 5 ); +acado_copyHTH( 15, 6 ); +acado_copyHTH( 15, 7 ); +acado_copyHTH( 15, 8 ); +acado_copyHTH( 15, 9 ); +acado_copyHTH( 15, 10 ); +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[156] = acadoWorkspace.H10[0]; +acadoWorkspace.H[157] = acadoWorkspace.H10[1]; +acadoWorkspace.H[158] = acadoWorkspace.H10[2]; +acadoWorkspace.H[159] = acadoWorkspace.H10[3]; +acadoWorkspace.H[160] = acadoWorkspace.H10[4]; +acadoWorkspace.H[161] = acadoWorkspace.H10[5]; +acadoWorkspace.H[182] = acadoWorkspace.H10[6]; +acadoWorkspace.H[183] = acadoWorkspace.H10[7]; +acadoWorkspace.H[184] = acadoWorkspace.H10[8]; +acadoWorkspace.H[185] = acadoWorkspace.H10[9]; +acadoWorkspace.H[186] = acadoWorkspace.H10[10]; +acadoWorkspace.H[187] = acadoWorkspace.H10[11]; +acadoWorkspace.H[208] = acadoWorkspace.H10[12]; +acadoWorkspace.H[209] = acadoWorkspace.H10[13]; +acadoWorkspace.H[210] = acadoWorkspace.H10[14]; +acadoWorkspace.H[211] = acadoWorkspace.H10[15]; +acadoWorkspace.H[212] = acadoWorkspace.H10[16]; +acadoWorkspace.H[213] = acadoWorkspace.H10[17]; +acadoWorkspace.H[234] = acadoWorkspace.H10[18]; +acadoWorkspace.H[235] = acadoWorkspace.H10[19]; +acadoWorkspace.H[236] = acadoWorkspace.H10[20]; +acadoWorkspace.H[237] = acadoWorkspace.H10[21]; +acadoWorkspace.H[238] = acadoWorkspace.H10[22]; +acadoWorkspace.H[239] = acadoWorkspace.H10[23]; +acadoWorkspace.H[260] = acadoWorkspace.H10[24]; +acadoWorkspace.H[261] = acadoWorkspace.H10[25]; +acadoWorkspace.H[262] = acadoWorkspace.H10[26]; +acadoWorkspace.H[263] = acadoWorkspace.H10[27]; +acadoWorkspace.H[264] = acadoWorkspace.H10[28]; +acadoWorkspace.H[265] = acadoWorkspace.H10[29]; +acadoWorkspace.H[286] = acadoWorkspace.H10[30]; +acadoWorkspace.H[287] = 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[316] = acadoWorkspace.H10[40]; +acadoWorkspace.H[317] = acadoWorkspace.H10[41]; +acadoWorkspace.H[338] = acadoWorkspace.H10[42]; +acadoWorkspace.H[339] = acadoWorkspace.H10[43]; +acadoWorkspace.H[340] = acadoWorkspace.H10[44]; +acadoWorkspace.H[341] = acadoWorkspace.H10[45]; +acadoWorkspace.H[342] = acadoWorkspace.H10[46]; +acadoWorkspace.H[343] = acadoWorkspace.H10[47]; +acadoWorkspace.H[364] = acadoWorkspace.H10[48]; +acadoWorkspace.H[365] = acadoWorkspace.H10[49]; +acadoWorkspace.H[366] = acadoWorkspace.H10[50]; +acadoWorkspace.H[367] = acadoWorkspace.H10[51]; +acadoWorkspace.H[368] = acadoWorkspace.H10[52]; +acadoWorkspace.H[369] = acadoWorkspace.H10[53]; +acadoWorkspace.H[390] = acadoWorkspace.H10[54]; +acadoWorkspace.H[391] = acadoWorkspace.H10[55]; +acadoWorkspace.H[392] = acadoWorkspace.H10[56]; +acadoWorkspace.H[393] = acadoWorkspace.H10[57]; +acadoWorkspace.H[394] = acadoWorkspace.H10[58]; +acadoWorkspace.H[395] = acadoWorkspace.H10[59]; +acadoWorkspace.H[416] = acadoWorkspace.H10[60]; +acadoWorkspace.H[417] = acadoWorkspace.H10[61]; +acadoWorkspace.H[418] = acadoWorkspace.H10[62]; +acadoWorkspace.H[419] = acadoWorkspace.H10[63]; +acadoWorkspace.H[420] = acadoWorkspace.H10[64]; +acadoWorkspace.H[421] = acadoWorkspace.H10[65]; +acadoWorkspace.H[442] = acadoWorkspace.H10[66]; +acadoWorkspace.H[443] = acadoWorkspace.H10[67]; +acadoWorkspace.H[444] = acadoWorkspace.H10[68]; +acadoWorkspace.H[445] = acadoWorkspace.H10[69]; +acadoWorkspace.H[446] = acadoWorkspace.H10[70]; +acadoWorkspace.H[447] = acadoWorkspace.H10[71]; +acadoWorkspace.H[468] = acadoWorkspace.H10[72]; +acadoWorkspace.H[469] = acadoWorkspace.H10[73]; +acadoWorkspace.H[470] = acadoWorkspace.H10[74]; +acadoWorkspace.H[471] = acadoWorkspace.H10[75]; +acadoWorkspace.H[472] = acadoWorkspace.H10[76]; +acadoWorkspace.H[473] = acadoWorkspace.H10[77]; +acadoWorkspace.H[494] = acadoWorkspace.H10[78]; +acadoWorkspace.H[495] = acadoWorkspace.H10[79]; +acadoWorkspace.H[496] = acadoWorkspace.H10[80]; +acadoWorkspace.H[497] = acadoWorkspace.H10[81]; +acadoWorkspace.H[498] = acadoWorkspace.H10[82]; +acadoWorkspace.H[499] = acadoWorkspace.H10[83]; +acadoWorkspace.H[520] = acadoWorkspace.H10[84]; +acadoWorkspace.H[521] = acadoWorkspace.H10[85]; +acadoWorkspace.H[522] = acadoWorkspace.H10[86]; +acadoWorkspace.H[523] = acadoWorkspace.H10[87]; +acadoWorkspace.H[524] = acadoWorkspace.H10[88]; +acadoWorkspace.H[525] = acadoWorkspace.H10[89]; +acadoWorkspace.H[546] = acadoWorkspace.H10[90]; +acadoWorkspace.H[547] = acadoWorkspace.H10[91]; +acadoWorkspace.H[548] = acadoWorkspace.H10[92]; +acadoWorkspace.H[549] = acadoWorkspace.H10[93]; +acadoWorkspace.H[550] = acadoWorkspace.H10[94]; +acadoWorkspace.H[551] = acadoWorkspace.H10[95]; +acadoWorkspace.H[572] = acadoWorkspace.H10[96]; +acadoWorkspace.H[573] = acadoWorkspace.H10[97]; +acadoWorkspace.H[574] = acadoWorkspace.H10[98]; +acadoWorkspace.H[575] = acadoWorkspace.H10[99]; +acadoWorkspace.H[576] = acadoWorkspace.H10[100]; +acadoWorkspace.H[577] = acadoWorkspace.H10[101]; +acadoWorkspace.H[598] = acadoWorkspace.H10[102]; +acadoWorkspace.H[599] = acadoWorkspace.H10[103]; +acadoWorkspace.H[600] = acadoWorkspace.H10[104]; +acadoWorkspace.H[601] = acadoWorkspace.H10[105]; +acadoWorkspace.H[602] = acadoWorkspace.H10[106]; +acadoWorkspace.H[603] = acadoWorkspace.H10[107]; +acadoWorkspace.H[624] = acadoWorkspace.H10[108]; +acadoWorkspace.H[625] = acadoWorkspace.H10[109]; +acadoWorkspace.H[626] = acadoWorkspace.H10[110]; +acadoWorkspace.H[627] = acadoWorkspace.H10[111]; +acadoWorkspace.H[628] = acadoWorkspace.H10[112]; +acadoWorkspace.H[629] = acadoWorkspace.H10[113]; +acadoWorkspace.H[650] = acadoWorkspace.H10[114]; +acadoWorkspace.H[651] = acadoWorkspace.H10[115]; +acadoWorkspace.H[652] = acadoWorkspace.H10[116]; +acadoWorkspace.H[653] = acadoWorkspace.H10[117]; +acadoWorkspace.H[654] = acadoWorkspace.H10[118]; +acadoWorkspace.H[655] = acadoWorkspace.H10[119]; acado_multQ1d( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.d, acadoWorkspace.Qd ); acado_multQ1d( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.Qd[ 6 ]) ); @@ -923,37 +3944,7 @@ acado_multQ1d( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.d[ 90 ]), &(acadoWo acado_multQ1d( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.Qd[ 96 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.d[ 102 ]), &(acadoWorkspace.Qd[ 102 ]) ); acado_multQ1d( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.Qd[ 108 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 720 ]), &(acadoWorkspace.d[ 114 ]), &(acadoWorkspace.Qd[ 114 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 756 ]), &(acadoWorkspace.d[ 120 ]), &(acadoWorkspace.Qd[ 120 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 792 ]), &(acadoWorkspace.d[ 126 ]), &(acadoWorkspace.Qd[ 126 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 828 ]), &(acadoWorkspace.d[ 132 ]), &(acadoWorkspace.Qd[ 132 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 864 ]), &(acadoWorkspace.d[ 138 ]), &(acadoWorkspace.Qd[ 138 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 900 ]), &(acadoWorkspace.d[ 144 ]), &(acadoWorkspace.Qd[ 144 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 936 ]), &(acadoWorkspace.d[ 150 ]), &(acadoWorkspace.Qd[ 150 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 972 ]), &(acadoWorkspace.d[ 156 ]), &(acadoWorkspace.Qd[ 156 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1008 ]), &(acadoWorkspace.d[ 162 ]), &(acadoWorkspace.Qd[ 162 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1044 ]), &(acadoWorkspace.d[ 168 ]), &(acadoWorkspace.Qd[ 168 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1080 ]), &(acadoWorkspace.d[ 174 ]), &(acadoWorkspace.Qd[ 174 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1116 ]), &(acadoWorkspace.d[ 180 ]), &(acadoWorkspace.Qd[ 180 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1152 ]), &(acadoWorkspace.d[ 186 ]), &(acadoWorkspace.Qd[ 186 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1188 ]), &(acadoWorkspace.d[ 192 ]), &(acadoWorkspace.Qd[ 192 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1224 ]), &(acadoWorkspace.d[ 198 ]), &(acadoWorkspace.Qd[ 198 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1260 ]), &(acadoWorkspace.d[ 204 ]), &(acadoWorkspace.Qd[ 204 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1296 ]), &(acadoWorkspace.d[ 210 ]), &(acadoWorkspace.Qd[ 210 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1332 ]), &(acadoWorkspace.d[ 216 ]), &(acadoWorkspace.Qd[ 216 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1368 ]), &(acadoWorkspace.d[ 222 ]), &(acadoWorkspace.Qd[ 222 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1404 ]), &(acadoWorkspace.d[ 228 ]), &(acadoWorkspace.Qd[ 228 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1440 ]), &(acadoWorkspace.d[ 234 ]), &(acadoWorkspace.Qd[ 234 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1476 ]), &(acadoWorkspace.d[ 240 ]), &(acadoWorkspace.Qd[ 240 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1512 ]), &(acadoWorkspace.d[ 246 ]), &(acadoWorkspace.Qd[ 246 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1548 ]), &(acadoWorkspace.d[ 252 ]), &(acadoWorkspace.Qd[ 252 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1584 ]), &(acadoWorkspace.d[ 258 ]), &(acadoWorkspace.Qd[ 258 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1620 ]), &(acadoWorkspace.d[ 264 ]), &(acadoWorkspace.Qd[ 264 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1656 ]), &(acadoWorkspace.d[ 270 ]), &(acadoWorkspace.Qd[ 270 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1692 ]), &(acadoWorkspace.d[ 276 ]), &(acadoWorkspace.Qd[ 276 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1728 ]), &(acadoWorkspace.d[ 282 ]), &(acadoWorkspace.Qd[ 282 ]) ); -acado_multQ1d( &(acadoWorkspace.Q1[ 1764 ]), &(acadoWorkspace.d[ 288 ]), &(acadoWorkspace.Qd[ 288 ]) ); -acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 294 ]), &(acadoWorkspace.Qd[ 294 ]) ); +acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 114 ]), &(acadoWorkspace.Qd[ 114 ]) ); acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.g ); @@ -975,44 +3966,216 @@ acado_macCTSlx( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.g ); acado_macCTSlx( &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 720 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 756 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 792 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 828 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 864 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 900 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 936 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 972 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1008 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1044 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1080 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1116 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1152 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1188 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1224 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1260 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1296 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1332 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1368 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1404 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1440 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1476 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1512 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1548 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1584 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1620 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1656 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1692 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1728 ]), acadoWorkspace.g ); -acado_macCTSlx( &(acadoWorkspace.evGx[ 1764 ]), acadoWorkspace.g ); -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_macETSlu( &(acadoWorkspace.QE[ lRun3 * 6 ]), &(acadoWorkspace.g[ lRun1 + 6 ]) ); -} -} +acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 630 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 918 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1026 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1140 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 726 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 822 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 924 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1032 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1146 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 642 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 930 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1038 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1152 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 738 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 834 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 936 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1044 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1158 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 654 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 840 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 942 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1050 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1164 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 750 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 846 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 948 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1056 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1170 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 666 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 852 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 954 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1062 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1176 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 762 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 858 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 960 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1068 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1182 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 678 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 864 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 966 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1074 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1188 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 774 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 870 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 972 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1080 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1194 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 690 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 876 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 978 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1086 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1200 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 786 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 882 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 984 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1092 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1206 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 702 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 888 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 990 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1098 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1212 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 798 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 894 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 996 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1104 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1218 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 714 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 900 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1002 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1110 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1224 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 810 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 906 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1008 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1116 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1230 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 912 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1014 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1122 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1236 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1020 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1128 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1242 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1134 ]), &(acadoWorkspace.g[ 24 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1248 ]), &(acadoWorkspace.g[ 24 ]) ); +acado_macETSlu( &(acadoWorkspace.QE[ 1254 ]), &(acadoWorkspace.g[ 25 ]) ); acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2]; @@ -1033,36 +4196,6 @@ acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16]; acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17]; acadoWorkspace.lb[24] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18]; acadoWorkspace.lb[25] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19]; -acadoWorkspace.lb[26] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[20]; -acadoWorkspace.lb[27] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[21]; -acadoWorkspace.lb[28] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[22]; -acadoWorkspace.lb[29] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[23]; -acadoWorkspace.lb[30] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[24]; -acadoWorkspace.lb[31] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[25]; -acadoWorkspace.lb[32] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[26]; -acadoWorkspace.lb[33] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[27]; -acadoWorkspace.lb[34] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[28]; -acadoWorkspace.lb[35] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[29]; -acadoWorkspace.lb[36] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[30]; -acadoWorkspace.lb[37] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[31]; -acadoWorkspace.lb[38] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[32]; -acadoWorkspace.lb[39] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[33]; -acadoWorkspace.lb[40] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[34]; -acadoWorkspace.lb[41] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[35]; -acadoWorkspace.lb[42] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[36]; -acadoWorkspace.lb[43] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[37]; -acadoWorkspace.lb[44] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[38]; -acadoWorkspace.lb[45] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[39]; -acadoWorkspace.lb[46] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[40]; -acadoWorkspace.lb[47] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[41]; -acadoWorkspace.lb[48] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[42]; -acadoWorkspace.lb[49] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[43]; -acadoWorkspace.lb[50] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[44]; -acadoWorkspace.lb[51] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[45]; -acadoWorkspace.lb[52] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[46]; -acadoWorkspace.lb[53] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[47]; -acadoWorkspace.lb[54] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[48]; -acadoWorkspace.lb[55] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[49]; acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0]; acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1]; acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2]; @@ -1083,51 +4216,21 @@ acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16]; acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17]; acadoWorkspace.ub[24] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18]; acadoWorkspace.ub[25] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19]; -acadoWorkspace.ub[26] = (real_t)1.0000000000000000e+12 - acadoVariables.u[20]; -acadoWorkspace.ub[27] = (real_t)1.0000000000000000e+12 - acadoVariables.u[21]; -acadoWorkspace.ub[28] = (real_t)1.0000000000000000e+12 - acadoVariables.u[22]; -acadoWorkspace.ub[29] = (real_t)1.0000000000000000e+12 - acadoVariables.u[23]; -acadoWorkspace.ub[30] = (real_t)1.0000000000000000e+12 - acadoVariables.u[24]; -acadoWorkspace.ub[31] = (real_t)1.0000000000000000e+12 - acadoVariables.u[25]; -acadoWorkspace.ub[32] = (real_t)1.0000000000000000e+12 - acadoVariables.u[26]; -acadoWorkspace.ub[33] = (real_t)1.0000000000000000e+12 - acadoVariables.u[27]; -acadoWorkspace.ub[34] = (real_t)1.0000000000000000e+12 - acadoVariables.u[28]; -acadoWorkspace.ub[35] = (real_t)1.0000000000000000e+12 - acadoVariables.u[29]; -acadoWorkspace.ub[36] = (real_t)1.0000000000000000e+12 - acadoVariables.u[30]; -acadoWorkspace.ub[37] = (real_t)1.0000000000000000e+12 - acadoVariables.u[31]; -acadoWorkspace.ub[38] = (real_t)1.0000000000000000e+12 - acadoVariables.u[32]; -acadoWorkspace.ub[39] = (real_t)1.0000000000000000e+12 - acadoVariables.u[33]; -acadoWorkspace.ub[40] = (real_t)1.0000000000000000e+12 - acadoVariables.u[34]; -acadoWorkspace.ub[41] = (real_t)1.0000000000000000e+12 - acadoVariables.u[35]; -acadoWorkspace.ub[42] = (real_t)1.0000000000000000e+12 - acadoVariables.u[36]; -acadoWorkspace.ub[43] = (real_t)1.0000000000000000e+12 - acadoVariables.u[37]; -acadoWorkspace.ub[44] = (real_t)1.0000000000000000e+12 - acadoVariables.u[38]; -acadoWorkspace.ub[45] = (real_t)1.0000000000000000e+12 - acadoVariables.u[39]; -acadoWorkspace.ub[46] = (real_t)1.0000000000000000e+12 - acadoVariables.u[40]; -acadoWorkspace.ub[47] = (real_t)1.0000000000000000e+12 - acadoVariables.u[41]; -acadoWorkspace.ub[48] = (real_t)1.0000000000000000e+12 - acadoVariables.u[42]; -acadoWorkspace.ub[49] = (real_t)1.0000000000000000e+12 - acadoVariables.u[43]; -acadoWorkspace.ub[50] = (real_t)1.0000000000000000e+12 - acadoVariables.u[44]; -acadoWorkspace.ub[51] = (real_t)1.0000000000000000e+12 - acadoVariables.u[45]; -acadoWorkspace.ub[52] = (real_t)1.0000000000000000e+12 - acadoVariables.u[46]; -acadoWorkspace.ub[53] = (real_t)1.0000000000000000e+12 - acadoVariables.u[47]; -acadoWorkspace.ub[54] = (real_t)1.0000000000000000e+12 - acadoVariables.u[48]; -acadoWorkspace.ub[55] = (real_t)1.0000000000000000e+12 - acadoVariables.u[49]; - -for (lRun1 = 0; lRun1 < 50; ++lRun1) + +for (lRun1 = 0; lRun1 < 20; ++lRun1) { lRun3 = xBoundIndices[ lRun1 ] - 6; lRun4 = ((lRun3) / (6)) + (1); -acadoWorkspace.A[lRun1 * 56] = acadoWorkspace.evGx[lRun3 * 6]; -acadoWorkspace.A[lRun1 * 56 + 1] = acadoWorkspace.evGx[lRun3 * 6 + 1]; -acadoWorkspace.A[lRun1 * 56 + 2] = acadoWorkspace.evGx[lRun3 * 6 + 2]; -acadoWorkspace.A[lRun1 * 56 + 3] = acadoWorkspace.evGx[lRun3 * 6 + 3]; -acadoWorkspace.A[lRun1 * 56 + 4] = acadoWorkspace.evGx[lRun3 * 6 + 4]; -acadoWorkspace.A[lRun1 * 56 + 5] = acadoWorkspace.evGx[lRun3 * 6 + 5]; +acadoWorkspace.A[lRun1 * 26] = acadoWorkspace.evGx[lRun3 * 6]; +acadoWorkspace.A[lRun1 * 26 + 1] = acadoWorkspace.evGx[lRun3 * 6 + 1]; +acadoWorkspace.A[lRun1 * 26 + 2] = acadoWorkspace.evGx[lRun3 * 6 + 2]; +acadoWorkspace.A[lRun1 * 26 + 3] = acadoWorkspace.evGx[lRun3 * 6 + 3]; +acadoWorkspace.A[lRun1 * 26 + 4] = acadoWorkspace.evGx[lRun3 * 6 + 4]; +acadoWorkspace.A[lRun1 * 26 + 5] = acadoWorkspace.evGx[lRun3 * 6 + 5]; for (lRun2 = 0; lRun2 < lRun4; ++lRun2) { lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (6)) + ((lRun3) % (6)); -acadoWorkspace.A[(lRun1 * 56) + (lRun2 + 6)] = acadoWorkspace.E[lRun5]; +acadoWorkspace.A[(lRun1 * 26) + (lRun2 + 6)] = acadoWorkspace.E[lRun5]; } } @@ -1135,9 +4238,6 @@ acadoWorkspace.A[(lRun1 * 56) + (lRun2 + 6)] = acadoWorkspace.E[lRun5]; void acado_condenseFdb( ) { -int lRun1; -int lRun2; -int lRun3; real_t tmp; acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0]; @@ -1147,9 +4247,86 @@ acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3]; acadoWorkspace.Dx0[4] = acadoVariables.x0[4] - acadoVariables.x[4]; acadoWorkspace.Dx0[5] = acadoVariables.x0[5] - acadoVariables.x[5]; -for (lRun2 = 0; lRun2 < 200; ++lRun2) -acadoWorkspace.Dy[lRun2] -= acadoVariables.y[lRun2]; - +acadoWorkspace.Dy[0] -= acadoVariables.y[0]; +acadoWorkspace.Dy[1] -= acadoVariables.y[1]; +acadoWorkspace.Dy[2] -= acadoVariables.y[2]; +acadoWorkspace.Dy[3] -= acadoVariables.y[3]; +acadoWorkspace.Dy[4] -= acadoVariables.y[4]; +acadoWorkspace.Dy[5] -= acadoVariables.y[5]; +acadoWorkspace.Dy[6] -= acadoVariables.y[6]; +acadoWorkspace.Dy[7] -= acadoVariables.y[7]; +acadoWorkspace.Dy[8] -= acadoVariables.y[8]; +acadoWorkspace.Dy[9] -= acadoVariables.y[9]; +acadoWorkspace.Dy[10] -= acadoVariables.y[10]; +acadoWorkspace.Dy[11] -= acadoVariables.y[11]; +acadoWorkspace.Dy[12] -= acadoVariables.y[12]; +acadoWorkspace.Dy[13] -= acadoVariables.y[13]; +acadoWorkspace.Dy[14] -= acadoVariables.y[14]; +acadoWorkspace.Dy[15] -= acadoVariables.y[15]; +acadoWorkspace.Dy[16] -= acadoVariables.y[16]; +acadoWorkspace.Dy[17] -= acadoVariables.y[17]; +acadoWorkspace.Dy[18] -= acadoVariables.y[18]; +acadoWorkspace.Dy[19] -= acadoVariables.y[19]; +acadoWorkspace.Dy[20] -= acadoVariables.y[20]; +acadoWorkspace.Dy[21] -= acadoVariables.y[21]; +acadoWorkspace.Dy[22] -= acadoVariables.y[22]; +acadoWorkspace.Dy[23] -= acadoVariables.y[23]; +acadoWorkspace.Dy[24] -= acadoVariables.y[24]; +acadoWorkspace.Dy[25] -= acadoVariables.y[25]; +acadoWorkspace.Dy[26] -= acadoVariables.y[26]; +acadoWorkspace.Dy[27] -= acadoVariables.y[27]; +acadoWorkspace.Dy[28] -= acadoVariables.y[28]; +acadoWorkspace.Dy[29] -= acadoVariables.y[29]; +acadoWorkspace.Dy[30] -= acadoVariables.y[30]; +acadoWorkspace.Dy[31] -= acadoVariables.y[31]; +acadoWorkspace.Dy[32] -= acadoVariables.y[32]; +acadoWorkspace.Dy[33] -= acadoVariables.y[33]; +acadoWorkspace.Dy[34] -= acadoVariables.y[34]; +acadoWorkspace.Dy[35] -= acadoVariables.y[35]; +acadoWorkspace.Dy[36] -= acadoVariables.y[36]; +acadoWorkspace.Dy[37] -= acadoVariables.y[37]; +acadoWorkspace.Dy[38] -= acadoVariables.y[38]; +acadoWorkspace.Dy[39] -= acadoVariables.y[39]; +acadoWorkspace.Dy[40] -= acadoVariables.y[40]; +acadoWorkspace.Dy[41] -= acadoVariables.y[41]; +acadoWorkspace.Dy[42] -= acadoVariables.y[42]; +acadoWorkspace.Dy[43] -= acadoVariables.y[43]; +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.DyN[0] -= acadoVariables.yN[0]; acadoWorkspace.DyN[1] -= acadoVariables.yN[1]; acadoWorkspace.DyN[2] -= acadoVariables.yN[2]; @@ -1174,36 +4351,6 @@ acado_multRDy( &(acadoWorkspace.R2[ 64 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWo acado_multRDy( &(acadoWorkspace.R2[ 68 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.g[ 23 ]) ); acado_multRDy( &(acadoWorkspace.R2[ 72 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.g[ 24 ]) ); acado_multRDy( &(acadoWorkspace.R2[ 76 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.g[ 25 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 26 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 84 ]), &(acadoWorkspace.Dy[ 84 ]), &(acadoWorkspace.g[ 27 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 88 ]), &(acadoWorkspace.Dy[ 88 ]), &(acadoWorkspace.g[ 28 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 92 ]), &(acadoWorkspace.Dy[ 92 ]), &(acadoWorkspace.g[ 29 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 96 ]), &(acadoWorkspace.Dy[ 96 ]), &(acadoWorkspace.g[ 30 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 100 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.g[ 31 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 104 ]), &(acadoWorkspace.Dy[ 104 ]), &(acadoWorkspace.g[ 32 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 108 ]), &(acadoWorkspace.Dy[ 108 ]), &(acadoWorkspace.g[ 33 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 112 ]), &(acadoWorkspace.Dy[ 112 ]), &(acadoWorkspace.g[ 34 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 116 ]), &(acadoWorkspace.Dy[ 116 ]), &(acadoWorkspace.g[ 35 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 120 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.g[ 36 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 124 ]), &(acadoWorkspace.Dy[ 124 ]), &(acadoWorkspace.g[ 37 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 128 ]), &(acadoWorkspace.Dy[ 128 ]), &(acadoWorkspace.g[ 38 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 132 ]), &(acadoWorkspace.Dy[ 132 ]), &(acadoWorkspace.g[ 39 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 136 ]), &(acadoWorkspace.Dy[ 136 ]), &(acadoWorkspace.g[ 40 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 140 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.g[ 41 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 144 ]), &(acadoWorkspace.Dy[ 144 ]), &(acadoWorkspace.g[ 42 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 148 ]), &(acadoWorkspace.Dy[ 148 ]), &(acadoWorkspace.g[ 43 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 152 ]), &(acadoWorkspace.Dy[ 152 ]), &(acadoWorkspace.g[ 44 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 156 ]), &(acadoWorkspace.Dy[ 156 ]), &(acadoWorkspace.g[ 45 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 160 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.g[ 46 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 164 ]), &(acadoWorkspace.Dy[ 164 ]), &(acadoWorkspace.g[ 47 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 168 ]), &(acadoWorkspace.Dy[ 168 ]), &(acadoWorkspace.g[ 48 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 172 ]), &(acadoWorkspace.Dy[ 172 ]), &(acadoWorkspace.g[ 49 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 176 ]), &(acadoWorkspace.Dy[ 176 ]), &(acadoWorkspace.g[ 50 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 180 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.g[ 51 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 184 ]), &(acadoWorkspace.Dy[ 184 ]), &(acadoWorkspace.g[ 52 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 188 ]), &(acadoWorkspace.Dy[ 188 ]), &(acadoWorkspace.g[ 53 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 192 ]), &(acadoWorkspace.Dy[ 192 ]), &(acadoWorkspace.g[ 54 ]) ); -acado_multRDy( &(acadoWorkspace.R2[ 196 ]), &(acadoWorkspace.Dy[ 196 ]), &(acadoWorkspace.g[ 55 ]) ); acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy ); acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.QDy[ 6 ]) ); @@ -1225,64 +4372,353 @@ acado_multQDy( &(acadoWorkspace.Q2[ 384 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoW acado_multQDy( &(acadoWorkspace.Q2[ 408 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.QDy[ 102 ]) ); acado_multQDy( &(acadoWorkspace.Q2[ 432 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.QDy[ 108 ]) ); acado_multQDy( &(acadoWorkspace.Q2[ 456 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.QDy[ 114 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 480 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 120 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 504 ]), &(acadoWorkspace.Dy[ 84 ]), &(acadoWorkspace.QDy[ 126 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 528 ]), &(acadoWorkspace.Dy[ 88 ]), &(acadoWorkspace.QDy[ 132 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 552 ]), &(acadoWorkspace.Dy[ 92 ]), &(acadoWorkspace.QDy[ 138 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 576 ]), &(acadoWorkspace.Dy[ 96 ]), &(acadoWorkspace.QDy[ 144 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 600 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.QDy[ 150 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 624 ]), &(acadoWorkspace.Dy[ 104 ]), &(acadoWorkspace.QDy[ 156 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 648 ]), &(acadoWorkspace.Dy[ 108 ]), &(acadoWorkspace.QDy[ 162 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 672 ]), &(acadoWorkspace.Dy[ 112 ]), &(acadoWorkspace.QDy[ 168 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 696 ]), &(acadoWorkspace.Dy[ 116 ]), &(acadoWorkspace.QDy[ 174 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 720 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.QDy[ 180 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 744 ]), &(acadoWorkspace.Dy[ 124 ]), &(acadoWorkspace.QDy[ 186 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 768 ]), &(acadoWorkspace.Dy[ 128 ]), &(acadoWorkspace.QDy[ 192 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 792 ]), &(acadoWorkspace.Dy[ 132 ]), &(acadoWorkspace.QDy[ 198 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 816 ]), &(acadoWorkspace.Dy[ 136 ]), &(acadoWorkspace.QDy[ 204 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 840 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.QDy[ 210 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 864 ]), &(acadoWorkspace.Dy[ 144 ]), &(acadoWorkspace.QDy[ 216 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 888 ]), &(acadoWorkspace.Dy[ 148 ]), &(acadoWorkspace.QDy[ 222 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 912 ]), &(acadoWorkspace.Dy[ 152 ]), &(acadoWorkspace.QDy[ 228 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 936 ]), &(acadoWorkspace.Dy[ 156 ]), &(acadoWorkspace.QDy[ 234 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 960 ]), &(acadoWorkspace.Dy[ 160 ]), &(acadoWorkspace.QDy[ 240 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 984 ]), &(acadoWorkspace.Dy[ 164 ]), &(acadoWorkspace.QDy[ 246 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1008 ]), &(acadoWorkspace.Dy[ 168 ]), &(acadoWorkspace.QDy[ 252 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1032 ]), &(acadoWorkspace.Dy[ 172 ]), &(acadoWorkspace.QDy[ 258 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1056 ]), &(acadoWorkspace.Dy[ 176 ]), &(acadoWorkspace.QDy[ 264 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1080 ]), &(acadoWorkspace.Dy[ 180 ]), &(acadoWorkspace.QDy[ 270 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1104 ]), &(acadoWorkspace.Dy[ 184 ]), &(acadoWorkspace.QDy[ 276 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1128 ]), &(acadoWorkspace.Dy[ 188 ]), &(acadoWorkspace.QDy[ 282 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1152 ]), &(acadoWorkspace.Dy[ 192 ]), &(acadoWorkspace.QDy[ 288 ]) ); -acado_multQDy( &(acadoWorkspace.Q2[ 1176 ]), &(acadoWorkspace.Dy[ 196 ]), &(acadoWorkspace.QDy[ 294 ]) ); - -acadoWorkspace.QDy[300] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2]; -acadoWorkspace.QDy[301] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2]; -acadoWorkspace.QDy[302] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2]; -acadoWorkspace.QDy[303] = + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[2]; -acadoWorkspace.QDy[304] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2]; -acadoWorkspace.QDy[305] = + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[16]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[17]*acadoWorkspace.DyN[2]; - -for (lRun2 = 0; lRun2 < 300; ++lRun2) -acadoWorkspace.QDy[lRun2 + 6] += acadoWorkspace.Qd[lRun2]; - - -acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[516]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[522]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[528]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[534]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[540]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[546]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[552]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[558]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[564]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[570]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[576]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[582]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[588]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[594]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[600]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[606]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[612]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[618]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[624]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[630]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[636]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[642]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[648]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[654]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[660]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[666]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[672]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[678]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[684]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[690]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[696]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[702]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[708]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[714]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[720]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[726]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[732]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[738]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[744]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[750]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[756]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[762]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[768]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[774]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[780]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[786]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[792]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[798]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[804]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[810]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[816]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[822]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[828]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[834]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[840]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[846]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[852]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[858]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[864]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[870]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[876]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[882]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[888]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[894]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[900]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[906]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[912]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[918]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[924]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[930]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[936]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[942]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[948]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[954]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[960]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[966]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[972]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[978]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[984]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[990]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[996]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1002]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1008]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1014]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1020]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1026]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1032]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1038]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1044]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1050]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1056]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1062]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1068]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1074]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1080]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1086]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1092]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1098]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1104]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1110]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1116]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1122]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1128]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1134]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1140]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1146]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1152]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1158]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1164]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1170]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1176]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1182]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1188]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1194]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1200]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1206]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1212]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1218]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1224]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1230]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1236]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1242]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1248]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1254]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1260]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1266]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1272]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1278]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1284]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1290]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1296]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1302]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1308]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1314]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1320]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1326]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1332]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1338]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1344]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1350]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1356]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1362]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1368]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1374]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1380]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1386]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1392]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1398]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1404]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1410]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1416]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1422]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1428]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1434]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1440]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1446]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1452]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1458]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1464]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1470]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1476]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1482]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1488]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1494]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1500]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1506]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1512]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1518]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1524]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1530]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1536]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1542]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1548]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1554]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1560]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1566]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1572]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1578]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1584]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1590]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1596]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1602]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1608]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1614]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1620]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1626]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1632]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1638]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1644]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1650]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1656]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1662]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1668]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1674]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1680]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1686]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1692]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1698]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1704]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1710]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1716]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1722]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1728]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1734]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1740]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1746]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1752]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1758]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1764]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1770]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1776]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1782]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1788]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1794]*acadoWorkspace.QDy[305]; -acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[517]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[523]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[529]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[535]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[541]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[547]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[553]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[559]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[565]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[571]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[577]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[583]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[589]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[595]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[601]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[607]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[613]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[619]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[625]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[631]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[637]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[643]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[649]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[655]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[661]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[667]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[673]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[679]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[685]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[691]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[697]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[703]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[709]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[715]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[721]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[727]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[733]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[739]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[745]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[751]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[757]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[763]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[769]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[775]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[781]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[787]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[793]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[799]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[805]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[811]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[817]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[823]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[829]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[835]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[841]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[847]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[853]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[859]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[865]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[871]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[877]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[883]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[889]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[895]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[901]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[907]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[913]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[919]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[925]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[931]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[937]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[943]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[949]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[955]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[961]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[967]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[973]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[979]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[985]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[991]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[997]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1003]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1009]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1015]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1021]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1027]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1033]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1039]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1045]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1051]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1057]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1063]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1069]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1075]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1081]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1087]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1093]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1099]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1105]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1111]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1117]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1123]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1129]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1135]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1141]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1147]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1153]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1159]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1165]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1171]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1177]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1183]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1189]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1195]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1201]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1207]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1213]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1219]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1225]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1231]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1237]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1243]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1249]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1255]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1261]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1267]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1273]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1279]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1285]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1291]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1297]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1303]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1309]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1315]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1321]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1327]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1333]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1339]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1345]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1351]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1357]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1363]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1369]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1375]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1381]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1387]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1393]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1399]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1405]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1411]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1417]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1423]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1429]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1435]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1441]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1447]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1453]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1459]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1465]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1471]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1477]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1483]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1489]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1495]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1501]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1507]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1513]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1519]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1525]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1531]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1537]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1543]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1549]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1555]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1561]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1567]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1573]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1579]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1585]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1591]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1597]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1603]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1609]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1615]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1621]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1627]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1633]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1639]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1645]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1651]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1657]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1663]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1669]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1675]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1681]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1687]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1693]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1699]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1705]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1711]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1717]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1723]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1729]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1735]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1741]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1747]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1753]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1759]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1765]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1771]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1777]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1783]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1789]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1795]*acadoWorkspace.QDy[305]; -acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[320]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[512]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[518]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[524]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[530]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[536]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[542]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[548]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[554]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[560]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[566]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[572]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[578]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[584]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[590]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[596]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[602]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[608]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[614]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[620]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[626]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[632]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[638]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[644]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[650]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[656]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[662]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[668]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[674]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[680]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[686]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[692]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[698]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[704]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[710]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[716]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[722]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[728]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[734]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[740]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[746]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[752]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[758]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[764]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[770]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[776]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[782]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[788]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[794]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[800]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[806]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[812]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[818]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[824]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[830]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[836]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[842]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[848]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[854]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[860]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[866]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[872]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[878]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[884]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[890]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[896]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[902]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[908]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[914]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[920]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[926]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[932]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[938]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[944]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[950]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[956]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[962]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[968]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[974]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[980]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[986]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[992]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[998]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1004]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1010]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1016]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1022]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1028]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1034]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1040]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1046]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1052]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1058]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1064]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1070]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1076]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1082]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1088]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1094]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1100]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1106]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1112]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1118]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1124]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1130]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1136]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1142]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1148]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1154]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1160]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1166]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1172]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1178]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1184]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1190]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1196]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1202]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1208]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1214]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1220]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1226]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1232]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1238]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1244]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1250]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1256]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1262]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1268]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1274]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1280]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1286]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1292]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1298]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1304]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1310]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1316]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1322]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1328]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1334]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1340]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1346]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1352]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1358]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1364]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1370]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1376]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1382]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1388]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1394]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1400]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1406]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1412]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1418]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1424]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1430]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1436]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1442]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1448]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1454]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1460]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1466]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1472]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1478]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1484]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1490]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1496]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1502]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1508]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1514]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1520]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1526]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1532]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1538]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1544]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1550]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1556]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1562]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1568]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1574]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1580]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1586]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1592]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1598]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1604]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1610]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1616]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1622]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1628]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1634]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1640]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1646]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1652]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1658]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1664]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1670]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1676]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1682]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1688]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1694]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1700]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1706]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1712]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1718]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1724]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1730]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1736]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1742]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1748]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1754]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1760]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1766]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1772]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1778]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1784]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1790]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1796]*acadoWorkspace.QDy[305]; -acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[321]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[513]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[519]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[525]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[531]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[537]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[543]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[549]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[555]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[561]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[567]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[573]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[579]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[585]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[591]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[597]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[603]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[609]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[615]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[621]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[627]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[633]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[639]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[645]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[651]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[657]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[663]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[669]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[675]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[681]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[687]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[693]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[699]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[705]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[711]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[717]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[723]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[729]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[735]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[741]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[747]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[753]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[759]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[765]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[771]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[777]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[783]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[789]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[795]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[801]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[807]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[813]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[819]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[825]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[831]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[837]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[843]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[849]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[855]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[861]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[867]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[873]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[879]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[885]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[891]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[897]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[903]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[909]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[915]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[921]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[927]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[933]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[939]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[945]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[951]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[957]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[963]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[969]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[975]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[981]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[987]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[993]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[999]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1005]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1011]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1017]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1023]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1029]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1035]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1041]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1047]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1053]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1059]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1065]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1071]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1077]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1083]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1089]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1095]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1101]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1107]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1113]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1119]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1125]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1131]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1137]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1143]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1149]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1155]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1161]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1167]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1173]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1179]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1185]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1191]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1197]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1203]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1209]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1215]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1221]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1227]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1233]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1239]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1245]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1251]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1257]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1263]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1269]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1275]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1281]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1287]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1293]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1299]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1305]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1311]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1317]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1323]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1329]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1335]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1341]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1347]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1353]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1359]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1365]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1371]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1377]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1383]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1389]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1395]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1401]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1407]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1413]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1419]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1425]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1431]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1437]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1443]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1449]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1455]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1461]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1467]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1473]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1479]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1485]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1491]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1497]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1503]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1509]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1515]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1521]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1527]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1533]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1539]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1545]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1551]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1557]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1563]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1569]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1575]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1581]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1587]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1593]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1599]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1605]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1611]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1617]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1623]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1629]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1635]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1641]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1647]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1653]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1659]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1665]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1671]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1677]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1683]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1689]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1695]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1701]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1707]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1713]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1719]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1725]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1731]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1737]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1743]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1749]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1755]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1761]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1767]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1773]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1779]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1785]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1791]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1797]*acadoWorkspace.QDy[305]; -acadoWorkspace.g[4] = + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[322]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[514]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[520]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[526]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[532]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[538]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[544]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[550]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[556]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[562]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[568]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[574]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[580]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[586]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[592]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[598]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[604]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[610]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[616]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[622]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[628]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[634]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[640]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[646]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[652]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[658]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[664]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[670]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[676]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[682]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[688]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[694]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[700]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[706]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[712]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[718]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[724]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[730]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[736]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[742]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[748]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[754]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[760]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[766]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[772]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[778]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[784]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[790]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[796]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[802]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[808]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[814]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[820]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[826]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[832]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[838]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[844]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[850]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[856]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[862]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[868]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[874]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[880]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[886]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[892]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[898]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[904]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[910]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[916]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[922]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[928]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[934]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[940]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[946]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[952]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[958]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[964]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[970]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[976]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[982]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[988]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[994]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[1000]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1006]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1012]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1018]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1024]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1030]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1036]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1042]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1048]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1054]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1060]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1066]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1072]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1078]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1084]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1090]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1096]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1102]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1108]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1114]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1120]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1126]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1132]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1138]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1144]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1150]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1156]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1162]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1168]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1174]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1180]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1186]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1192]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1198]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1204]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1210]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1216]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1222]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1228]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1234]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1240]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1246]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1252]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1258]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1264]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1270]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1276]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1282]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1288]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1294]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1300]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1306]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1312]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1318]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1324]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1330]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1336]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1342]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1348]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1354]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1360]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1366]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1372]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1378]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1384]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1390]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1396]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1402]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1408]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1414]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1420]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1426]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1432]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1438]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1444]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1450]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1456]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1462]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1468]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1474]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1480]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1486]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1492]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1498]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1504]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1510]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1516]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1522]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1528]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1534]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1540]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1546]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1552]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1558]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1564]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1570]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1576]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1582]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1588]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1594]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1600]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1606]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1612]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1618]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1624]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1630]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1636]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1642]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1648]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1654]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1660]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1666]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1672]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1678]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1684]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1690]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1696]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1702]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1708]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1714]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1720]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1726]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1732]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1738]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1744]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1750]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1756]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1762]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1768]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1774]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1780]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1786]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1792]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1798]*acadoWorkspace.QDy[305]; -acadoWorkspace.g[5] = + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[323]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[515]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[521]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[527]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[533]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[539]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[545]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[551]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[557]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[563]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[569]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[575]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[581]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[587]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[593]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[599]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[605]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[611]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[617]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[623]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[629]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[635]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[641]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[647]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[653]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[659]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[665]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[671]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[677]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[683]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[689]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[695]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[701]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[707]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[713]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[719]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[725]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[731]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[737]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[743]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[749]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[755]*acadoWorkspace.QDy[131] + acadoWorkspace.evGx[761]*acadoWorkspace.QDy[132] + acadoWorkspace.evGx[767]*acadoWorkspace.QDy[133] + acadoWorkspace.evGx[773]*acadoWorkspace.QDy[134] + acadoWorkspace.evGx[779]*acadoWorkspace.QDy[135] + acadoWorkspace.evGx[785]*acadoWorkspace.QDy[136] + acadoWorkspace.evGx[791]*acadoWorkspace.QDy[137] + acadoWorkspace.evGx[797]*acadoWorkspace.QDy[138] + acadoWorkspace.evGx[803]*acadoWorkspace.QDy[139] + acadoWorkspace.evGx[809]*acadoWorkspace.QDy[140] + acadoWorkspace.evGx[815]*acadoWorkspace.QDy[141] + acadoWorkspace.evGx[821]*acadoWorkspace.QDy[142] + acadoWorkspace.evGx[827]*acadoWorkspace.QDy[143] + acadoWorkspace.evGx[833]*acadoWorkspace.QDy[144] + acadoWorkspace.evGx[839]*acadoWorkspace.QDy[145] + acadoWorkspace.evGx[845]*acadoWorkspace.QDy[146] + acadoWorkspace.evGx[851]*acadoWorkspace.QDy[147] + acadoWorkspace.evGx[857]*acadoWorkspace.QDy[148] + acadoWorkspace.evGx[863]*acadoWorkspace.QDy[149] + acadoWorkspace.evGx[869]*acadoWorkspace.QDy[150] + acadoWorkspace.evGx[875]*acadoWorkspace.QDy[151] + acadoWorkspace.evGx[881]*acadoWorkspace.QDy[152] + acadoWorkspace.evGx[887]*acadoWorkspace.QDy[153] + acadoWorkspace.evGx[893]*acadoWorkspace.QDy[154] + acadoWorkspace.evGx[899]*acadoWorkspace.QDy[155] + acadoWorkspace.evGx[905]*acadoWorkspace.QDy[156] + acadoWorkspace.evGx[911]*acadoWorkspace.QDy[157] + acadoWorkspace.evGx[917]*acadoWorkspace.QDy[158] + acadoWorkspace.evGx[923]*acadoWorkspace.QDy[159] + acadoWorkspace.evGx[929]*acadoWorkspace.QDy[160] + acadoWorkspace.evGx[935]*acadoWorkspace.QDy[161] + acadoWorkspace.evGx[941]*acadoWorkspace.QDy[162] + acadoWorkspace.evGx[947]*acadoWorkspace.QDy[163] + acadoWorkspace.evGx[953]*acadoWorkspace.QDy[164] + acadoWorkspace.evGx[959]*acadoWorkspace.QDy[165] + acadoWorkspace.evGx[965]*acadoWorkspace.QDy[166] + acadoWorkspace.evGx[971]*acadoWorkspace.QDy[167] + acadoWorkspace.evGx[977]*acadoWorkspace.QDy[168] + acadoWorkspace.evGx[983]*acadoWorkspace.QDy[169] + acadoWorkspace.evGx[989]*acadoWorkspace.QDy[170] + acadoWorkspace.evGx[995]*acadoWorkspace.QDy[171] + acadoWorkspace.evGx[1001]*acadoWorkspace.QDy[172] + acadoWorkspace.evGx[1007]*acadoWorkspace.QDy[173] + acadoWorkspace.evGx[1013]*acadoWorkspace.QDy[174] + acadoWorkspace.evGx[1019]*acadoWorkspace.QDy[175] + acadoWorkspace.evGx[1025]*acadoWorkspace.QDy[176] + acadoWorkspace.evGx[1031]*acadoWorkspace.QDy[177] + acadoWorkspace.evGx[1037]*acadoWorkspace.QDy[178] + acadoWorkspace.evGx[1043]*acadoWorkspace.QDy[179] + acadoWorkspace.evGx[1049]*acadoWorkspace.QDy[180] + acadoWorkspace.evGx[1055]*acadoWorkspace.QDy[181] + acadoWorkspace.evGx[1061]*acadoWorkspace.QDy[182] + acadoWorkspace.evGx[1067]*acadoWorkspace.QDy[183] + acadoWorkspace.evGx[1073]*acadoWorkspace.QDy[184] + acadoWorkspace.evGx[1079]*acadoWorkspace.QDy[185] + acadoWorkspace.evGx[1085]*acadoWorkspace.QDy[186] + acadoWorkspace.evGx[1091]*acadoWorkspace.QDy[187] + acadoWorkspace.evGx[1097]*acadoWorkspace.QDy[188] + acadoWorkspace.evGx[1103]*acadoWorkspace.QDy[189] + acadoWorkspace.evGx[1109]*acadoWorkspace.QDy[190] + acadoWorkspace.evGx[1115]*acadoWorkspace.QDy[191] + acadoWorkspace.evGx[1121]*acadoWorkspace.QDy[192] + acadoWorkspace.evGx[1127]*acadoWorkspace.QDy[193] + acadoWorkspace.evGx[1133]*acadoWorkspace.QDy[194] + acadoWorkspace.evGx[1139]*acadoWorkspace.QDy[195] + acadoWorkspace.evGx[1145]*acadoWorkspace.QDy[196] + acadoWorkspace.evGx[1151]*acadoWorkspace.QDy[197] + acadoWorkspace.evGx[1157]*acadoWorkspace.QDy[198] + acadoWorkspace.evGx[1163]*acadoWorkspace.QDy[199] + acadoWorkspace.evGx[1169]*acadoWorkspace.QDy[200] + acadoWorkspace.evGx[1175]*acadoWorkspace.QDy[201] + acadoWorkspace.evGx[1181]*acadoWorkspace.QDy[202] + acadoWorkspace.evGx[1187]*acadoWorkspace.QDy[203] + acadoWorkspace.evGx[1193]*acadoWorkspace.QDy[204] + acadoWorkspace.evGx[1199]*acadoWorkspace.QDy[205] + acadoWorkspace.evGx[1205]*acadoWorkspace.QDy[206] + acadoWorkspace.evGx[1211]*acadoWorkspace.QDy[207] + acadoWorkspace.evGx[1217]*acadoWorkspace.QDy[208] + acadoWorkspace.evGx[1223]*acadoWorkspace.QDy[209] + acadoWorkspace.evGx[1229]*acadoWorkspace.QDy[210] + acadoWorkspace.evGx[1235]*acadoWorkspace.QDy[211] + acadoWorkspace.evGx[1241]*acadoWorkspace.QDy[212] + acadoWorkspace.evGx[1247]*acadoWorkspace.QDy[213] + acadoWorkspace.evGx[1253]*acadoWorkspace.QDy[214] + acadoWorkspace.evGx[1259]*acadoWorkspace.QDy[215] + acadoWorkspace.evGx[1265]*acadoWorkspace.QDy[216] + acadoWorkspace.evGx[1271]*acadoWorkspace.QDy[217] + acadoWorkspace.evGx[1277]*acadoWorkspace.QDy[218] + acadoWorkspace.evGx[1283]*acadoWorkspace.QDy[219] + acadoWorkspace.evGx[1289]*acadoWorkspace.QDy[220] + acadoWorkspace.evGx[1295]*acadoWorkspace.QDy[221] + acadoWorkspace.evGx[1301]*acadoWorkspace.QDy[222] + acadoWorkspace.evGx[1307]*acadoWorkspace.QDy[223] + acadoWorkspace.evGx[1313]*acadoWorkspace.QDy[224] + acadoWorkspace.evGx[1319]*acadoWorkspace.QDy[225] + acadoWorkspace.evGx[1325]*acadoWorkspace.QDy[226] + acadoWorkspace.evGx[1331]*acadoWorkspace.QDy[227] + acadoWorkspace.evGx[1337]*acadoWorkspace.QDy[228] + acadoWorkspace.evGx[1343]*acadoWorkspace.QDy[229] + acadoWorkspace.evGx[1349]*acadoWorkspace.QDy[230] + acadoWorkspace.evGx[1355]*acadoWorkspace.QDy[231] + acadoWorkspace.evGx[1361]*acadoWorkspace.QDy[232] + acadoWorkspace.evGx[1367]*acadoWorkspace.QDy[233] + acadoWorkspace.evGx[1373]*acadoWorkspace.QDy[234] + acadoWorkspace.evGx[1379]*acadoWorkspace.QDy[235] + acadoWorkspace.evGx[1385]*acadoWorkspace.QDy[236] + acadoWorkspace.evGx[1391]*acadoWorkspace.QDy[237] + acadoWorkspace.evGx[1397]*acadoWorkspace.QDy[238] + acadoWorkspace.evGx[1403]*acadoWorkspace.QDy[239] + acadoWorkspace.evGx[1409]*acadoWorkspace.QDy[240] + acadoWorkspace.evGx[1415]*acadoWorkspace.QDy[241] + acadoWorkspace.evGx[1421]*acadoWorkspace.QDy[242] + acadoWorkspace.evGx[1427]*acadoWorkspace.QDy[243] + acadoWorkspace.evGx[1433]*acadoWorkspace.QDy[244] + acadoWorkspace.evGx[1439]*acadoWorkspace.QDy[245] + acadoWorkspace.evGx[1445]*acadoWorkspace.QDy[246] + acadoWorkspace.evGx[1451]*acadoWorkspace.QDy[247] + acadoWorkspace.evGx[1457]*acadoWorkspace.QDy[248] + acadoWorkspace.evGx[1463]*acadoWorkspace.QDy[249] + acadoWorkspace.evGx[1469]*acadoWorkspace.QDy[250] + acadoWorkspace.evGx[1475]*acadoWorkspace.QDy[251] + acadoWorkspace.evGx[1481]*acadoWorkspace.QDy[252] + acadoWorkspace.evGx[1487]*acadoWorkspace.QDy[253] + acadoWorkspace.evGx[1493]*acadoWorkspace.QDy[254] + acadoWorkspace.evGx[1499]*acadoWorkspace.QDy[255] + acadoWorkspace.evGx[1505]*acadoWorkspace.QDy[256] + acadoWorkspace.evGx[1511]*acadoWorkspace.QDy[257] + acadoWorkspace.evGx[1517]*acadoWorkspace.QDy[258] + acadoWorkspace.evGx[1523]*acadoWorkspace.QDy[259] + acadoWorkspace.evGx[1529]*acadoWorkspace.QDy[260] + acadoWorkspace.evGx[1535]*acadoWorkspace.QDy[261] + acadoWorkspace.evGx[1541]*acadoWorkspace.QDy[262] + acadoWorkspace.evGx[1547]*acadoWorkspace.QDy[263] + acadoWorkspace.evGx[1553]*acadoWorkspace.QDy[264] + acadoWorkspace.evGx[1559]*acadoWorkspace.QDy[265] + acadoWorkspace.evGx[1565]*acadoWorkspace.QDy[266] + acadoWorkspace.evGx[1571]*acadoWorkspace.QDy[267] + acadoWorkspace.evGx[1577]*acadoWorkspace.QDy[268] + acadoWorkspace.evGx[1583]*acadoWorkspace.QDy[269] + acadoWorkspace.evGx[1589]*acadoWorkspace.QDy[270] + acadoWorkspace.evGx[1595]*acadoWorkspace.QDy[271] + acadoWorkspace.evGx[1601]*acadoWorkspace.QDy[272] + acadoWorkspace.evGx[1607]*acadoWorkspace.QDy[273] + acadoWorkspace.evGx[1613]*acadoWorkspace.QDy[274] + acadoWorkspace.evGx[1619]*acadoWorkspace.QDy[275] + acadoWorkspace.evGx[1625]*acadoWorkspace.QDy[276] + acadoWorkspace.evGx[1631]*acadoWorkspace.QDy[277] + acadoWorkspace.evGx[1637]*acadoWorkspace.QDy[278] + acadoWorkspace.evGx[1643]*acadoWorkspace.QDy[279] + acadoWorkspace.evGx[1649]*acadoWorkspace.QDy[280] + acadoWorkspace.evGx[1655]*acadoWorkspace.QDy[281] + acadoWorkspace.evGx[1661]*acadoWorkspace.QDy[282] + acadoWorkspace.evGx[1667]*acadoWorkspace.QDy[283] + acadoWorkspace.evGx[1673]*acadoWorkspace.QDy[284] + acadoWorkspace.evGx[1679]*acadoWorkspace.QDy[285] + acadoWorkspace.evGx[1685]*acadoWorkspace.QDy[286] + acadoWorkspace.evGx[1691]*acadoWorkspace.QDy[287] + acadoWorkspace.evGx[1697]*acadoWorkspace.QDy[288] + acadoWorkspace.evGx[1703]*acadoWorkspace.QDy[289] + acadoWorkspace.evGx[1709]*acadoWorkspace.QDy[290] + acadoWorkspace.evGx[1715]*acadoWorkspace.QDy[291] + acadoWorkspace.evGx[1721]*acadoWorkspace.QDy[292] + acadoWorkspace.evGx[1727]*acadoWorkspace.QDy[293] + acadoWorkspace.evGx[1733]*acadoWorkspace.QDy[294] + acadoWorkspace.evGx[1739]*acadoWorkspace.QDy[295] + acadoWorkspace.evGx[1745]*acadoWorkspace.QDy[296] + acadoWorkspace.evGx[1751]*acadoWorkspace.QDy[297] + acadoWorkspace.evGx[1757]*acadoWorkspace.QDy[298] + acadoWorkspace.evGx[1763]*acadoWorkspace.QDy[299] + acadoWorkspace.evGx[1769]*acadoWorkspace.QDy[300] + acadoWorkspace.evGx[1775]*acadoWorkspace.QDy[301] + acadoWorkspace.evGx[1781]*acadoWorkspace.QDy[302] + acadoWorkspace.evGx[1787]*acadoWorkspace.QDy[303] + acadoWorkspace.evGx[1793]*acadoWorkspace.QDy[304] + acadoWorkspace.evGx[1799]*acadoWorkspace.QDy[305]; - - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = lRun1; lRun2 < 50; ++lRun2) -{ -lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1); -acado_multEQDy( &(acadoWorkspace.E[ lRun3 * 6 ]), &(acadoWorkspace.QDy[ lRun2 * 6 + 6 ]), &(acadoWorkspace.g[ lRun1 + 6 ]) ); -} -} + +acadoWorkspace.QDy[120] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[121] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[122] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[123] = + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[124] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2]; +acadoWorkspace.QDy[125] = + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[16]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[17]*acadoWorkspace.DyN[2]; + +acadoWorkspace.QDy[6] += acadoWorkspace.Qd[0]; +acadoWorkspace.QDy[7] += acadoWorkspace.Qd[1]; +acadoWorkspace.QDy[8] += acadoWorkspace.Qd[2]; +acadoWorkspace.QDy[9] += acadoWorkspace.Qd[3]; +acadoWorkspace.QDy[10] += acadoWorkspace.Qd[4]; +acadoWorkspace.QDy[11] += acadoWorkspace.Qd[5]; +acadoWorkspace.QDy[12] += acadoWorkspace.Qd[6]; +acadoWorkspace.QDy[13] += acadoWorkspace.Qd[7]; +acadoWorkspace.QDy[14] += acadoWorkspace.Qd[8]; +acadoWorkspace.QDy[15] += acadoWorkspace.Qd[9]; +acadoWorkspace.QDy[16] += acadoWorkspace.Qd[10]; +acadoWorkspace.QDy[17] += acadoWorkspace.Qd[11]; +acadoWorkspace.QDy[18] += acadoWorkspace.Qd[12]; +acadoWorkspace.QDy[19] += acadoWorkspace.Qd[13]; +acadoWorkspace.QDy[20] += acadoWorkspace.Qd[14]; +acadoWorkspace.QDy[21] += acadoWorkspace.Qd[15]; +acadoWorkspace.QDy[22] += acadoWorkspace.Qd[16]; +acadoWorkspace.QDy[23] += acadoWorkspace.Qd[17]; +acadoWorkspace.QDy[24] += acadoWorkspace.Qd[18]; +acadoWorkspace.QDy[25] += acadoWorkspace.Qd[19]; +acadoWorkspace.QDy[26] += acadoWorkspace.Qd[20]; +acadoWorkspace.QDy[27] += acadoWorkspace.Qd[21]; +acadoWorkspace.QDy[28] += acadoWorkspace.Qd[22]; +acadoWorkspace.QDy[29] += acadoWorkspace.Qd[23]; +acadoWorkspace.QDy[30] += acadoWorkspace.Qd[24]; +acadoWorkspace.QDy[31] += acadoWorkspace.Qd[25]; +acadoWorkspace.QDy[32] += acadoWorkspace.Qd[26]; +acadoWorkspace.QDy[33] += acadoWorkspace.Qd[27]; +acadoWorkspace.QDy[34] += acadoWorkspace.Qd[28]; +acadoWorkspace.QDy[35] += acadoWorkspace.Qd[29]; +acadoWorkspace.QDy[36] += acadoWorkspace.Qd[30]; +acadoWorkspace.QDy[37] += acadoWorkspace.Qd[31]; +acadoWorkspace.QDy[38] += acadoWorkspace.Qd[32]; +acadoWorkspace.QDy[39] += acadoWorkspace.Qd[33]; +acadoWorkspace.QDy[40] += acadoWorkspace.Qd[34]; +acadoWorkspace.QDy[41] += acadoWorkspace.Qd[35]; +acadoWorkspace.QDy[42] += acadoWorkspace.Qd[36]; +acadoWorkspace.QDy[43] += acadoWorkspace.Qd[37]; +acadoWorkspace.QDy[44] += acadoWorkspace.Qd[38]; +acadoWorkspace.QDy[45] += acadoWorkspace.Qd[39]; +acadoWorkspace.QDy[46] += acadoWorkspace.Qd[40]; +acadoWorkspace.QDy[47] += acadoWorkspace.Qd[41]; +acadoWorkspace.QDy[48] += acadoWorkspace.Qd[42]; +acadoWorkspace.QDy[49] += acadoWorkspace.Qd[43]; +acadoWorkspace.QDy[50] += acadoWorkspace.Qd[44]; +acadoWorkspace.QDy[51] += acadoWorkspace.Qd[45]; +acadoWorkspace.QDy[52] += acadoWorkspace.Qd[46]; +acadoWorkspace.QDy[53] += acadoWorkspace.Qd[47]; +acadoWorkspace.QDy[54] += acadoWorkspace.Qd[48]; +acadoWorkspace.QDy[55] += acadoWorkspace.Qd[49]; +acadoWorkspace.QDy[56] += acadoWorkspace.Qd[50]; +acadoWorkspace.QDy[57] += acadoWorkspace.Qd[51]; +acadoWorkspace.QDy[58] += acadoWorkspace.Qd[52]; +acadoWorkspace.QDy[59] += acadoWorkspace.Qd[53]; +acadoWorkspace.QDy[60] += acadoWorkspace.Qd[54]; +acadoWorkspace.QDy[61] += acadoWorkspace.Qd[55]; +acadoWorkspace.QDy[62] += acadoWorkspace.Qd[56]; +acadoWorkspace.QDy[63] += acadoWorkspace.Qd[57]; +acadoWorkspace.QDy[64] += acadoWorkspace.Qd[58]; +acadoWorkspace.QDy[65] += acadoWorkspace.Qd[59]; +acadoWorkspace.QDy[66] += acadoWorkspace.Qd[60]; +acadoWorkspace.QDy[67] += acadoWorkspace.Qd[61]; +acadoWorkspace.QDy[68] += acadoWorkspace.Qd[62]; +acadoWorkspace.QDy[69] += acadoWorkspace.Qd[63]; +acadoWorkspace.QDy[70] += acadoWorkspace.Qd[64]; +acadoWorkspace.QDy[71] += acadoWorkspace.Qd[65]; +acadoWorkspace.QDy[72] += acadoWorkspace.Qd[66]; +acadoWorkspace.QDy[73] += acadoWorkspace.Qd[67]; +acadoWorkspace.QDy[74] += acadoWorkspace.Qd[68]; +acadoWorkspace.QDy[75] += acadoWorkspace.Qd[69]; +acadoWorkspace.QDy[76] += acadoWorkspace.Qd[70]; +acadoWorkspace.QDy[77] += acadoWorkspace.Qd[71]; +acadoWorkspace.QDy[78] += acadoWorkspace.Qd[72]; +acadoWorkspace.QDy[79] += acadoWorkspace.Qd[73]; +acadoWorkspace.QDy[80] += acadoWorkspace.Qd[74]; +acadoWorkspace.QDy[81] += acadoWorkspace.Qd[75]; +acadoWorkspace.QDy[82] += acadoWorkspace.Qd[76]; +acadoWorkspace.QDy[83] += acadoWorkspace.Qd[77]; +acadoWorkspace.QDy[84] += acadoWorkspace.Qd[78]; +acadoWorkspace.QDy[85] += acadoWorkspace.Qd[79]; +acadoWorkspace.QDy[86] += acadoWorkspace.Qd[80]; +acadoWorkspace.QDy[87] += acadoWorkspace.Qd[81]; +acadoWorkspace.QDy[88] += acadoWorkspace.Qd[82]; +acadoWorkspace.QDy[89] += acadoWorkspace.Qd[83]; +acadoWorkspace.QDy[90] += acadoWorkspace.Qd[84]; +acadoWorkspace.QDy[91] += acadoWorkspace.Qd[85]; +acadoWorkspace.QDy[92] += acadoWorkspace.Qd[86]; +acadoWorkspace.QDy[93] += acadoWorkspace.Qd[87]; +acadoWorkspace.QDy[94] += acadoWorkspace.Qd[88]; +acadoWorkspace.QDy[95] += acadoWorkspace.Qd[89]; +acadoWorkspace.QDy[96] += acadoWorkspace.Qd[90]; +acadoWorkspace.QDy[97] += acadoWorkspace.Qd[91]; +acadoWorkspace.QDy[98] += acadoWorkspace.Qd[92]; +acadoWorkspace.QDy[99] += acadoWorkspace.Qd[93]; +acadoWorkspace.QDy[100] += acadoWorkspace.Qd[94]; +acadoWorkspace.QDy[101] += acadoWorkspace.Qd[95]; +acadoWorkspace.QDy[102] += acadoWorkspace.Qd[96]; +acadoWorkspace.QDy[103] += acadoWorkspace.Qd[97]; +acadoWorkspace.QDy[104] += acadoWorkspace.Qd[98]; +acadoWorkspace.QDy[105] += acadoWorkspace.Qd[99]; +acadoWorkspace.QDy[106] += acadoWorkspace.Qd[100]; +acadoWorkspace.QDy[107] += acadoWorkspace.Qd[101]; +acadoWorkspace.QDy[108] += acadoWorkspace.Qd[102]; +acadoWorkspace.QDy[109] += acadoWorkspace.Qd[103]; +acadoWorkspace.QDy[110] += acadoWorkspace.Qd[104]; +acadoWorkspace.QDy[111] += acadoWorkspace.Qd[105]; +acadoWorkspace.QDy[112] += acadoWorkspace.Qd[106]; +acadoWorkspace.QDy[113] += acadoWorkspace.Qd[107]; +acadoWorkspace.QDy[114] += acadoWorkspace.Qd[108]; +acadoWorkspace.QDy[115] += acadoWorkspace.Qd[109]; +acadoWorkspace.QDy[116] += acadoWorkspace.Qd[110]; +acadoWorkspace.QDy[117] += acadoWorkspace.Qd[111]; +acadoWorkspace.QDy[118] += acadoWorkspace.Qd[112]; +acadoWorkspace.QDy[119] += acadoWorkspace.Qd[113]; +acadoWorkspace.QDy[120] += acadoWorkspace.Qd[114]; +acadoWorkspace.QDy[121] += acadoWorkspace.Qd[115]; +acadoWorkspace.QDy[122] += acadoWorkspace.Qd[116]; +acadoWorkspace.QDy[123] += acadoWorkspace.Qd[117]; +acadoWorkspace.QDy[124] += acadoWorkspace.Qd[118]; +acadoWorkspace.QDy[125] += acadoWorkspace.Qd[119]; + +acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[516]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[522]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[528]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[534]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[540]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[546]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[552]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[558]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[564]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[570]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[576]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[582]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[588]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[594]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[600]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[606]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[612]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[618]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[624]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[630]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[636]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[642]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[648]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[654]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[660]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[666]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[672]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[678]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[684]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[690]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[696]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[702]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[708]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[714]*acadoWorkspace.QDy[125]; +acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[517]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[523]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[529]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[535]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[541]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[547]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[553]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[559]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[565]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[571]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[577]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[583]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[589]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[595]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[601]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[607]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[613]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[619]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[625]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[631]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[637]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[643]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[649]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[655]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[661]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[667]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[673]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[679]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[685]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[691]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[697]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[703]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[709]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[715]*acadoWorkspace.QDy[125]; +acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[320]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[512]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[518]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[524]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[530]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[536]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[542]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[548]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[554]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[560]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[566]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[572]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[578]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[584]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[590]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[596]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[602]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[608]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[614]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[620]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[626]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[632]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[638]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[644]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[650]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[656]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[662]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[668]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[674]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[680]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[686]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[692]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[698]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[704]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[710]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[716]*acadoWorkspace.QDy[125]; +acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[321]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[513]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[519]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[525]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[531]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[537]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[543]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[549]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[555]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[561]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[567]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[573]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[579]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[585]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[591]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[597]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[603]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[609]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[615]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[621]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[627]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[633]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[639]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[645]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[651]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[657]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[663]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[669]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[675]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[681]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[687]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[693]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[699]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[705]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[711]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[717]*acadoWorkspace.QDy[125]; +acadoWorkspace.g[4] = + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[322]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[514]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[520]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[526]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[532]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[538]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[544]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[550]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[556]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[562]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[568]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[574]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[580]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[586]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[592]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[598]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[604]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[610]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[616]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[622]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[628]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[634]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[640]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[646]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[652]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[658]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[664]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[670]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[676]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[682]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[688]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[694]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[700]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[706]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[712]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[718]*acadoWorkspace.QDy[125]; +acadoWorkspace.g[5] = + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[323]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[515]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[521]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[527]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[533]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[539]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[545]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[551]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[557]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[563]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[569]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[575]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[581]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[587]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[593]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[599]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[605]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[611]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[617]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[623]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[629]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[635]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[641]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[647]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[653]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[659]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[665]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[671]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[677]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[683]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[689]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[695]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[701]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[707]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[713]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[719]*acadoWorkspace.QDy[125]; + + +acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 6 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 7 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 8 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 9 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 10 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 11 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 12 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 13 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 14 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 15 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 16 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 17 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 18 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 19 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 20 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 21 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 22 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 23 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 24 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 24 ]) ); +acado_multEQDy( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 25 ]) ); acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0]; acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1]; @@ -1356,104 +4792,11 @@ acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[121] + acadoWorkspace.d[115]; acadoWorkspace.lbA[19] = - tmp; acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[127] + acadoWorkspace.d[121]; -acadoWorkspace.lbA[20] = - tmp; -acadoWorkspace.ubA[20] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[133] + acadoWorkspace.d[127]; -acadoWorkspace.lbA[21] = - tmp; -acadoWorkspace.ubA[21] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[139] + acadoWorkspace.d[133]; -acadoWorkspace.lbA[22] = - tmp; -acadoWorkspace.ubA[22] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[145] + acadoWorkspace.d[139]; -acadoWorkspace.lbA[23] = - tmp; -acadoWorkspace.ubA[23] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[151] + acadoWorkspace.d[145]; -acadoWorkspace.lbA[24] = - tmp; -acadoWorkspace.ubA[24] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[157] + acadoWorkspace.d[151]; -acadoWorkspace.lbA[25] = - tmp; -acadoWorkspace.ubA[25] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[163] + acadoWorkspace.d[157]; -acadoWorkspace.lbA[26] = - tmp; -acadoWorkspace.ubA[26] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[169] + acadoWorkspace.d[163]; -acadoWorkspace.lbA[27] = - tmp; -acadoWorkspace.ubA[27] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[175] + acadoWorkspace.d[169]; -acadoWorkspace.lbA[28] = - tmp; -acadoWorkspace.ubA[28] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[181] + acadoWorkspace.d[175]; -acadoWorkspace.lbA[29] = - tmp; -acadoWorkspace.ubA[29] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[187] + acadoWorkspace.d[181]; -acadoWorkspace.lbA[30] = - tmp; -acadoWorkspace.ubA[30] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[193] + acadoWorkspace.d[187]; -acadoWorkspace.lbA[31] = - tmp; -acadoWorkspace.ubA[31] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[199] + acadoWorkspace.d[193]; -acadoWorkspace.lbA[32] = - tmp; -acadoWorkspace.ubA[32] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[205] + acadoWorkspace.d[199]; -acadoWorkspace.lbA[33] = - tmp; -acadoWorkspace.ubA[33] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[211] + acadoWorkspace.d[205]; -acadoWorkspace.lbA[34] = - tmp; -acadoWorkspace.ubA[34] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[217] + acadoWorkspace.d[211]; -acadoWorkspace.lbA[35] = - tmp; -acadoWorkspace.ubA[35] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[223] + acadoWorkspace.d[217]; -acadoWorkspace.lbA[36] = - tmp; -acadoWorkspace.ubA[36] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[229] + acadoWorkspace.d[223]; -acadoWorkspace.lbA[37] = - tmp; -acadoWorkspace.ubA[37] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[235] + acadoWorkspace.d[229]; -acadoWorkspace.lbA[38] = - tmp; -acadoWorkspace.ubA[38] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[241] + acadoWorkspace.d[235]; -acadoWorkspace.lbA[39] = - tmp; -acadoWorkspace.ubA[39] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[247] + acadoWorkspace.d[241]; -acadoWorkspace.lbA[40] = - tmp; -acadoWorkspace.ubA[40] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[253] + acadoWorkspace.d[247]; -acadoWorkspace.lbA[41] = - tmp; -acadoWorkspace.ubA[41] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[259] + acadoWorkspace.d[253]; -acadoWorkspace.lbA[42] = - tmp; -acadoWorkspace.ubA[42] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[265] + acadoWorkspace.d[259]; -acadoWorkspace.lbA[43] = - tmp; -acadoWorkspace.ubA[43] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[271] + acadoWorkspace.d[265]; -acadoWorkspace.lbA[44] = - tmp; -acadoWorkspace.ubA[44] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[277] + acadoWorkspace.d[271]; -acadoWorkspace.lbA[45] = - tmp; -acadoWorkspace.ubA[45] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[283] + acadoWorkspace.d[277]; -acadoWorkspace.lbA[46] = - tmp; -acadoWorkspace.ubA[46] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[289] + acadoWorkspace.d[283]; -acadoWorkspace.lbA[47] = - tmp; -acadoWorkspace.ubA[47] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[295] + acadoWorkspace.d[289]; -acadoWorkspace.lbA[48] = - tmp; -acadoWorkspace.ubA[48] = (real_t)1.0000000000000000e+12 - tmp; -tmp = acadoVariables.x[301] + acadoWorkspace.d[295]; -acadoWorkspace.lbA[49] = - tmp; -acadoWorkspace.ubA[49] = (real_t)1.0000000000000000e+12 - tmp; } void acado_expand( ) { -int lRun1; -int lRun2; -int lRun3; acadoVariables.x[0] += acadoWorkspace.x[0]; acadoVariables.x[1] += acadoWorkspace.x[1]; acadoVariables.x[2] += acadoWorkspace.x[2]; @@ -1481,36 +4824,6 @@ acadoVariables.u[16] += acadoWorkspace.x[22]; acadoVariables.u[17] += acadoWorkspace.x[23]; acadoVariables.u[18] += acadoWorkspace.x[24]; acadoVariables.u[19] += acadoWorkspace.x[25]; -acadoVariables.u[20] += acadoWorkspace.x[26]; -acadoVariables.u[21] += acadoWorkspace.x[27]; -acadoVariables.u[22] += acadoWorkspace.x[28]; -acadoVariables.u[23] += acadoWorkspace.x[29]; -acadoVariables.u[24] += acadoWorkspace.x[30]; -acadoVariables.u[25] += acadoWorkspace.x[31]; -acadoVariables.u[26] += acadoWorkspace.x[32]; -acadoVariables.u[27] += acadoWorkspace.x[33]; -acadoVariables.u[28] += acadoWorkspace.x[34]; -acadoVariables.u[29] += acadoWorkspace.x[35]; -acadoVariables.u[30] += acadoWorkspace.x[36]; -acadoVariables.u[31] += acadoWorkspace.x[37]; -acadoVariables.u[32] += acadoWorkspace.x[38]; -acadoVariables.u[33] += acadoWorkspace.x[39]; -acadoVariables.u[34] += acadoWorkspace.x[40]; -acadoVariables.u[35] += acadoWorkspace.x[41]; -acadoVariables.u[36] += acadoWorkspace.x[42]; -acadoVariables.u[37] += acadoWorkspace.x[43]; -acadoVariables.u[38] += acadoWorkspace.x[44]; -acadoVariables.u[39] += acadoWorkspace.x[45]; -acadoVariables.u[40] += acadoWorkspace.x[46]; -acadoVariables.u[41] += acadoWorkspace.x[47]; -acadoVariables.u[42] += acadoWorkspace.x[48]; -acadoVariables.u[43] += acadoWorkspace.x[49]; -acadoVariables.u[44] += acadoWorkspace.x[50]; -acadoVariables.u[45] += acadoWorkspace.x[51]; -acadoVariables.u[46] += acadoWorkspace.x[52]; -acadoVariables.u[47] += acadoWorkspace.x[53]; -acadoVariables.u[48] += acadoWorkspace.x[54]; -acadoVariables.u[49] += acadoWorkspace.x[55]; acadoVariables.x[6] += + 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.evGx[4]*acadoWorkspace.x[4] + acadoWorkspace.evGx[5]*acadoWorkspace.x[5] + acadoWorkspace.d[0]; acadoVariables.x[7] += + acadoWorkspace.evGx[6]*acadoWorkspace.x[0] + acadoWorkspace.evGx[7]*acadoWorkspace.x[1] + acadoWorkspace.evGx[8]*acadoWorkspace.x[2] + acadoWorkspace.evGx[9]*acadoWorkspace.x[3] + acadoWorkspace.evGx[10]*acadoWorkspace.x[4] + acadoWorkspace.evGx[11]*acadoWorkspace.x[5] + acadoWorkspace.d[1]; @@ -1632,195 +4945,217 @@ acadoVariables.x[122] += + acadoWorkspace.evGx[696]*acadoWorkspace.x[0] + acadoW acadoVariables.x[123] += + acadoWorkspace.evGx[702]*acadoWorkspace.x[0] + acadoWorkspace.evGx[703]*acadoWorkspace.x[1] + acadoWorkspace.evGx[704]*acadoWorkspace.x[2] + acadoWorkspace.evGx[705]*acadoWorkspace.x[3] + acadoWorkspace.evGx[706]*acadoWorkspace.x[4] + acadoWorkspace.evGx[707]*acadoWorkspace.x[5] + acadoWorkspace.d[117]; acadoVariables.x[124] += + acadoWorkspace.evGx[708]*acadoWorkspace.x[0] + acadoWorkspace.evGx[709]*acadoWorkspace.x[1] + acadoWorkspace.evGx[710]*acadoWorkspace.x[2] + acadoWorkspace.evGx[711]*acadoWorkspace.x[3] + acadoWorkspace.evGx[712]*acadoWorkspace.x[4] + acadoWorkspace.evGx[713]*acadoWorkspace.x[5] + acadoWorkspace.d[118]; acadoVariables.x[125] += + acadoWorkspace.evGx[714]*acadoWorkspace.x[0] + acadoWorkspace.evGx[715]*acadoWorkspace.x[1] + acadoWorkspace.evGx[716]*acadoWorkspace.x[2] + acadoWorkspace.evGx[717]*acadoWorkspace.x[3] + acadoWorkspace.evGx[718]*acadoWorkspace.x[4] + acadoWorkspace.evGx[719]*acadoWorkspace.x[5] + acadoWorkspace.d[119]; -acadoVariables.x[126] += + acadoWorkspace.evGx[720]*acadoWorkspace.x[0] + acadoWorkspace.evGx[721]*acadoWorkspace.x[1] + acadoWorkspace.evGx[722]*acadoWorkspace.x[2] + acadoWorkspace.evGx[723]*acadoWorkspace.x[3] + acadoWorkspace.evGx[724]*acadoWorkspace.x[4] + acadoWorkspace.evGx[725]*acadoWorkspace.x[5] + acadoWorkspace.d[120]; -acadoVariables.x[127] += + acadoWorkspace.evGx[726]*acadoWorkspace.x[0] + acadoWorkspace.evGx[727]*acadoWorkspace.x[1] + acadoWorkspace.evGx[728]*acadoWorkspace.x[2] + acadoWorkspace.evGx[729]*acadoWorkspace.x[3] + acadoWorkspace.evGx[730]*acadoWorkspace.x[4] + acadoWorkspace.evGx[731]*acadoWorkspace.x[5] + acadoWorkspace.d[121]; -acadoVariables.x[128] += + acadoWorkspace.evGx[732]*acadoWorkspace.x[0] + acadoWorkspace.evGx[733]*acadoWorkspace.x[1] + acadoWorkspace.evGx[734]*acadoWorkspace.x[2] + acadoWorkspace.evGx[735]*acadoWorkspace.x[3] + acadoWorkspace.evGx[736]*acadoWorkspace.x[4] + acadoWorkspace.evGx[737]*acadoWorkspace.x[5] + acadoWorkspace.d[122]; -acadoVariables.x[129] += + acadoWorkspace.evGx[738]*acadoWorkspace.x[0] + acadoWorkspace.evGx[739]*acadoWorkspace.x[1] + acadoWorkspace.evGx[740]*acadoWorkspace.x[2] + acadoWorkspace.evGx[741]*acadoWorkspace.x[3] + acadoWorkspace.evGx[742]*acadoWorkspace.x[4] + acadoWorkspace.evGx[743]*acadoWorkspace.x[5] + acadoWorkspace.d[123]; -acadoVariables.x[130] += + acadoWorkspace.evGx[744]*acadoWorkspace.x[0] + acadoWorkspace.evGx[745]*acadoWorkspace.x[1] + acadoWorkspace.evGx[746]*acadoWorkspace.x[2] + acadoWorkspace.evGx[747]*acadoWorkspace.x[3] + acadoWorkspace.evGx[748]*acadoWorkspace.x[4] + acadoWorkspace.evGx[749]*acadoWorkspace.x[5] + acadoWorkspace.d[124]; -acadoVariables.x[131] += + acadoWorkspace.evGx[750]*acadoWorkspace.x[0] + acadoWorkspace.evGx[751]*acadoWorkspace.x[1] + acadoWorkspace.evGx[752]*acadoWorkspace.x[2] + acadoWorkspace.evGx[753]*acadoWorkspace.x[3] + acadoWorkspace.evGx[754]*acadoWorkspace.x[4] + acadoWorkspace.evGx[755]*acadoWorkspace.x[5] + acadoWorkspace.d[125]; -acadoVariables.x[132] += + acadoWorkspace.evGx[756]*acadoWorkspace.x[0] + acadoWorkspace.evGx[757]*acadoWorkspace.x[1] + acadoWorkspace.evGx[758]*acadoWorkspace.x[2] + acadoWorkspace.evGx[759]*acadoWorkspace.x[3] + acadoWorkspace.evGx[760]*acadoWorkspace.x[4] + acadoWorkspace.evGx[761]*acadoWorkspace.x[5] + acadoWorkspace.d[126]; -acadoVariables.x[133] += + acadoWorkspace.evGx[762]*acadoWorkspace.x[0] + acadoWorkspace.evGx[763]*acadoWorkspace.x[1] + acadoWorkspace.evGx[764]*acadoWorkspace.x[2] + acadoWorkspace.evGx[765]*acadoWorkspace.x[3] + acadoWorkspace.evGx[766]*acadoWorkspace.x[4] + acadoWorkspace.evGx[767]*acadoWorkspace.x[5] + acadoWorkspace.d[127]; -acadoVariables.x[134] += + acadoWorkspace.evGx[768]*acadoWorkspace.x[0] + acadoWorkspace.evGx[769]*acadoWorkspace.x[1] + acadoWorkspace.evGx[770]*acadoWorkspace.x[2] + acadoWorkspace.evGx[771]*acadoWorkspace.x[3] + acadoWorkspace.evGx[772]*acadoWorkspace.x[4] + acadoWorkspace.evGx[773]*acadoWorkspace.x[5] + acadoWorkspace.d[128]; -acadoVariables.x[135] += + acadoWorkspace.evGx[774]*acadoWorkspace.x[0] + acadoWorkspace.evGx[775]*acadoWorkspace.x[1] + acadoWorkspace.evGx[776]*acadoWorkspace.x[2] + acadoWorkspace.evGx[777]*acadoWorkspace.x[3] + acadoWorkspace.evGx[778]*acadoWorkspace.x[4] + acadoWorkspace.evGx[779]*acadoWorkspace.x[5] + acadoWorkspace.d[129]; -acadoVariables.x[136] += + acadoWorkspace.evGx[780]*acadoWorkspace.x[0] + acadoWorkspace.evGx[781]*acadoWorkspace.x[1] + acadoWorkspace.evGx[782]*acadoWorkspace.x[2] + acadoWorkspace.evGx[783]*acadoWorkspace.x[3] + acadoWorkspace.evGx[784]*acadoWorkspace.x[4] + acadoWorkspace.evGx[785]*acadoWorkspace.x[5] + acadoWorkspace.d[130]; -acadoVariables.x[137] += + acadoWorkspace.evGx[786]*acadoWorkspace.x[0] + acadoWorkspace.evGx[787]*acadoWorkspace.x[1] + acadoWorkspace.evGx[788]*acadoWorkspace.x[2] + acadoWorkspace.evGx[789]*acadoWorkspace.x[3] + acadoWorkspace.evGx[790]*acadoWorkspace.x[4] + acadoWorkspace.evGx[791]*acadoWorkspace.x[5] + acadoWorkspace.d[131]; -acadoVariables.x[138] += + acadoWorkspace.evGx[792]*acadoWorkspace.x[0] + acadoWorkspace.evGx[793]*acadoWorkspace.x[1] + acadoWorkspace.evGx[794]*acadoWorkspace.x[2] + acadoWorkspace.evGx[795]*acadoWorkspace.x[3] + acadoWorkspace.evGx[796]*acadoWorkspace.x[4] + acadoWorkspace.evGx[797]*acadoWorkspace.x[5] + acadoWorkspace.d[132]; -acadoVariables.x[139] += + acadoWorkspace.evGx[798]*acadoWorkspace.x[0] + acadoWorkspace.evGx[799]*acadoWorkspace.x[1] + acadoWorkspace.evGx[800]*acadoWorkspace.x[2] + acadoWorkspace.evGx[801]*acadoWorkspace.x[3] + acadoWorkspace.evGx[802]*acadoWorkspace.x[4] + acadoWorkspace.evGx[803]*acadoWorkspace.x[5] + acadoWorkspace.d[133]; -acadoVariables.x[140] += + acadoWorkspace.evGx[804]*acadoWorkspace.x[0] + acadoWorkspace.evGx[805]*acadoWorkspace.x[1] + acadoWorkspace.evGx[806]*acadoWorkspace.x[2] + acadoWorkspace.evGx[807]*acadoWorkspace.x[3] + acadoWorkspace.evGx[808]*acadoWorkspace.x[4] + acadoWorkspace.evGx[809]*acadoWorkspace.x[5] + acadoWorkspace.d[134]; -acadoVariables.x[141] += + acadoWorkspace.evGx[810]*acadoWorkspace.x[0] + acadoWorkspace.evGx[811]*acadoWorkspace.x[1] + acadoWorkspace.evGx[812]*acadoWorkspace.x[2] + acadoWorkspace.evGx[813]*acadoWorkspace.x[3] + acadoWorkspace.evGx[814]*acadoWorkspace.x[4] + acadoWorkspace.evGx[815]*acadoWorkspace.x[5] + acadoWorkspace.d[135]; -acadoVariables.x[142] += + acadoWorkspace.evGx[816]*acadoWorkspace.x[0] + acadoWorkspace.evGx[817]*acadoWorkspace.x[1] + acadoWorkspace.evGx[818]*acadoWorkspace.x[2] + acadoWorkspace.evGx[819]*acadoWorkspace.x[3] + acadoWorkspace.evGx[820]*acadoWorkspace.x[4] + acadoWorkspace.evGx[821]*acadoWorkspace.x[5] + acadoWorkspace.d[136]; -acadoVariables.x[143] += + acadoWorkspace.evGx[822]*acadoWorkspace.x[0] + acadoWorkspace.evGx[823]*acadoWorkspace.x[1] + acadoWorkspace.evGx[824]*acadoWorkspace.x[2] + acadoWorkspace.evGx[825]*acadoWorkspace.x[3] + acadoWorkspace.evGx[826]*acadoWorkspace.x[4] + acadoWorkspace.evGx[827]*acadoWorkspace.x[5] + acadoWorkspace.d[137]; -acadoVariables.x[144] += + acadoWorkspace.evGx[828]*acadoWorkspace.x[0] + acadoWorkspace.evGx[829]*acadoWorkspace.x[1] + acadoWorkspace.evGx[830]*acadoWorkspace.x[2] + acadoWorkspace.evGx[831]*acadoWorkspace.x[3] + acadoWorkspace.evGx[832]*acadoWorkspace.x[4] + acadoWorkspace.evGx[833]*acadoWorkspace.x[5] + acadoWorkspace.d[138]; -acadoVariables.x[145] += + acadoWorkspace.evGx[834]*acadoWorkspace.x[0] + acadoWorkspace.evGx[835]*acadoWorkspace.x[1] + acadoWorkspace.evGx[836]*acadoWorkspace.x[2] + acadoWorkspace.evGx[837]*acadoWorkspace.x[3] + acadoWorkspace.evGx[838]*acadoWorkspace.x[4] + acadoWorkspace.evGx[839]*acadoWorkspace.x[5] + acadoWorkspace.d[139]; -acadoVariables.x[146] += + acadoWorkspace.evGx[840]*acadoWorkspace.x[0] + acadoWorkspace.evGx[841]*acadoWorkspace.x[1] + acadoWorkspace.evGx[842]*acadoWorkspace.x[2] + acadoWorkspace.evGx[843]*acadoWorkspace.x[3] + acadoWorkspace.evGx[844]*acadoWorkspace.x[4] + acadoWorkspace.evGx[845]*acadoWorkspace.x[5] + acadoWorkspace.d[140]; -acadoVariables.x[147] += + acadoWorkspace.evGx[846]*acadoWorkspace.x[0] + acadoWorkspace.evGx[847]*acadoWorkspace.x[1] + acadoWorkspace.evGx[848]*acadoWorkspace.x[2] + acadoWorkspace.evGx[849]*acadoWorkspace.x[3] + acadoWorkspace.evGx[850]*acadoWorkspace.x[4] + acadoWorkspace.evGx[851]*acadoWorkspace.x[5] + acadoWorkspace.d[141]; -acadoVariables.x[148] += + acadoWorkspace.evGx[852]*acadoWorkspace.x[0] + acadoWorkspace.evGx[853]*acadoWorkspace.x[1] + acadoWorkspace.evGx[854]*acadoWorkspace.x[2] + acadoWorkspace.evGx[855]*acadoWorkspace.x[3] + acadoWorkspace.evGx[856]*acadoWorkspace.x[4] + acadoWorkspace.evGx[857]*acadoWorkspace.x[5] + acadoWorkspace.d[142]; -acadoVariables.x[149] += + acadoWorkspace.evGx[858]*acadoWorkspace.x[0] + acadoWorkspace.evGx[859]*acadoWorkspace.x[1] + acadoWorkspace.evGx[860]*acadoWorkspace.x[2] + acadoWorkspace.evGx[861]*acadoWorkspace.x[3] + acadoWorkspace.evGx[862]*acadoWorkspace.x[4] + acadoWorkspace.evGx[863]*acadoWorkspace.x[5] + acadoWorkspace.d[143]; -acadoVariables.x[150] += + acadoWorkspace.evGx[864]*acadoWorkspace.x[0] + acadoWorkspace.evGx[865]*acadoWorkspace.x[1] + acadoWorkspace.evGx[866]*acadoWorkspace.x[2] + acadoWorkspace.evGx[867]*acadoWorkspace.x[3] + acadoWorkspace.evGx[868]*acadoWorkspace.x[4] + acadoWorkspace.evGx[869]*acadoWorkspace.x[5] + acadoWorkspace.d[144]; -acadoVariables.x[151] += + acadoWorkspace.evGx[870]*acadoWorkspace.x[0] + acadoWorkspace.evGx[871]*acadoWorkspace.x[1] + acadoWorkspace.evGx[872]*acadoWorkspace.x[2] + acadoWorkspace.evGx[873]*acadoWorkspace.x[3] + acadoWorkspace.evGx[874]*acadoWorkspace.x[4] + acadoWorkspace.evGx[875]*acadoWorkspace.x[5] + acadoWorkspace.d[145]; -acadoVariables.x[152] += + acadoWorkspace.evGx[876]*acadoWorkspace.x[0] + acadoWorkspace.evGx[877]*acadoWorkspace.x[1] + acadoWorkspace.evGx[878]*acadoWorkspace.x[2] + acadoWorkspace.evGx[879]*acadoWorkspace.x[3] + acadoWorkspace.evGx[880]*acadoWorkspace.x[4] + acadoWorkspace.evGx[881]*acadoWorkspace.x[5] + acadoWorkspace.d[146]; -acadoVariables.x[153] += + acadoWorkspace.evGx[882]*acadoWorkspace.x[0] + acadoWorkspace.evGx[883]*acadoWorkspace.x[1] + acadoWorkspace.evGx[884]*acadoWorkspace.x[2] + acadoWorkspace.evGx[885]*acadoWorkspace.x[3] + acadoWorkspace.evGx[886]*acadoWorkspace.x[4] + acadoWorkspace.evGx[887]*acadoWorkspace.x[5] + acadoWorkspace.d[147]; -acadoVariables.x[154] += + acadoWorkspace.evGx[888]*acadoWorkspace.x[0] + acadoWorkspace.evGx[889]*acadoWorkspace.x[1] + acadoWorkspace.evGx[890]*acadoWorkspace.x[2] + acadoWorkspace.evGx[891]*acadoWorkspace.x[3] + acadoWorkspace.evGx[892]*acadoWorkspace.x[4] + acadoWorkspace.evGx[893]*acadoWorkspace.x[5] + acadoWorkspace.d[148]; -acadoVariables.x[155] += + acadoWorkspace.evGx[894]*acadoWorkspace.x[0] + acadoWorkspace.evGx[895]*acadoWorkspace.x[1] + acadoWorkspace.evGx[896]*acadoWorkspace.x[2] + acadoWorkspace.evGx[897]*acadoWorkspace.x[3] + acadoWorkspace.evGx[898]*acadoWorkspace.x[4] + acadoWorkspace.evGx[899]*acadoWorkspace.x[5] + acadoWorkspace.d[149]; -acadoVariables.x[156] += + acadoWorkspace.evGx[900]*acadoWorkspace.x[0] + acadoWorkspace.evGx[901]*acadoWorkspace.x[1] + acadoWorkspace.evGx[902]*acadoWorkspace.x[2] + acadoWorkspace.evGx[903]*acadoWorkspace.x[3] + acadoWorkspace.evGx[904]*acadoWorkspace.x[4] + acadoWorkspace.evGx[905]*acadoWorkspace.x[5] + acadoWorkspace.d[150]; -acadoVariables.x[157] += + acadoWorkspace.evGx[906]*acadoWorkspace.x[0] + acadoWorkspace.evGx[907]*acadoWorkspace.x[1] + acadoWorkspace.evGx[908]*acadoWorkspace.x[2] + acadoWorkspace.evGx[909]*acadoWorkspace.x[3] + acadoWorkspace.evGx[910]*acadoWorkspace.x[4] + acadoWorkspace.evGx[911]*acadoWorkspace.x[5] + acadoWorkspace.d[151]; -acadoVariables.x[158] += + acadoWorkspace.evGx[912]*acadoWorkspace.x[0] + acadoWorkspace.evGx[913]*acadoWorkspace.x[1] + acadoWorkspace.evGx[914]*acadoWorkspace.x[2] + acadoWorkspace.evGx[915]*acadoWorkspace.x[3] + acadoWorkspace.evGx[916]*acadoWorkspace.x[4] + acadoWorkspace.evGx[917]*acadoWorkspace.x[5] + acadoWorkspace.d[152]; -acadoVariables.x[159] += + acadoWorkspace.evGx[918]*acadoWorkspace.x[0] + acadoWorkspace.evGx[919]*acadoWorkspace.x[1] + acadoWorkspace.evGx[920]*acadoWorkspace.x[2] + acadoWorkspace.evGx[921]*acadoWorkspace.x[3] + acadoWorkspace.evGx[922]*acadoWorkspace.x[4] + acadoWorkspace.evGx[923]*acadoWorkspace.x[5] + acadoWorkspace.d[153]; -acadoVariables.x[160] += + acadoWorkspace.evGx[924]*acadoWorkspace.x[0] + acadoWorkspace.evGx[925]*acadoWorkspace.x[1] + acadoWorkspace.evGx[926]*acadoWorkspace.x[2] + acadoWorkspace.evGx[927]*acadoWorkspace.x[3] + acadoWorkspace.evGx[928]*acadoWorkspace.x[4] + acadoWorkspace.evGx[929]*acadoWorkspace.x[5] + acadoWorkspace.d[154]; -acadoVariables.x[161] += + acadoWorkspace.evGx[930]*acadoWorkspace.x[0] + acadoWorkspace.evGx[931]*acadoWorkspace.x[1] + acadoWorkspace.evGx[932]*acadoWorkspace.x[2] + acadoWorkspace.evGx[933]*acadoWorkspace.x[3] + acadoWorkspace.evGx[934]*acadoWorkspace.x[4] + acadoWorkspace.evGx[935]*acadoWorkspace.x[5] + acadoWorkspace.d[155]; -acadoVariables.x[162] += + acadoWorkspace.evGx[936]*acadoWorkspace.x[0] + acadoWorkspace.evGx[937]*acadoWorkspace.x[1] + acadoWorkspace.evGx[938]*acadoWorkspace.x[2] + acadoWorkspace.evGx[939]*acadoWorkspace.x[3] + acadoWorkspace.evGx[940]*acadoWorkspace.x[4] + acadoWorkspace.evGx[941]*acadoWorkspace.x[5] + acadoWorkspace.d[156]; -acadoVariables.x[163] += + acadoWorkspace.evGx[942]*acadoWorkspace.x[0] + acadoWorkspace.evGx[943]*acadoWorkspace.x[1] + acadoWorkspace.evGx[944]*acadoWorkspace.x[2] + acadoWorkspace.evGx[945]*acadoWorkspace.x[3] + acadoWorkspace.evGx[946]*acadoWorkspace.x[4] + acadoWorkspace.evGx[947]*acadoWorkspace.x[5] + acadoWorkspace.d[157]; -acadoVariables.x[164] += + acadoWorkspace.evGx[948]*acadoWorkspace.x[0] + acadoWorkspace.evGx[949]*acadoWorkspace.x[1] + acadoWorkspace.evGx[950]*acadoWorkspace.x[2] + acadoWorkspace.evGx[951]*acadoWorkspace.x[3] + acadoWorkspace.evGx[952]*acadoWorkspace.x[4] + acadoWorkspace.evGx[953]*acadoWorkspace.x[5] + acadoWorkspace.d[158]; -acadoVariables.x[165] += + acadoWorkspace.evGx[954]*acadoWorkspace.x[0] + acadoWorkspace.evGx[955]*acadoWorkspace.x[1] + acadoWorkspace.evGx[956]*acadoWorkspace.x[2] + acadoWorkspace.evGx[957]*acadoWorkspace.x[3] + acadoWorkspace.evGx[958]*acadoWorkspace.x[4] + acadoWorkspace.evGx[959]*acadoWorkspace.x[5] + acadoWorkspace.d[159]; -acadoVariables.x[166] += + acadoWorkspace.evGx[960]*acadoWorkspace.x[0] + acadoWorkspace.evGx[961]*acadoWorkspace.x[1] + acadoWorkspace.evGx[962]*acadoWorkspace.x[2] + acadoWorkspace.evGx[963]*acadoWorkspace.x[3] + acadoWorkspace.evGx[964]*acadoWorkspace.x[4] + acadoWorkspace.evGx[965]*acadoWorkspace.x[5] + acadoWorkspace.d[160]; -acadoVariables.x[167] += + acadoWorkspace.evGx[966]*acadoWorkspace.x[0] + acadoWorkspace.evGx[967]*acadoWorkspace.x[1] + acadoWorkspace.evGx[968]*acadoWorkspace.x[2] + acadoWorkspace.evGx[969]*acadoWorkspace.x[3] + acadoWorkspace.evGx[970]*acadoWorkspace.x[4] + acadoWorkspace.evGx[971]*acadoWorkspace.x[5] + acadoWorkspace.d[161]; -acadoVariables.x[168] += + acadoWorkspace.evGx[972]*acadoWorkspace.x[0] + acadoWorkspace.evGx[973]*acadoWorkspace.x[1] + acadoWorkspace.evGx[974]*acadoWorkspace.x[2] + acadoWorkspace.evGx[975]*acadoWorkspace.x[3] + acadoWorkspace.evGx[976]*acadoWorkspace.x[4] + acadoWorkspace.evGx[977]*acadoWorkspace.x[5] + acadoWorkspace.d[162]; -acadoVariables.x[169] += + acadoWorkspace.evGx[978]*acadoWorkspace.x[0] + acadoWorkspace.evGx[979]*acadoWorkspace.x[1] + acadoWorkspace.evGx[980]*acadoWorkspace.x[2] + acadoWorkspace.evGx[981]*acadoWorkspace.x[3] + acadoWorkspace.evGx[982]*acadoWorkspace.x[4] + acadoWorkspace.evGx[983]*acadoWorkspace.x[5] + acadoWorkspace.d[163]; -acadoVariables.x[170] += + acadoWorkspace.evGx[984]*acadoWorkspace.x[0] + acadoWorkspace.evGx[985]*acadoWorkspace.x[1] + acadoWorkspace.evGx[986]*acadoWorkspace.x[2] + acadoWorkspace.evGx[987]*acadoWorkspace.x[3] + acadoWorkspace.evGx[988]*acadoWorkspace.x[4] + acadoWorkspace.evGx[989]*acadoWorkspace.x[5] + acadoWorkspace.d[164]; -acadoVariables.x[171] += + acadoWorkspace.evGx[990]*acadoWorkspace.x[0] + acadoWorkspace.evGx[991]*acadoWorkspace.x[1] + acadoWorkspace.evGx[992]*acadoWorkspace.x[2] + acadoWorkspace.evGx[993]*acadoWorkspace.x[3] + acadoWorkspace.evGx[994]*acadoWorkspace.x[4] + acadoWorkspace.evGx[995]*acadoWorkspace.x[5] + acadoWorkspace.d[165]; -acadoVariables.x[172] += + acadoWorkspace.evGx[996]*acadoWorkspace.x[0] + acadoWorkspace.evGx[997]*acadoWorkspace.x[1] + acadoWorkspace.evGx[998]*acadoWorkspace.x[2] + acadoWorkspace.evGx[999]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1000]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1001]*acadoWorkspace.x[5] + acadoWorkspace.d[166]; -acadoVariables.x[173] += + acadoWorkspace.evGx[1002]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1003]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1004]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1005]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1006]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1007]*acadoWorkspace.x[5] + acadoWorkspace.d[167]; -acadoVariables.x[174] += + acadoWorkspace.evGx[1008]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1009]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1010]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1011]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1012]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1013]*acadoWorkspace.x[5] + acadoWorkspace.d[168]; -acadoVariables.x[175] += + acadoWorkspace.evGx[1014]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1015]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1016]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1017]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1018]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1019]*acadoWorkspace.x[5] + acadoWorkspace.d[169]; -acadoVariables.x[176] += + acadoWorkspace.evGx[1020]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1021]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1022]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1023]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1024]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1025]*acadoWorkspace.x[5] + acadoWorkspace.d[170]; -acadoVariables.x[177] += + acadoWorkspace.evGx[1026]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1027]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1028]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1029]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1030]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1031]*acadoWorkspace.x[5] + acadoWorkspace.d[171]; -acadoVariables.x[178] += + acadoWorkspace.evGx[1032]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1033]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1034]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1035]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1036]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1037]*acadoWorkspace.x[5] + acadoWorkspace.d[172]; -acadoVariables.x[179] += + acadoWorkspace.evGx[1038]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1039]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1040]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1041]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1042]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1043]*acadoWorkspace.x[5] + acadoWorkspace.d[173]; -acadoVariables.x[180] += + acadoWorkspace.evGx[1044]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1045]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1046]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1047]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1048]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1049]*acadoWorkspace.x[5] + acadoWorkspace.d[174]; -acadoVariables.x[181] += + acadoWorkspace.evGx[1050]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1051]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1052]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1053]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1054]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1055]*acadoWorkspace.x[5] + acadoWorkspace.d[175]; -acadoVariables.x[182] += + acadoWorkspace.evGx[1056]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1057]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1058]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1059]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1060]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1061]*acadoWorkspace.x[5] + acadoWorkspace.d[176]; -acadoVariables.x[183] += + acadoWorkspace.evGx[1062]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1063]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1064]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1065]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1066]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1067]*acadoWorkspace.x[5] + acadoWorkspace.d[177]; -acadoVariables.x[184] += + acadoWorkspace.evGx[1068]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1069]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1070]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1071]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1072]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1073]*acadoWorkspace.x[5] + acadoWorkspace.d[178]; -acadoVariables.x[185] += + acadoWorkspace.evGx[1074]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1075]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1076]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1077]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1078]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1079]*acadoWorkspace.x[5] + acadoWorkspace.d[179]; -acadoVariables.x[186] += + acadoWorkspace.evGx[1080]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1081]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1082]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1083]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1084]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1085]*acadoWorkspace.x[5] + acadoWorkspace.d[180]; -acadoVariables.x[187] += + acadoWorkspace.evGx[1086]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1087]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1088]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1089]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1090]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1091]*acadoWorkspace.x[5] + acadoWorkspace.d[181]; -acadoVariables.x[188] += + acadoWorkspace.evGx[1092]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1093]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1094]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1095]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1096]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1097]*acadoWorkspace.x[5] + acadoWorkspace.d[182]; -acadoVariables.x[189] += + acadoWorkspace.evGx[1098]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1099]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1100]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1101]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1102]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1103]*acadoWorkspace.x[5] + acadoWorkspace.d[183]; -acadoVariables.x[190] += + acadoWorkspace.evGx[1104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1107]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1108]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1109]*acadoWorkspace.x[5] + acadoWorkspace.d[184]; -acadoVariables.x[191] += + acadoWorkspace.evGx[1110]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1111]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1112]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1113]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1114]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1115]*acadoWorkspace.x[5] + acadoWorkspace.d[185]; -acadoVariables.x[192] += + acadoWorkspace.evGx[1116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1119]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1120]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1121]*acadoWorkspace.x[5] + acadoWorkspace.d[186]; -acadoVariables.x[193] += + acadoWorkspace.evGx[1122]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1123]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1124]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1125]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1126]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1127]*acadoWorkspace.x[5] + acadoWorkspace.d[187]; -acadoVariables.x[194] += + acadoWorkspace.evGx[1128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1131]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1132]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1133]*acadoWorkspace.x[5] + acadoWorkspace.d[188]; -acadoVariables.x[195] += + acadoWorkspace.evGx[1134]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1135]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1136]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1137]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1138]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1139]*acadoWorkspace.x[5] + acadoWorkspace.d[189]; -acadoVariables.x[196] += + acadoWorkspace.evGx[1140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1143]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1144]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1145]*acadoWorkspace.x[5] + acadoWorkspace.d[190]; -acadoVariables.x[197] += + acadoWorkspace.evGx[1146]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1147]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1148]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1149]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1150]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1151]*acadoWorkspace.x[5] + acadoWorkspace.d[191]; -acadoVariables.x[198] += + acadoWorkspace.evGx[1152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1155]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1156]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1157]*acadoWorkspace.x[5] + acadoWorkspace.d[192]; -acadoVariables.x[199] += + acadoWorkspace.evGx[1158]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1159]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1160]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1161]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1162]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1163]*acadoWorkspace.x[5] + acadoWorkspace.d[193]; -acadoVariables.x[200] += + acadoWorkspace.evGx[1164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1167]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1168]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1169]*acadoWorkspace.x[5] + acadoWorkspace.d[194]; -acadoVariables.x[201] += + acadoWorkspace.evGx[1170]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1171]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1172]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1173]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1174]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1175]*acadoWorkspace.x[5] + acadoWorkspace.d[195]; -acadoVariables.x[202] += + acadoWorkspace.evGx[1176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1179]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1180]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1181]*acadoWorkspace.x[5] + acadoWorkspace.d[196]; -acadoVariables.x[203] += + acadoWorkspace.evGx[1182]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1183]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1184]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1185]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1186]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1187]*acadoWorkspace.x[5] + acadoWorkspace.d[197]; -acadoVariables.x[204] += + acadoWorkspace.evGx[1188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1191]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1192]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1193]*acadoWorkspace.x[5] + acadoWorkspace.d[198]; -acadoVariables.x[205] += + acadoWorkspace.evGx[1194]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1195]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1196]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1197]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1198]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1199]*acadoWorkspace.x[5] + acadoWorkspace.d[199]; -acadoVariables.x[206] += + acadoWorkspace.evGx[1200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1203]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1204]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1205]*acadoWorkspace.x[5] + acadoWorkspace.d[200]; -acadoVariables.x[207] += + acadoWorkspace.evGx[1206]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1207]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1208]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1209]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1210]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1211]*acadoWorkspace.x[5] + acadoWorkspace.d[201]; -acadoVariables.x[208] += + acadoWorkspace.evGx[1212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1215]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1216]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1217]*acadoWorkspace.x[5] + acadoWorkspace.d[202]; -acadoVariables.x[209] += + acadoWorkspace.evGx[1218]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1219]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1220]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1221]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1222]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1223]*acadoWorkspace.x[5] + acadoWorkspace.d[203]; -acadoVariables.x[210] += + acadoWorkspace.evGx[1224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1227]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1228]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1229]*acadoWorkspace.x[5] + acadoWorkspace.d[204]; -acadoVariables.x[211] += + acadoWorkspace.evGx[1230]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1231]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1232]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1233]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1234]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1235]*acadoWorkspace.x[5] + acadoWorkspace.d[205]; -acadoVariables.x[212] += + acadoWorkspace.evGx[1236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1239]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1240]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1241]*acadoWorkspace.x[5] + acadoWorkspace.d[206]; -acadoVariables.x[213] += + acadoWorkspace.evGx[1242]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1243]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1244]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1245]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1246]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1247]*acadoWorkspace.x[5] + acadoWorkspace.d[207]; -acadoVariables.x[214] += + acadoWorkspace.evGx[1248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1251]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1252]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1253]*acadoWorkspace.x[5] + acadoWorkspace.d[208]; -acadoVariables.x[215] += + acadoWorkspace.evGx[1254]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1255]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1256]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1257]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1258]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1259]*acadoWorkspace.x[5] + acadoWorkspace.d[209]; -acadoVariables.x[216] += + acadoWorkspace.evGx[1260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1263]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1264]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1265]*acadoWorkspace.x[5] + acadoWorkspace.d[210]; -acadoVariables.x[217] += + acadoWorkspace.evGx[1266]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1267]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1268]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1269]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1270]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1271]*acadoWorkspace.x[5] + acadoWorkspace.d[211]; -acadoVariables.x[218] += + acadoWorkspace.evGx[1272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1275]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1276]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1277]*acadoWorkspace.x[5] + acadoWorkspace.d[212]; -acadoVariables.x[219] += + acadoWorkspace.evGx[1278]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1279]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1280]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1281]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1282]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1283]*acadoWorkspace.x[5] + acadoWorkspace.d[213]; -acadoVariables.x[220] += + acadoWorkspace.evGx[1284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1287]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1288]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1289]*acadoWorkspace.x[5] + acadoWorkspace.d[214]; -acadoVariables.x[221] += + acadoWorkspace.evGx[1290]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1291]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1292]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1293]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1294]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1295]*acadoWorkspace.x[5] + acadoWorkspace.d[215]; -acadoVariables.x[222] += + acadoWorkspace.evGx[1296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1299]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1300]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1301]*acadoWorkspace.x[5] + acadoWorkspace.d[216]; -acadoVariables.x[223] += + acadoWorkspace.evGx[1302]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1303]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1304]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1305]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1306]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1307]*acadoWorkspace.x[5] + acadoWorkspace.d[217]; -acadoVariables.x[224] += + acadoWorkspace.evGx[1308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1311]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1312]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1313]*acadoWorkspace.x[5] + acadoWorkspace.d[218]; -acadoVariables.x[225] += + acadoWorkspace.evGx[1314]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1315]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1316]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1317]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1318]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1319]*acadoWorkspace.x[5] + acadoWorkspace.d[219]; -acadoVariables.x[226] += + acadoWorkspace.evGx[1320]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1321]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1322]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1323]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1324]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1325]*acadoWorkspace.x[5] + acadoWorkspace.d[220]; -acadoVariables.x[227] += + acadoWorkspace.evGx[1326]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1327]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1328]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1329]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1330]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1331]*acadoWorkspace.x[5] + acadoWorkspace.d[221]; -acadoVariables.x[228] += + acadoWorkspace.evGx[1332]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1333]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1334]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1335]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1336]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1337]*acadoWorkspace.x[5] + acadoWorkspace.d[222]; -acadoVariables.x[229] += + acadoWorkspace.evGx[1338]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1339]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1340]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1341]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1342]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1343]*acadoWorkspace.x[5] + acadoWorkspace.d[223]; -acadoVariables.x[230] += + acadoWorkspace.evGx[1344]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1345]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1346]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1347]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1348]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1349]*acadoWorkspace.x[5] + acadoWorkspace.d[224]; -acadoVariables.x[231] += + acadoWorkspace.evGx[1350]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1351]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1352]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1353]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1354]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1355]*acadoWorkspace.x[5] + acadoWorkspace.d[225]; -acadoVariables.x[232] += + acadoWorkspace.evGx[1356]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1357]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1358]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1359]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1360]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1361]*acadoWorkspace.x[5] + acadoWorkspace.d[226]; -acadoVariables.x[233] += + acadoWorkspace.evGx[1362]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1363]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1364]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1365]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1366]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1367]*acadoWorkspace.x[5] + acadoWorkspace.d[227]; -acadoVariables.x[234] += + acadoWorkspace.evGx[1368]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1369]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1370]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1371]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1372]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1373]*acadoWorkspace.x[5] + acadoWorkspace.d[228]; -acadoVariables.x[235] += + acadoWorkspace.evGx[1374]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1375]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1376]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1377]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1378]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1379]*acadoWorkspace.x[5] + acadoWorkspace.d[229]; -acadoVariables.x[236] += + acadoWorkspace.evGx[1380]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1381]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1382]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1383]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1384]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1385]*acadoWorkspace.x[5] + acadoWorkspace.d[230]; -acadoVariables.x[237] += + acadoWorkspace.evGx[1386]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1387]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1388]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1389]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1390]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1391]*acadoWorkspace.x[5] + acadoWorkspace.d[231]; -acadoVariables.x[238] += + acadoWorkspace.evGx[1392]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1393]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1394]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1395]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1396]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1397]*acadoWorkspace.x[5] + acadoWorkspace.d[232]; -acadoVariables.x[239] += + acadoWorkspace.evGx[1398]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1399]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1400]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1401]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1402]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1403]*acadoWorkspace.x[5] + acadoWorkspace.d[233]; -acadoVariables.x[240] += + acadoWorkspace.evGx[1404]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1405]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1406]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1407]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1408]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1409]*acadoWorkspace.x[5] + acadoWorkspace.d[234]; -acadoVariables.x[241] += + acadoWorkspace.evGx[1410]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1411]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1412]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1413]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1414]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1415]*acadoWorkspace.x[5] + acadoWorkspace.d[235]; -acadoVariables.x[242] += + acadoWorkspace.evGx[1416]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1417]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1418]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1419]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1420]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1421]*acadoWorkspace.x[5] + acadoWorkspace.d[236]; -acadoVariables.x[243] += + acadoWorkspace.evGx[1422]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1423]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1424]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1425]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1426]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1427]*acadoWorkspace.x[5] + acadoWorkspace.d[237]; -acadoVariables.x[244] += + acadoWorkspace.evGx[1428]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1429]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1430]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1431]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1432]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1433]*acadoWorkspace.x[5] + acadoWorkspace.d[238]; -acadoVariables.x[245] += + acadoWorkspace.evGx[1434]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1435]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1436]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1437]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1438]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1439]*acadoWorkspace.x[5] + acadoWorkspace.d[239]; -acadoVariables.x[246] += + acadoWorkspace.evGx[1440]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1441]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1442]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1443]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1444]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1445]*acadoWorkspace.x[5] + acadoWorkspace.d[240]; -acadoVariables.x[247] += + acadoWorkspace.evGx[1446]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1447]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1448]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1449]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1450]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1451]*acadoWorkspace.x[5] + acadoWorkspace.d[241]; -acadoVariables.x[248] += + acadoWorkspace.evGx[1452]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1453]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1454]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1455]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1456]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1457]*acadoWorkspace.x[5] + acadoWorkspace.d[242]; -acadoVariables.x[249] += + acadoWorkspace.evGx[1458]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1459]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1460]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1461]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1462]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1463]*acadoWorkspace.x[5] + acadoWorkspace.d[243]; -acadoVariables.x[250] += + acadoWorkspace.evGx[1464]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1465]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1466]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1467]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1468]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1469]*acadoWorkspace.x[5] + acadoWorkspace.d[244]; -acadoVariables.x[251] += + acadoWorkspace.evGx[1470]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1471]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1472]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1473]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1474]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1475]*acadoWorkspace.x[5] + acadoWorkspace.d[245]; -acadoVariables.x[252] += + acadoWorkspace.evGx[1476]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1477]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1478]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1479]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1480]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1481]*acadoWorkspace.x[5] + acadoWorkspace.d[246]; -acadoVariables.x[253] += + acadoWorkspace.evGx[1482]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1483]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1484]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1485]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1486]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1487]*acadoWorkspace.x[5] + acadoWorkspace.d[247]; -acadoVariables.x[254] += + acadoWorkspace.evGx[1488]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1489]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1490]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1491]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1492]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1493]*acadoWorkspace.x[5] + acadoWorkspace.d[248]; -acadoVariables.x[255] += + acadoWorkspace.evGx[1494]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1495]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1496]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1497]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1498]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1499]*acadoWorkspace.x[5] + acadoWorkspace.d[249]; -acadoVariables.x[256] += + acadoWorkspace.evGx[1500]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1501]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1502]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1503]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1504]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1505]*acadoWorkspace.x[5] + acadoWorkspace.d[250]; -acadoVariables.x[257] += + acadoWorkspace.evGx[1506]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1507]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1508]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1509]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1510]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1511]*acadoWorkspace.x[5] + acadoWorkspace.d[251]; -acadoVariables.x[258] += + acadoWorkspace.evGx[1512]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1513]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1514]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1515]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1516]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1517]*acadoWorkspace.x[5] + acadoWorkspace.d[252]; -acadoVariables.x[259] += + acadoWorkspace.evGx[1518]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1519]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1520]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1521]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1522]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1523]*acadoWorkspace.x[5] + acadoWorkspace.d[253]; -acadoVariables.x[260] += + acadoWorkspace.evGx[1524]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1525]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1526]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1527]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1528]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1529]*acadoWorkspace.x[5] + acadoWorkspace.d[254]; -acadoVariables.x[261] += + acadoWorkspace.evGx[1530]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1531]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1532]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1533]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1534]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1535]*acadoWorkspace.x[5] + acadoWorkspace.d[255]; -acadoVariables.x[262] += + acadoWorkspace.evGx[1536]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1537]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1538]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1539]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1540]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1541]*acadoWorkspace.x[5] + acadoWorkspace.d[256]; -acadoVariables.x[263] += + acadoWorkspace.evGx[1542]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1543]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1544]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1545]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1546]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1547]*acadoWorkspace.x[5] + acadoWorkspace.d[257]; -acadoVariables.x[264] += + acadoWorkspace.evGx[1548]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1549]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1550]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1551]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1552]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1553]*acadoWorkspace.x[5] + acadoWorkspace.d[258]; -acadoVariables.x[265] += + acadoWorkspace.evGx[1554]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1555]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1556]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1557]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1558]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1559]*acadoWorkspace.x[5] + acadoWorkspace.d[259]; -acadoVariables.x[266] += + acadoWorkspace.evGx[1560]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1561]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1562]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1563]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1564]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1565]*acadoWorkspace.x[5] + acadoWorkspace.d[260]; -acadoVariables.x[267] += + acadoWorkspace.evGx[1566]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1567]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1568]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1569]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1570]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1571]*acadoWorkspace.x[5] + acadoWorkspace.d[261]; -acadoVariables.x[268] += + acadoWorkspace.evGx[1572]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1573]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1574]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1575]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1576]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1577]*acadoWorkspace.x[5] + acadoWorkspace.d[262]; -acadoVariables.x[269] += + acadoWorkspace.evGx[1578]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1579]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1580]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1581]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1582]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1583]*acadoWorkspace.x[5] + acadoWorkspace.d[263]; -acadoVariables.x[270] += + acadoWorkspace.evGx[1584]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1585]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1586]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1587]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1588]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1589]*acadoWorkspace.x[5] + acadoWorkspace.d[264]; -acadoVariables.x[271] += + acadoWorkspace.evGx[1590]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1591]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1592]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1593]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1594]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1595]*acadoWorkspace.x[5] + acadoWorkspace.d[265]; -acadoVariables.x[272] += + acadoWorkspace.evGx[1596]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1597]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1598]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1599]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1600]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1601]*acadoWorkspace.x[5] + acadoWorkspace.d[266]; -acadoVariables.x[273] += + acadoWorkspace.evGx[1602]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1603]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1604]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1605]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1606]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1607]*acadoWorkspace.x[5] + acadoWorkspace.d[267]; -acadoVariables.x[274] += + acadoWorkspace.evGx[1608]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1609]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1610]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1611]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1612]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1613]*acadoWorkspace.x[5] + acadoWorkspace.d[268]; -acadoVariables.x[275] += + acadoWorkspace.evGx[1614]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1615]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1616]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1617]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1618]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1619]*acadoWorkspace.x[5] + acadoWorkspace.d[269]; -acadoVariables.x[276] += + acadoWorkspace.evGx[1620]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1621]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1622]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1623]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1624]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1625]*acadoWorkspace.x[5] + acadoWorkspace.d[270]; -acadoVariables.x[277] += + acadoWorkspace.evGx[1626]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1627]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1628]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1629]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1630]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1631]*acadoWorkspace.x[5] + acadoWorkspace.d[271]; -acadoVariables.x[278] += + acadoWorkspace.evGx[1632]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1633]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1634]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1635]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1636]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1637]*acadoWorkspace.x[5] + acadoWorkspace.d[272]; -acadoVariables.x[279] += + acadoWorkspace.evGx[1638]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1639]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1640]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1641]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1642]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1643]*acadoWorkspace.x[5] + acadoWorkspace.d[273]; -acadoVariables.x[280] += + acadoWorkspace.evGx[1644]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1645]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1646]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1647]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1648]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1649]*acadoWorkspace.x[5] + acadoWorkspace.d[274]; -acadoVariables.x[281] += + acadoWorkspace.evGx[1650]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1651]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1652]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1653]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1654]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1655]*acadoWorkspace.x[5] + acadoWorkspace.d[275]; -acadoVariables.x[282] += + acadoWorkspace.evGx[1656]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1657]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1658]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1659]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1660]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1661]*acadoWorkspace.x[5] + acadoWorkspace.d[276]; -acadoVariables.x[283] += + acadoWorkspace.evGx[1662]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1663]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1664]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1665]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1666]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1667]*acadoWorkspace.x[5] + acadoWorkspace.d[277]; -acadoVariables.x[284] += + acadoWorkspace.evGx[1668]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1669]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1670]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1671]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1672]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1673]*acadoWorkspace.x[5] + acadoWorkspace.d[278]; -acadoVariables.x[285] += + acadoWorkspace.evGx[1674]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1675]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1676]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1677]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1678]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1679]*acadoWorkspace.x[5] + acadoWorkspace.d[279]; -acadoVariables.x[286] += + acadoWorkspace.evGx[1680]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1681]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1682]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1683]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1684]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1685]*acadoWorkspace.x[5] + acadoWorkspace.d[280]; -acadoVariables.x[287] += + acadoWorkspace.evGx[1686]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1687]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1688]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1689]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1690]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1691]*acadoWorkspace.x[5] + acadoWorkspace.d[281]; -acadoVariables.x[288] += + acadoWorkspace.evGx[1692]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1693]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1694]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1695]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1696]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1697]*acadoWorkspace.x[5] + acadoWorkspace.d[282]; -acadoVariables.x[289] += + acadoWorkspace.evGx[1698]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1699]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1700]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1701]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1702]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1703]*acadoWorkspace.x[5] + acadoWorkspace.d[283]; -acadoVariables.x[290] += + acadoWorkspace.evGx[1704]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1705]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1706]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1707]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1708]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1709]*acadoWorkspace.x[5] + acadoWorkspace.d[284]; -acadoVariables.x[291] += + acadoWorkspace.evGx[1710]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1711]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1712]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1713]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1714]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1715]*acadoWorkspace.x[5] + acadoWorkspace.d[285]; -acadoVariables.x[292] += + acadoWorkspace.evGx[1716]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1717]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1718]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1719]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1720]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1721]*acadoWorkspace.x[5] + acadoWorkspace.d[286]; -acadoVariables.x[293] += + acadoWorkspace.evGx[1722]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1723]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1724]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1725]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1726]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1727]*acadoWorkspace.x[5] + acadoWorkspace.d[287]; -acadoVariables.x[294] += + acadoWorkspace.evGx[1728]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1729]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1730]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1731]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1732]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1733]*acadoWorkspace.x[5] + acadoWorkspace.d[288]; -acadoVariables.x[295] += + acadoWorkspace.evGx[1734]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1735]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1736]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1737]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1738]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1739]*acadoWorkspace.x[5] + acadoWorkspace.d[289]; -acadoVariables.x[296] += + acadoWorkspace.evGx[1740]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1741]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1742]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1743]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1744]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1745]*acadoWorkspace.x[5] + acadoWorkspace.d[290]; -acadoVariables.x[297] += + acadoWorkspace.evGx[1746]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1747]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1748]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1749]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1750]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1751]*acadoWorkspace.x[5] + acadoWorkspace.d[291]; -acadoVariables.x[298] += + acadoWorkspace.evGx[1752]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1753]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1754]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1755]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1756]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1757]*acadoWorkspace.x[5] + acadoWorkspace.d[292]; -acadoVariables.x[299] += + acadoWorkspace.evGx[1758]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1759]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1760]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1761]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1762]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1763]*acadoWorkspace.x[5] + acadoWorkspace.d[293]; -acadoVariables.x[300] += + acadoWorkspace.evGx[1764]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1765]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1766]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1767]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1768]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1769]*acadoWorkspace.x[5] + acadoWorkspace.d[294]; -acadoVariables.x[301] += + acadoWorkspace.evGx[1770]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1771]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1772]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1773]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1774]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1775]*acadoWorkspace.x[5] + acadoWorkspace.d[295]; -acadoVariables.x[302] += + acadoWorkspace.evGx[1776]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1777]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1778]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1779]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1780]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1781]*acadoWorkspace.x[5] + acadoWorkspace.d[296]; -acadoVariables.x[303] += + acadoWorkspace.evGx[1782]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1783]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1784]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1785]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1786]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1787]*acadoWorkspace.x[5] + acadoWorkspace.d[297]; -acadoVariables.x[304] += + acadoWorkspace.evGx[1788]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1789]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1790]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1791]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1792]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1793]*acadoWorkspace.x[5] + acadoWorkspace.d[298]; -acadoVariables.x[305] += + acadoWorkspace.evGx[1794]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1795]*acadoWorkspace.x[1] + acadoWorkspace.evGx[1796]*acadoWorkspace.x[2] + acadoWorkspace.evGx[1797]*acadoWorkspace.x[3] + acadoWorkspace.evGx[1798]*acadoWorkspace.x[4] + acadoWorkspace.evGx[1799]*acadoWorkspace.x[5] + acadoWorkspace.d[299]; - -for (lRun1 = 0; lRun1 < 50; ++lRun1) -{ -for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2) -{ -lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2); -acado_multEDu( &(acadoWorkspace.E[ lRun3 * 6 ]), &(acadoWorkspace.x[ lRun2 + 6 ]), &(acadoVariables.x[ lRun1 * 6 + 6 ]) ); -} -} + +acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 6 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 12 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 18 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 30 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 42 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 54 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 66 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 72 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 78 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 84 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 90 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 96 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 102 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 108 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVariables.x[ 114 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVariables.x[ 120 ]) ); +acado_multEDu( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.x[ 25 ]), &(acadoVariables.x[ 120 ]) ); } int acado_preparationStep( ) @@ -1861,7 +5196,7 @@ return ret; void acado_initializeNodesByForwardSimulation( ) { int index; -for (index = 0; index < 50; ++index) +for (index = 0; index < 20; ++index) { acadoWorkspace.state[0] = acadoVariables.x[index * 6]; acadoWorkspace.state[1] = acadoVariables.x[index * 6 + 1]; @@ -1872,7 +5207,7 @@ acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[index]; acadoWorkspace.state[49] = acadoVariables.od[index]; -acado_integrate(acadoWorkspace.state, index == 0); +acado_integrate(acadoWorkspace.state, index == 0, index); acadoVariables.x[index * 6 + 6] = acadoWorkspace.state[0]; acadoVariables.x[index * 6 + 7] = acadoWorkspace.state[1]; @@ -1886,7 +5221,7 @@ acadoVariables.x[index * 6 + 11] = acadoWorkspace.state[5]; void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd ) { int index; -for (index = 0; index < 50; ++index) +for (index = 0; index < 20; ++index) { acadoVariables.x[index * 6] = acadoVariables.x[index * 6 + 6]; acadoVariables.x[index * 6 + 1] = acadoVariables.x[index * 6 + 7]; @@ -1898,53 +5233,53 @@ acadoVariables.x[index * 6 + 5] = acadoVariables.x[index * 6 + 11]; if (strategy == 1 && xEnd != 0) { -acadoVariables.x[300] = xEnd[0]; -acadoVariables.x[301] = xEnd[1]; -acadoVariables.x[302] = xEnd[2]; -acadoVariables.x[303] = xEnd[3]; -acadoVariables.x[304] = xEnd[4]; -acadoVariables.x[305] = xEnd[5]; +acadoVariables.x[120] = xEnd[0]; +acadoVariables.x[121] = xEnd[1]; +acadoVariables.x[122] = xEnd[2]; +acadoVariables.x[123] = xEnd[3]; +acadoVariables.x[124] = xEnd[4]; +acadoVariables.x[125] = xEnd[5]; } else if (strategy == 2) { -acadoWorkspace.state[0] = acadoVariables.x[300]; -acadoWorkspace.state[1] = acadoVariables.x[301]; -acadoWorkspace.state[2] = acadoVariables.x[302]; -acadoWorkspace.state[3] = acadoVariables.x[303]; -acadoWorkspace.state[4] = acadoVariables.x[304]; -acadoWorkspace.state[5] = acadoVariables.x[305]; +acadoWorkspace.state[0] = acadoVariables.x[120]; +acadoWorkspace.state[1] = acadoVariables.x[121]; +acadoWorkspace.state[2] = acadoVariables.x[122]; +acadoWorkspace.state[3] = acadoVariables.x[123]; +acadoWorkspace.state[4] = acadoVariables.x[124]; +acadoWorkspace.state[5] = acadoVariables.x[125]; if (uEnd != 0) { acadoWorkspace.state[48] = uEnd[0]; } else { -acadoWorkspace.state[48] = acadoVariables.u[49]; +acadoWorkspace.state[48] = acadoVariables.u[19]; } -acadoWorkspace.state[49] = acadoVariables.od[50]; +acadoWorkspace.state[49] = acadoVariables.od[20]; -acado_integrate(acadoWorkspace.state, 1); +acado_integrate(acadoWorkspace.state, 1, 19); -acadoVariables.x[300] = acadoWorkspace.state[0]; -acadoVariables.x[301] = acadoWorkspace.state[1]; -acadoVariables.x[302] = acadoWorkspace.state[2]; -acadoVariables.x[303] = acadoWorkspace.state[3]; -acadoVariables.x[304] = acadoWorkspace.state[4]; -acadoVariables.x[305] = acadoWorkspace.state[5]; +acadoVariables.x[120] = acadoWorkspace.state[0]; +acadoVariables.x[121] = acadoWorkspace.state[1]; +acadoVariables.x[122] = acadoWorkspace.state[2]; +acadoVariables.x[123] = acadoWorkspace.state[3]; +acadoVariables.x[124] = acadoWorkspace.state[4]; +acadoVariables.x[125] = acadoWorkspace.state[5]; } } void acado_shiftControls( real_t* const uEnd ) { int index; -for (index = 0; index < 49; ++index) +for (index = 0; index < 19; ++index) { acadoVariables.u[index] = acadoVariables.u[index + 1]; } if (uEnd != 0) { -acadoVariables.u[49] = uEnd[0]; +acadoVariables.u[19] = uEnd[0]; } } @@ -1955,9 +5290,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] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25] + acadoWorkspace.g[26]*acadoWorkspace.x[26] + acadoWorkspace.g[27]*acadoWorkspace.x[27] + acadoWorkspace.g[28]*acadoWorkspace.x[28] + acadoWorkspace.g[29]*acadoWorkspace.x[29] + acadoWorkspace.g[30]*acadoWorkspace.x[30] + acadoWorkspace.g[31]*acadoWorkspace.x[31] + acadoWorkspace.g[32]*acadoWorkspace.x[32] + acadoWorkspace.g[33]*acadoWorkspace.x[33] + acadoWorkspace.g[34]*acadoWorkspace.x[34] + acadoWorkspace.g[35]*acadoWorkspace.x[35] + acadoWorkspace.g[36]*acadoWorkspace.x[36] + acadoWorkspace.g[37]*acadoWorkspace.x[37] + acadoWorkspace.g[38]*acadoWorkspace.x[38] + acadoWorkspace.g[39]*acadoWorkspace.x[39] + acadoWorkspace.g[40]*acadoWorkspace.x[40] + acadoWorkspace.g[41]*acadoWorkspace.x[41] + acadoWorkspace.g[42]*acadoWorkspace.x[42] + acadoWorkspace.g[43]*acadoWorkspace.x[43] + acadoWorkspace.g[44]*acadoWorkspace.x[44] + acadoWorkspace.g[45]*acadoWorkspace.x[45] + acadoWorkspace.g[46]*acadoWorkspace.x[46] + acadoWorkspace.g[47]*acadoWorkspace.x[47] + acadoWorkspace.g[48]*acadoWorkspace.x[48] + acadoWorkspace.g[49]*acadoWorkspace.x[49] + acadoWorkspace.g[50]*acadoWorkspace.x[50] + acadoWorkspace.g[51]*acadoWorkspace.x[51] + acadoWorkspace.g[52]*acadoWorkspace.x[52] + acadoWorkspace.g[53]*acadoWorkspace.x[53] + acadoWorkspace.g[54]*acadoWorkspace.x[54] + acadoWorkspace.g[55]*acadoWorkspace.x[55]; +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] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25]; kkt = fabs( kkt ); -for (index = 0; index < 56; ++index) +for (index = 0; index < 26; ++index) { prd = acadoWorkspace.y[index]; if (prd > 1e-12) @@ -1965,9 +5300,9 @@ kkt += fabs(acadoWorkspace.lb[index] * prd); else if (prd < -1e-12) kkt += fabs(acadoWorkspace.ub[index] * prd); } -for (index = 0; index < 50; ++index) +for (index = 0; index < 20; ++index) { -prd = acadoWorkspace.y[index + 56]; +prd = acadoWorkspace.y[index + 26]; if (prd > 1e-12) kkt += fabs(acadoWorkspace.lbA[index] * prd); else if (prd < -1e-12) @@ -1987,7 +5322,7 @@ real_t tmpDy[ 4 ]; /** Row vector of size: 3 */ real_t tmpDyN[ 3 ]; -for (lRun1 = 0; lRun1 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 6]; acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 6 + 1]; @@ -2004,30 +5339,30 @@ acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariable acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; acadoWorkspace.Dy[lRun1 * 4 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 4 + 3]; } -acadoWorkspace.objValueIn[0] = acadoVariables.x[300]; -acadoWorkspace.objValueIn[1] = acadoVariables.x[301]; -acadoWorkspace.objValueIn[2] = acadoVariables.x[302]; -acadoWorkspace.objValueIn[3] = acadoVariables.x[303]; -acadoWorkspace.objValueIn[4] = acadoVariables.x[304]; -acadoWorkspace.objValueIn[5] = acadoVariables.x[305]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[50]; +acadoWorkspace.objValueIn[0] = acadoVariables.x[120]; +acadoWorkspace.objValueIn[1] = acadoVariables.x[121]; +acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; +acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; +acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; +acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[20]; 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]; objVal = 0.0000000000000000e+00; -for (lRun1 = 0; lRun1 < 50; ++lRun1) +for (lRun1 = 0; lRun1 < 20; ++lRun1) { -tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 4]*(real_t)5.0000000000000000e+00; -tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 4 + 1]*(real_t)1.0000000000000001e-01; -tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 4 + 2]*(real_t)1.0000000000000000e+01; -tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 4 + 3]*(real_t)2.0000000000000000e+01; +tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 4] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 8] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 12]; +tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 1] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 5] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 9] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 13]; +tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 2] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 6] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 10] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 14]; +tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 3] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 7] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 11] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 15]; objVal += + acadoWorkspace.Dy[lRun1 * 4]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 4 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 4 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 4 + 3]*tmpDy[3]; } -tmpDyN[0] = + acadoWorkspace.DyN[0]*(real_t)5.0000000000000000e+00; -tmpDyN[1] = + acadoWorkspace.DyN[1]*(real_t)1.0000000000000001e-01; -tmpDyN[2] = + acadoWorkspace.DyN[2]*(real_t)1.0000000000000000e+01; +tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[3] + acadoWorkspace.DyN[2]*acadoVariables.WN[6]; +tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[7]; +tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[8]; objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2]; objVal *= 0.5; diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index bb6711e8b0e913..ad304e88424abe 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -16,7 +16,7 @@ from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.longcontrol import LongCtrlState +from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED _DT = 0.01 # 100Hz _DT_MPC = 0.2 # 5Hz @@ -255,8 +255,9 @@ class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP - self.live20 = messaging.sub_sock(context, service_list['live20'].port) - self.model = messaging.sub_sock(context, service_list['model'].port) + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port) @@ -326,7 +327,15 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS - md = messaging.recv_sock(self.model) + md = None + l20 = None + + for socket, event in self.poller.poll(0): + if socket is self.model: + md = messaging.recv_one(socket) + elif socket is self.live20: + l20 = messaging.recv_one(socket) + if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time @@ -334,7 +343,6 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): self.PP.update(CS.vEgo, md) - l20 = messaging.recv_sock(self.live20) if md is None else None if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time @@ -368,17 +376,21 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) else: starting = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) - self.v_cruise = CS.vEgo - self.a_cruise = self.CP.startAccel if starting else a_ego - self.v_acc_start = CS.vEgo - self.a_acc_start = self.CP.startAccel if starting else a_ego - self.v_acc = CS.vEgo - self.a_acc = self.CP.startAccel if starting else a_ego - self.v_acc_sol = CS.vEgo - self.a_acc_sol = self.CP.startAccel if starting else a_ego + reset_speed = MIN_CAN_SPEED if starting else CS.vEgo + reset_accel = self.CP.startAccel if starting else a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel + self.v_acc_sol = reset_speed + self.a_acc_sol = reset_accel self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) @@ -399,7 +411,7 @@ def update(self, CS, LoC, v_cruise_kph, user_distracted): self.lead_1.fcw, blinkers) \ and not CS.brakePressed if self.fcw: - cloudlog.info("FCW triggered") + cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py index d9bc0329d1f58b..1ee7ec6b18cb5d 100644 --- a/selfdrive/controls/lib/speed_smoother.py +++ b/selfdrive/controls/lib/speed_smoother.py @@ -67,15 +67,20 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): if aPeak > aMax: aPeak = aMax t1 = (aPeak - aEgo) / jMax - vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin - if vChange < aPeak * ts: - t2 = t1 + vChange / aPeak + if aPeak <= 0: # there is no solution, so stop after t1 + t2 = t1 + ts + 1e-9 + t3 = t2 else: - t2 = t1 + ts + vChange = dV - 0.5 * (aPeak**2 - aEgo**2) / jMax + 0.5 * aPeak**2 / jMin + if vChange < aPeak * ts: + t2 = t1 + vChange / aPeak + else: + t2 = t1 + ts + t3 = t2 - aPeak / jMin else: t1 = (aPeak - aEgo) / jMax t2 = t1 - t3 = t2 - aPeak / jMin + t3 = t2 - aPeak / jMin dt1 = min(ts, t1) dt2 = max(min(ts, t2) - t1, 0.) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 3cdb8a104b627c..f10e31a3b8d678 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -89,6 +89,8 @@ def state_prediction(self, sa, u): A, B = create_dyn_state_matrices(u, self) return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt + def yaw_rate(self, sa, u): + return self.calc_curvature(sa, u) * u if __name__ == '__main__': from selfdrive.car.honda.interface import CarInterface diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6dec7d1e105559..72e98d732d1680 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -44,11 +44,12 @@ def calc_transfer_fun(self, dt): # fuses camera and radar data for best lead detection def radard_thread(gctx=None): - set_realtime_priority(1) + set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) + mocked= CP.radarName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") @@ -59,8 +60,9 @@ def radard_thread(gctx=None): context = zmq.Context() # *** subscribe to features and model from visiond - model = messaging.sub_sock(context, service_list['model'].port) - live100 = messaging.sub_sock(context, service_list['live100'].port) + poller = zmq.Poller() + model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) + live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) PP = PathPlanner() RI = RadarInterface() @@ -96,7 +98,6 @@ def radard_thread(gctx=None): rk = Ratekeeper(rate, print_delay_threshold=np.inf) while 1: - rr = RI.update() ar_pts = {} @@ -104,7 +105,15 @@ def radard_thread(gctx=None): ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] # receive the live100s - l100 = messaging.recv_sock(live100) + l100 = None + md = None + + for socket, event in poller.poll(0): + if socket is live100: + l100 = messaging.recv_one(socket) + elif socket is model: + md = messaging.recv_one(socket) + if l100 is not None: active = l100.live100.active v_ego = l100.live100.vEgo @@ -119,7 +128,6 @@ def radard_thread(gctx=None): if v_ego is None: continue - md = messaging.recv_sock(model) if md is not None: last_md_ts = md.logMonoTime @@ -140,9 +148,11 @@ def radard_thread(gctx=None): del ar_pts[VISION_POINT] # *** compute the likely path_y *** - if active and not steer_override: # use path from model path_poly + if (active and not steer_override) or mocked: + # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(PP.d_poly, path_x) - else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset + else: + # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0] # *** remove missing points from meta data *** @@ -152,8 +162,8 @@ def radard_thread(gctx=None): # *** compute the tracks *** for ids in ar_pts: - # ignore the vision point for now - if ids == VISION_POINT: + # ignore standalone vision point, unless we are mocking the radar + if ids == VISION_POINT and not mocked: continue rpt = ar_pts[ids] @@ -185,33 +195,11 @@ def radard_thread(gctx=None): tracks[fused_id].vision_cnt += 1 tracks[fused_id].update_vision_fusion() - - # publish tracks (debugging) - dat = messaging.new_message() - dat.init('liveTracks', len(tracks)) - if DEBUG: print "NEW CYCLE" if VISION_POINT in ar_pts: print "vision", ar_pts[VISION_POINT] - for cnt, ids in enumerate(tracks.keys()): - if DEBUG: - print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \ - (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, - tracks[ids].dPath, tracks[ids].vLat, - tracks[ids].vLead, tracks[ids].vLeadK, - tracks[ids].aLeadK, - tracks[ids].stationary) - dat.liveTracks[cnt].trackId = ids - dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) - dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) - dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) - dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) - dat.liveTracks[cnt].stationary = tracks[ids].stationary - dat.liveTracks[cnt].oncoming = tracks[ids].oncoming - liveTracks.send(dat.to_bytes()) - idens = tracks.keys() track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) @@ -268,6 +256,27 @@ def radard_thread(gctx=None): dat.live20.cumLagMs = -rk.remaining*1000. live20.send(dat.to_bytes()) + # *** publish tracks for UI debugging (keep last) *** + dat = messaging.new_message() + dat.init('liveTracks', len(tracks)) + + for cnt, ids in enumerate(tracks.keys()): + if DEBUG: + print "id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f" % \ + (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, + tracks[ids].dPath, tracks[ids].vLat, + tracks[ids].vLead, tracks[ids].vLeadK, + tracks[ids].aLeadK, + tracks[ids].stationary) + dat.liveTracks[cnt].trackId = ids + dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) + dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) + dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) + dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) + dat.liveTracks[cnt].stationary = tracks[ids].stationary + dat.liveTracks[cnt].oncoming = tracks[ids].oncoming + liveTracks.send(dat.to_bytes()) + rk.monitor_time() def main(gctx=None): diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 648556b2244ed3..136420c2d1cb72 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 34a3a79aaa6369..f04bb0d030ddaf 100755 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -9,6 +9,7 @@ import requests import traceback import threading +import subprocess from collections import Counter from selfdrive.swaglog import cloudlog @@ -66,6 +67,13 @@ def clear_locks(root): except OSError: cloudlog.exception("clear_locks failed") +def is_on_wifi(): + # ConnectivityManager.getActiveNetworkInfo() + result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n") + data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) + + return "\x00".join("WIFI") in data + class Uploader(object): def __init__(self, dongle_id, access_token, root): @@ -111,21 +119,22 @@ def get_data_stats(self): total_size += os.stat(fn).st_size return dict(name_counts), total_size - def next_file_to_upload(self): + def next_file_to_upload(self, with_video): # try to upload log files first for name, key, fn in self.gen_upload_files(): if name in ["rlog", "rlog.bz2"]: return (key, fn, 0) - # then upload compressed camera file - for name, key, fn in self.gen_upload_files(): - if name in ["fcamera.hevc"]: - return (key, fn, 1) + if with_video: + # then upload compressed camera file + for name, key, fn in self.gen_upload_files(): + if name in ["fcamera.hevc"]: + return (key, fn, 1) - # then upload other files - for name, key, fn in self.gen_upload_files(): - if not name.endswith('.lock') and not name.endswith(".tmp"): - return (key, fn, 1) + # then upload other files + for name, key, fn in self.gen_upload_files(): + if not name.endswith('.lock') and not name.endswith(".tmp"): + return (key, fn, 1) return None @@ -240,13 +249,16 @@ def uploader_fn(exit_event): uploader = Uploader(dongle_id, access_token, ROOT) while True: + + upload_video = (params.get("IsUploadVideoOverCellularEnabled") != "0") or is_on_wifi() + backoff = 0.1 while True: if exit_event.is_set(): return - d = uploader.next_file_to_upload() + d = uploader.next_file_to_upload(upload_video) if d is None: break diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 4771776cf1d399..c3ed883382c562 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -7,13 +7,9 @@ import signal if __name__ == "__main__": - if os.path.isfile("/init.qcom.rc"): - # check if NEOS update is required - while ((not os.path.isfile("/VERSION") - or int(open("/VERSION").read()) < 3) - and not os.path.isfile("/data/media/0/noupdate")): - os.system("curl -o /tmp/updater https://neos.comma.ai/updater && chmod +x /tmp/updater && /tmp/updater") - time.sleep(10) + if os.path.isfile("/init.qcom.rc") \ + and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 4): + raise Exception("NEOS outdated") # get a non-blocking stdout child_pid, child_pty = os.forkpty() @@ -44,6 +40,7 @@ os._exit(os.wait()[1]) +import shutil import hashlib import importlib import subprocess @@ -219,22 +216,13 @@ def kill_managed_process(name): def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) - manage_baseui(False) + + for p in ("com.waze", "com.spotify.music", "ai.comma.plus.offroad", "ai.comma.plus.frame"): + system("am force-stop %s" % p) + for name in running.keys(): kill_managed_process(name) - sys.exit(0) - -baseui_running = False -def manage_baseui(start): - global baseui_running - if start and not baseui_running: - cloudlog.info("starting baseui") - os.system("am start -n com.baseui/.MainActivity") - baseui_running = True - elif not start and baseui_running: - cloudlog.info("stopping baseui") - os.system("am force-stop com.baseui") - baseui_running = False + cloudlog.info("everything is dead") # ****************** run loop ****************** @@ -252,7 +240,11 @@ def manager_init(): cloudlog.info("dongle id is " + dongle_id) os.environ['DONGLE_ID'] = dongle_id - dirty = subprocess.call(["git", "diff-index", "--quiet", "origin/release", "--"]) != 0 + if "-private" in subprocess.check_output(["git", "config", "--get", "remote.origin.url"]): + upstream = "origin/master" + else: + upstream = "origin/release" + dirty = subprocess.call(["git", "diff-index", "--quiet", upstream, "--"]) != 0 cloudlog.info("dirty is %d" % dirty) if not dirty: os.environ['CLEAN'] = '1' @@ -280,12 +272,9 @@ def system(cmd): output=e.output[-1024:], returncode=e.returncode) -# TODO: this is not proper gating for EON -try: +EON = os.path.exists("/EON") +if EON: from smbus2 import SMBus - EON = True -except ImportError: - EON = False def setup_eon_fan(): if not EON: @@ -322,8 +311,10 @@ def set_eon_fan(val): _TEMP_THRS_L = [42.5, 57.5, 72.5, 10000] # fan speed options _FAN_SPEEDS = [0, 16384, 32768, 65535] +# max fan speed only allowed if battery if hot +_BAT_TEMP_THERSHOLD = 45. -def handle_fan(max_temp, fan_speed): +def handle_fan(max_temp, bat_temp, fan_speed): new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp) new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp) @@ -334,6 +325,10 @@ def handle_fan(max_temp, fan_speed): # update speed if using the low thresholds results in fan speed decrement fan_speed = new_speed_l + if bat_temp < _BAT_TEMP_THERSHOLD: + # no max fan speed unless battery is hot + fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) + set_eon_fan(fan_speed/16384) return fan_speed @@ -369,8 +364,6 @@ def update(self, started_ts, location): return location.speed*3.6 > 10 def manager_thread(): - global baseui_running - # now loop context = zmq.Context() thermal_sock = messaging.pub_sock(context, service_list['thermal'].port) @@ -383,7 +376,8 @@ def manager_thread(): for p in persistent_processes: start_managed_process(p) - manage_baseui(True) + # start frame + system("am start -n ai.comma.plus.frame/.MainActivity") # do this before panda flashing setup_eon_fan() @@ -391,7 +385,9 @@ def manager_thread(): if os.getenv("NOBOARD") is None: start_managed_process("pandad") - passive = bool(os.getenv("PASSIVE")) + params = Params() + + passive = params.get("Passive") == "1" passive_starter = LocationStarter() started_ts = None @@ -399,6 +395,7 @@ def manager_thread(): count = 0 fan_speed = 0 ignition_seen = False + battery_was_high = False health_sock.RCVTIMEO = 1500 @@ -424,13 +421,19 @@ def manager_thread(): msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() + with open("/sys/class/power_supply/usb/online") as f: + msg.thermal.usbOnline = bool(int(f.read())) # TODO: add car battery voltage check max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 - fan_speed = handle_fan(max_temp, fan_speed) + bat_temp = msg.thermal.bat/1000. + fan_speed = handle_fan(max_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed + msg.thermal.started = started_ts is not None + msg.thermal.startedTs = int(1e9*(started_ts or 0)) + thermal_sock.send(msg.to_bytes()) print msg @@ -448,8 +451,10 @@ def manager_thread(): ignition = td is not None and td.health.started ignition_seen = ignition_seen or ignition - params = Params() - should_start = ignition and (params.get("HasAcceptedTerms") == "1") + do_uninstall = params.get("DoUninstall") == "1" + accepted_terms = params.get("HasAcceptedTerms") == "1" + + should_start = ignition # start on gps in passive mode if passive and not ignition_seen: @@ -458,6 +463,15 @@ def manager_thread(): # with 2% left, we killall, otherwise the phone is bricked should_start = should_start and avail > 0.02 + # require usb power + should_start = should_start and msg.thermal.usbOnline + + should_start = should_start and accepted_terms and (not do_uninstall) + + # if any CPU gets above 107 or the battery gets above 53, kill all processes + # controls will warn with CPU above 95 or battery above 50 + if max_temp > 107.0 or msg.thermal.bat >= 53000: + should_start = False if should_start: if not started_ts: @@ -468,20 +482,17 @@ def manager_thread(): kill_managed_process(p) else: start_managed_process(p) - manage_baseui(False) else: - manage_baseui(True) started_ts = None logger_dead = False for p in car_started_processes: kill_managed_process(p) - # shutdown if the battery gets lower than 10%, we aren't running, and we are discharging - if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging": + # shutdown if the battery gets lower than 5%, we aren't running, and we are discharging + if msg.thermal.batteryPercent < 5 and msg.thermal.batteryStatus == "Discharging" and battery_was_high: os.system('LD_LIBRARY_PATH="" svc power shutdown') - - # check the status of baseui - baseui_running = 'com.baseui' in subprocess.check_output(["ps"]) + if msg.thermal.batteryPercent > 10: + battery_was_high = True # check the status of all processes, did any of them die? for p in running: @@ -495,10 +506,13 @@ def manager_thread(): health=(td.to_dict() if td else None), thermal=msg.to_dict()) + if do_uninstall: + break + count += 1 def get_installed_apks(): - dat = subprocess.check_output(["pm", "list", "packages", "-3", "-f"]).strip().split("\n") + dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n") ret = {} for x in dat: if x.startswith("package:"): @@ -506,21 +520,16 @@ def get_installed_apks(): ret[k] = v return ret -# optional, build the c++ binaries and preimport the python for speed -def manager_prepare(): - if os.path.exists(os.path.join(BASEDIR, ".gitmodules")): - # update submodules - system("cd %s && git submodule init panda opendbc pyextra" % BASEDIR) - system("cd %s && git submodule update panda opendbc pyextra" % BASEDIR) +def install_apk(path): + # can only install from world readable path + install_path = "/sdcard/%s" % os.path.basename(path) + shutil.copyfile(path, install_path) - # build cereal first - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) - - # build all processes - os.chdir(os.path.dirname(os.path.abspath(__file__))) - for p in managed_processes: - prepare_managed_process(p) + ret = subprocess.call(["pm", "install", "-r", install_path]) + os.remove(install_path) + return ret == 0 +def update_apks(): # install apks installed = get_installed_apks() for app in os.listdir(os.path.join(BASEDIR, "apk/")): @@ -529,7 +538,8 @@ def manager_prepare(): if app not in installed: installed[app] = None cloudlog.info("installed apks %s" % (str(installed), )) - for app in installed: + + for app in installed.iterkeys(): apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") if os.path.isfile(apk_path): h1 = hashlib.sha1(open(apk_path).read()).hexdigest() @@ -537,16 +547,37 @@ def manager_prepare(): if installed[app] is not None: h2 = hashlib.sha1(open(installed[app]).read()).hexdigest() cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) + if h2 is None or h1 != h2: cloudlog.info("installing %s" % app) - for do_uninstall in [False, True]: - if do_uninstall: - cloudlog.info("needing to uninstall %s" % app) - os.system("pm uninstall %s" % app) - ret = os.system("cp %s /sdcard/%s.apk && pm install -r /sdcard/%s.apk && rm /sdcard/%s.apk" % (apk_path, app, app, app)) - if ret == 0: - break - assert ret == 0 + + success = install_apk(apk_path) + if not success: + cloudlog.info("needing to uninstall %s" % app) + system("pm uninstall %s" % app) + success = install_apk(apk_path) + + assert success + +def manager_update(): + update_apks() + +def manager_prepare(): + + # build cereal first + subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) + + # build all processes + os.chdir(os.path.dirname(os.path.abspath(__file__))) + for p in managed_processes: + prepare_managed_process(p) + +def uninstall(): + cloudlog.warning("uninstalling") + with open('/cache/recovery/command', 'w') as f: + f.write('--wipe_data\n') + # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) + os.system("service call power 16 i32 0 s16 recovery i32 1") def main(): if os.getenv("NOLOG") is not None: @@ -581,11 +612,13 @@ def main(): if params.get("IsMetric") is None: params.put("IsMetric", "0") if params.get("IsRearViewMirror") is None: - params.put("IsRearViewMirror", "1") + params.put("IsRearViewMirror", "0") if params.get("IsFcwEnabled") is None: params.put("IsFcwEnabled", "1") if params.get("HasAcceptedTerms") is None: params.put("HasAcceptedTerms", "0") + if params.get("IsUploadVideoOverCellularEnabled") is None: + params.put("IsUploadVideoOverCellularEnabled", "1") params.put("Passive", "1" if os.getenv("PASSIVE") else "0") @@ -597,6 +630,7 @@ def main(): cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), close_fds=True) try: + manager_update() manager_init() manager_prepare() finally: @@ -606,6 +640,9 @@ def main(): if os.getenv("PREPAREONLY") is not None: return + # SystemExit on sigterm + signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(1)) + try: manager_thread() except Exception: @@ -614,6 +651,9 @@ def main(): finally: cleanup_all_processes(None, None) + if params.get("DoUninstall") == "1": + uninstall() + if __name__ == "__main__": main() # manual exit because we are forked diff --git a/selfdrive/radar/mock/__init__.py b/selfdrive/radar/mock/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/selfdrive/radar/mock/interface.py b/selfdrive/radar/mock/interface.py new file mode 100755 index 00000000000000..7a5c66722ad036 --- /dev/null +++ b/selfdrive/radar/mock/interface.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python +from cereal import car +import time + + +class RadarInterface(object): + def __init__(self): + # radar + self.pts = {} + self.delay = 0.1 + + def update(self): + + ret = car.RadarState.new_message() + time.sleep(0.05) # radard runs on RI updates + + return ret + +if __name__ == "__main__": + RI = RadarInterface() + while 1: + ret = RI.update() + print(chr(27) + "[2J") + print ret diff --git a/selfdrive/radar/nidec/interface.py b/selfdrive/radar/nidec/interface.py index dca71f32f4d4db..fb2a68693711f7 100755 --- a/selfdrive/radar/nidec/interface.py +++ b/selfdrive/radar/nidec/interface.py @@ -33,7 +33,6 @@ def __init__(self): self.radar_fault = False self.delay = 0.1 # Delay of radar - self.cutin_prediction = True # Nidec self.rcp = _create_nidec_can_parser() diff --git a/selfdrive/radar/toyota/interface.py b/selfdrive/radar/toyota/interface.py index 40445f6686fe80..cd60925d0b8431 100755 --- a/selfdrive/radar/toyota/interface.py +++ b/selfdrive/radar/toyota/interface.py @@ -34,7 +34,6 @@ def __init__(self): # Nidec self.rcp = _create_radard_can_parser() - self.cutin_prediction = False context = zmq.Context() self.logcan = messaging.sub_sock(context, service_list['can'].port) diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 0779171bfb5f1a..ff522a12e379ee 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -19,16 +19,23 @@ def get_git_commit(): def get_git_branch(): return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() +def get_git_remote(): + return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip() + def register(): params = Params() - try: - params.put("Version", version) - params.put("GitCommit", get_git_commit()) - params.put("GitBranch", get_git_branch()) + params.put("Version", version) + params.put("GitCommit", get_git_commit()) + params.put("GitBranch", get_git_branch()) + params.put("GitRemote", get_git_remote()) - dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") + dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") + + try: if dongle_id is None or access_token is None: - resp = api_get("v1/pilotauth/", method='POST', imei=get_imei(), serial=get_serial()) + cloudlog.info("getting pilotauth") + resp = api_get("v1/pilotauth/", method='POST', timeout=15, + imei=get_imei(), serial=get_serial()) dongleauth = json.loads(resp.text) dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii') diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd index ec710f90772e2a..dfeab52461361c 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index f234a920d2bfd8..4ad04c9f17d8d7 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index e87f8ebac4d573..16dff8002c3eb6 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -46,6 +46,8 @@ ubloxGnss: [8033, true] clocks: [8034, true] liveMpc: [8035, true] liveLongitudinalMpc: [8036, true] +plusFrame: [8037, false] +navStatus: [8038, true] testModel: [8040, false] diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/plant/maneuver.py index 92186600e42c4e..af370ea7b6c370 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/plant/maneuver.py @@ -60,6 +60,7 @@ def evaluate(self): gas=gas, brake=brake, steer_torque=steer_torque, distance=distance, speed=speed, acceleration=acceleration, up_accel_cmd=last_live100.upAccelCmd, ui_accel_cmd=last_live100.uiAccelCmd, + uf_accel_cmd=last_live100.ufAccelCmd, d_rel=d_rel, v_rel=v_rel, v_lead=speed_lead, v_target_lead=last_live100.vTargetLead, pid_speed=last_live100.vPid, cruise_speed=last_live100.vCruise, diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/plant/maneuverplots.py index 0a3f3cd201cbb7..c0bbf6ef94f8f0 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/plant/maneuverplots.py @@ -20,6 +20,7 @@ def __init__(self, title = None): self.up_accel_cmd_array = [] self.ui_accel_cmd_array = [] + self.uf_accel_cmd_array = [] self.d_rel_array = [] self.v_rel_array = [] @@ -38,8 +39,8 @@ def __init__(self, title = None): self.title = title def add_data(self, time, gas, brake, steer_torque, distance, speed, - acceleration, up_accel_cmd, ui_accel_cmd, d_rel, v_rel, v_lead, - v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): + acceleration, up_accel_cmd, ui_accel_cmd, uf_accel_cmd, d_rel, v_rel, + v_lead, v_target_lead, pid_speed, cruise_speed, jerk_factor, a_target, fcw): self.time_array.append(time) self.gas_array.append(gas) self.brake_array.append(brake) @@ -49,6 +50,7 @@ def add_data(self, time, gas, brake, steer_torque, distance, speed, self.acceleration_array.append(acceleration) self.up_accel_cmd_array.append(up_accel_cmd) self.ui_accel_cmd_array.append(ui_accel_cmd) + self.uf_accel_cmd_array.append(uf_accel_cmd) self.d_rel_array.append(d_rel) self.v_rel_array.append(v_rel) self.v_lead_array.append(v_lead) @@ -117,12 +119,13 @@ def write_plot(self, path, maneuver_name): plt.figure(plt_num) plt.plot( np.array(self.time_array), np.array(self.up_accel_cmd_array), 'g', - np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b' + np.array(self.time_array), np.array(self.ui_accel_cmd_array), 'b', + np.array(self.time_array), np.array(self.uf_accel_cmd_array), 'r' ) plt.xlabel("Time, [s]") plt.ylabel("Accel Cmd [m/s^2]") plt.grid() - plt.legend(["Proportional", "Integral"], loc=0) + plt.legend(["Proportional", "Integral", "feedforward"], loc=0) pylab.savefig("/".join([path, maneuver_name, "pid.svg"]), dpi=1000) # relative distances chart ======= diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 2fcce2acbc0915..81fd2042a9435f 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -4,7 +4,6 @@ from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import start_managed_process, kill_managed_process, get_running -from selfdrive.manager import manage_baseui from selfdrive.config import CruiseButtons from functools import wraps import time @@ -93,10 +92,3 @@ def test_ui(): def test_uploader(): print "UPLOADER" time.sleep(10.0) - -@phone_only -def test_baseui(): - manage_baseui(True) - time.sleep(10.0) - manage_baseui(False) - diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile index d0a3778e21e5b8..38b91479353dd9 100644 --- a/selfdrive/ui/Makefile +++ b/selfdrive/ui/Makefile @@ -52,6 +52,7 @@ ui: $(OBJS) $(CEREAL_LIBS) \ $(ZMQ_LIBS) \ -L/system/vendor/lib64 \ + -lhardware \ $(OPENGL_LIBS) \ -lcutils -lm -llog diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index e621af4117525c..20f23df800cb92 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -37,8 +37,37 @@ #define CALIBRATION_CALIBRATED 1 #define CALIBRATION_INVALID 2 +#define STATUS_STOPPED 0 +#define STATUS_DISENGAGED 1 +#define STATUS_ENGAGED 2 +#define STATUS_WARNING 3 +#define STATUS_ALERT 4 +#define STATUS_MAX 5 + #define UI_BUF_COUNT 4 + +const int box_x = 330; +const int box_y = 30; +const int box_width = 1560; +const int box_height = 1020; + +const uint8_t bg_colors[][4] = { + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff}, +}; + +const uint8_t alert_colors[][4] = { + [STATUS_STOPPED] = {0x07, 0x23, 0x39, 0x80}, + [STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0x80}, + [STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0x80}, + [STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0x80}, + [STATUS_ALERT] = {0xC9, 0x22, 0x31, 0x80}, +}; + typedef struct UIScene { int frontview; @@ -57,8 +86,9 @@ typedef struct UIScene { mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. float v_cruise; + uint64_t v_cruise_update_ts; float v_ego; - float angle_steers; + float curvature; int engaged; int lead_status; @@ -70,9 +100,12 @@ typedef struct UIScene { uint64_t alert_ts; char alert_text1[1024]; char alert_text2[1024]; + uint8_t alert_size; float awareness_status; + uint64_t started_ts; + // Used to display calibration progress int cal_status; int cal_perc; @@ -80,16 +113,21 @@ typedef struct UIScene { typedef struct UIState { pthread_mutex_t lock; + pthread_cond_t bg_cond; FramebufferState *fb; - int fb_w, fb_h; EGLDisplay display; EGLSurface surface; NVGcontext *vg; - int font; + int font_courbd; + int font_sans_regular; + int font_sans_semibold; + + zsock_t *thermal_sock; + void *thermal_sock_raw; zsock_t *model_sock; void *model_sock_raw; zsock_t *live100_sock; @@ -100,6 +138,10 @@ typedef struct UIState { void *live20_sock_raw; zsock_t *livempc_sock; void *livempc_sock_raw; + zsock_t *plus_sock; + void *plus_sock_raw; + + int plus_state; // vision state bool vision_connected; @@ -135,14 +177,31 @@ typedef struct UIState { bool awake; int awake_timeout; + int status; bool is_metric; bool passive; + + float light_sensor; } UIState; +static int last_brightness = -1; +static void set_brightness(int brightness) { + if (last_brightness != brightness) { + //printf("setting brightness %d\n", brightness); + // can't hurt + FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); + if (f != NULL) { + fprintf(f, "%d", brightness); + fclose(f); + last_brightness = brightness; + } + } +} + static void set_awake(UIState *s, bool awake) { if (awake) { - // 15 second timeout at 30 fps - s->awake_timeout = 15*30; + // 30 second timeout at 30 fps + s->awake_timeout = 30*30; } if (s->awake != awake) { s->awake = awake; @@ -150,13 +209,6 @@ static void set_awake(UIState *s, bool awake) { if (awake) { LOG("awake normal"); framebuffer_set_power(s->fb, HWC_POWER_MODE_NORMAL); - - // can't hurt - FILE *f = fopen("/sys/class/leds/lcd-backlight/brightness", "wb"); - if (f != NULL) { - fprintf(f, "205"); - fclose(f); - } } else { LOG("awake off"); framebuffer_set_power(s->fb, HWC_POWER_MODE_OFF); @@ -164,6 +216,11 @@ static void set_awake(UIState *s, bool awake) { } } +volatile int do_exit = 0; +static void set_do_exit(int sig) { + do_exit = 1; +} + static const char frame_vertex_shader[] = "attribute vec4 aPosition;\n" @@ -209,22 +266,26 @@ static const mat4 device_transform = {{ 0.0, 0.0, 0.0, 1.0, }}; -// frame from 4/3 to 16/9 with a 2x zoon +// frame from 4/3 to box size with a 2x zoon static const mat4 frame_transform = {{ - 2*(4./3.)/(16./9.), 0.0, 0.0, 0.0, - 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, + 2*(4./3.)/((float)box_width/box_height), 0.0, 0.0, 0.0, + 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, }}; - - static void ui_init(UIState *s) { memset(s, 0, sizeof(UIState)); pthread_mutex_init(&s->lock, NULL); + pthread_cond_init(&s->bg_cond, NULL); // init connections + + s->thermal_sock = zsock_new_sub(">tcp://127.0.0.1:8005", ""); + assert(s->thermal_sock); + s->thermal_sock_raw = zsock_resolve(s->thermal_sock); + s->model_sock = zsock_new_sub(">tcp://127.0.0.1:8009", ""); assert(s->model_sock); s->model_sock_raw = zsock_resolve(s->model_sock); @@ -245,6 +306,9 @@ static void ui_init(UIState *s) { assert(s->livempc_sock); s->livempc_sock_raw = zsock_resolve(s->livempc_sock); + s->plus_sock = zsock_new_sub(">tcp://127.0.0.1:8037", ""); + assert(s->plus_sock); + s->plus_sock_raw = zsock_resolve(s->plus_sock); s->ipc_fd = -1; @@ -252,13 +316,19 @@ static void ui_init(UIState *s) { s->fb = framebuffer_init("ui", 0x00010000, true, &s->display, &s->surface, &s->fb_w, &s->fb_h); assert(s->fb); + set_awake(s, true); // init drawing s->vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG); assert(s->vg); - s->font = nvgCreateFont(s->vg, "Bold", "../assets/courbd.ttf"); - assert(s->font >= 0); + + s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/courbd.ttf"); + assert(s->font_courbd >= 0); + s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/OpenSans-Regular.ttf"); + assert(s->font_sans_regular >= 0); + s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/OpenSans-SemiBold.ttf"); + assert(s->font_sans_semibold >= 0); // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); @@ -374,6 +444,12 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs, } } +static bool ui_alert_active(UIState *s) { + return (nanos_since_boot() - s->scene.alert_ts) < 20000000000ULL && + strlen(s->scene.alert_text1) > 0 && + s->scene.alert_size == cereal_Live100Data_AlertSize_full; +} + static void ui_update_frame(UIState *s) { assert(glGetError() == GL_NO_ERROR); @@ -476,6 +552,14 @@ static void draw_cross(UIState *s, float x_in, float y_in, float sz, NVGcolor co const vec4 p_car_space = (vec4){{x_in, y_in, 0., 1.}}; const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space); + // scale with distance + // x_in = 0 -> sz = 30 (max) + // x_in = 90 -> sz = 15 (min) + sz *= 30; + sz /= (x_in / 3 + 30); + if (sz > 30) sz = 30; + if (sz < 15) sz = 15; + float x = p_full_frame.v[0]; float y = p_full_frame.v[1]; if (x >= 0 && y >= 0.) { @@ -590,22 +674,7 @@ static void draw_model_path(UIState *s, const PathData path, NVGcolor color) { draw_path(s, path.points, var, color); } -static double calc_curvature(float v_ego, float angle_steers) { - const double deg_to_rad = M_PI / 180.0f; - const double slip_fator = 0.0014; - const double steer_ratio = 15.3; - const double wheel_base = 2.67; - - const double angle_offset = 0.0; - - double angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad; - double curvature = angle_steers_rad / (steer_ratio * wheel_base * - (1. + slip_fator * v_ego * v_ego)); - return curvature; -} - -static void draw_steering(UIState *s, float v_ego, float angle_steers) { - double curvature = calc_curvature(v_ego, angle_steers); +static void draw_steering(UIState *s, float curvature) { float points[50]; for (int i = 0; i < 50; i++) { @@ -711,60 +780,41 @@ static void ui_draw_world(UIState *s) { return; } - draw_steering(s, scene->v_ego, scene->angle_steers); + //draw_steering(s, scene->curvature); if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { + int left_lane_color = (int)(255 * scene->model.left_lane.prob); + int right_lane_color = (int)(255 * scene->model.right_lane.prob); draw_model_path( s, scene->model.left_lane, - nvgRGBA(0, (int)(255 * scene->model.left_lane.prob), 0, 128)); + nvgRGBA(left_lane_color, left_lane_color, left_lane_color, 128)); draw_model_path( s, scene->model.right_lane, - nvgRGBA(0, (int)(255 * scene->model.right_lane.prob), 0, 128)); + nvgRGBA(right_lane_color, right_lane_color, right_lane_color, 128)); // draw paths - if (!s->passive) { - draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(128, 0, 255, 255)); - - draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 49, nvgRGBA(255, 0, 0, 255)); - } - } - - if (scene->lead_status) { - char radar_str[16]; - - // Draw background for radar text - ui_draw_rounded_rect(s->vg, 578, 0, 195, 180, 20, nvgRGBA(10,10,10,170)); + draw_path(s, scene->model.path.points, 0.0f, nvgRGBA(0xc0, 0xc0, 0xc0, 255)); - if (s->is_metric) { - int lead_v_rel = (int)(3.6 * scene->lead_v_rel); - snprintf(radar_str, sizeof(radar_str), "%3d m %+d kph", - (int)(scene->lead_d_rel), lead_v_rel); - } else { - int lead_v_rel = (int)(2.236 * scene->lead_v_rel); - snprintf(radar_str, sizeof(radar_str), "%3d m %+d mph", - (int)(scene->lead_d_rel), lead_v_rel); + // draw MPC only if engaged + if (scene->engaged) { + draw_x_y(s, &scene->mpc_x[1], &scene->mpc_y[1], 19, nvgRGBA(255, 0, 0, 255)); } - nvgFontSize(s->vg, 96.0f); - nvgFillColor(s->vg, nvgRGBA(200, 200, 0, 192)); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); - nvgText(s->vg, 1920 / 2 - 20, 40, radar_str, NULL); - - // 2.7 m fudge factor - draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 15, - nvgRGBA(255, 0, 0, 128)); } } static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; - // if (scene->engaged) { - // glClearColor(1.0, 0.5, 0.0, 1.0); - // } else { - glClearColor(0.1, 0.1, 0.1, 1.0); - glClear(GL_COLOR_BUFFER_BIT); + glClearColor(0.0, 0.0, 0.0, 0.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + // hack for eon ui + glEnable(GL_SCISSOR_TEST); + glScissor(box_x, s->fb_h-(box_y+box_height), box_width, box_height); + glViewport(box_x, s->fb_h-(box_y+box_height), box_width, box_height); draw_frame(s); + glViewport(0, 0, s->fb_w, s->fb_h); + glDisable(GL_SCISSOR_TEST); // nvg drawings glEnable(GL_BLEND); @@ -775,79 +825,26 @@ static void ui_draw_vision(UIState *s) { nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); - if (!scene->frontview) { - ui_draw_transformed_box(s, 0xFF00FF00); - ui_draw_world(s); - - // draw speed - char speed_str[16]; - float defaultfontsize = 128.0f; - float labelfontsize = 65.0f; - - if (!s->passive) { - - // background - ui_draw_rounded_rect(s->vg, -15, 0, 570, 180, 20, nvgRGBA(10,10,10,170)); - - if (scene->engaged) { - nvgFillColor(s->vg, nvgRGBA(255, 128, 0, 192)); + nvgSave(s->vg); - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: On", NULL); - } else { - nvgFillColor(s->vg, nvgRGBA(195, 195, 195, 192)); + // hack for eon ui + const int inner_height = box_width*9/16; + nvgScissor(s->vg, box_x, box_y, box_width, box_height); + nvgTranslate(s->vg, box_x, box_y + (box_height-inner_height)/2.0); + nvgScale(s->vg, (float)box_width / s->fb_w, (float)inner_height / s->fb_h); - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 20, 175-30, "OpenPilot: Off", NULL); - } + if (!scene->frontview) { + // ui_draw_transformed_box(s, 0xFF00FF00); + ui_draw_world(s); - nvgFontSize(s->vg, defaultfontsize); - if (scene->v_cruise != 255 && scene->v_cruise != 0) { - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d KPH", - (int)(scene->v_cruise + 0.5)); - } else { - /* Convert KPH to MPH. Using an approximated mph to kph - conversion factor of 1.609 because this is what the Honda - hud seems to be using */ - snprintf(speed_str, sizeof(speed_str), "%3d MPH", - (int)(scene->v_cruise * 0.621504 + 0.5)); - } - nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 480, 95, speed_str, NULL); - } + if (scene->lead_status) { + draw_cross(s, scene->lead_d_rel + 2.7, scene->lead_y_rel, 30, + nvgRGBA(255, 0, 0, 128)); } + const float label_size = 65.0f; - // speed background - ui_draw_rounded_rect(s->vg, 1920-530, 0, 150, 180, 20, nvgRGBA(10,10,10,170)); - - // Add label - nvgFontSize(s->vg, labelfontsize); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192)); - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 1920 - 475, 175-30, "Current Speed", NULL); - /******************************************/ - - nvgFontSize(s->vg, defaultfontsize); - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 192)); - if (s->is_metric) { - snprintf(speed_str, sizeof(speed_str), "%3d KPH", - (int)(scene->v_ego * 3.6 + 0.5)); - } else { - snprintf(speed_str, sizeof(speed_str), "%3d MPH", - (int)(scene->v_ego * 2.237 + 0.5)); - } - nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 1920 - 500, 95, speed_str, NULL); - - /*nvgFontSize(s->vg, 64.0f); - nvgTextAlign(s->vg, NVG_ALIGN_RIGHT | NVG_ALIGN_BASELINE); - nvgText(s->vg, 100+450-20, 1080-100, "mph", NULL);*/ + nvgFontFace(s->vg, "courbd"); if (scene->awareness_status > 0) { nvgBeginPath(s->vg); @@ -860,22 +857,163 @@ static void ui_draw_vision(UIState *s) { // Draw calibration progress (if needed) if (scene->cal_status == CALIBRATION_UNCALIBRATED) { - int rec_width = 1020; - int x_pos = 470; + int rec_width = 1120; + int x_pos = 500; nvgBeginPath(s->vg); nvgStrokeWidth(s->vg, 14); - nvgRoundedRect(s->vg, (1920-rec_width)/2, 970, rec_width, 100, 20); + nvgRoundedRect(s->vg, (1920-rec_width)/2, 920, rec_width, 225, 20); nvgStroke(s->vg); - nvgFillColor(s->vg, nvgRGBA(10,100,220,180)); + nvgFillColor(s->vg, nvgRGBA(0,0,0,180)); nvgFill(s->vg); - nvgFontSize(s->vg, labelfontsize); + nvgFontSize(s->vg, 40*2.5); nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_BASELINE); + nvgFontFace(s->vg, "sans-semibold"); nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 220)); - char calib_status_str[32]; - snprintf(calib_status_str, sizeof(calib_status_str), "Calibration In Progress: %d%%", scene->cal_perc); - nvgText(s->vg, x_pos, 1040, calib_status_str, NULL); + char calib_status_str[64]; + snprintf(calib_status_str, sizeof(calib_status_str), "Calibration in Progress: %d%%", scene->cal_perc); + + nvgText(s->vg, x_pos, 1010, calib_status_str, NULL); + if (s->is_metric) { + nvgText(s->vg, x_pos + 120, 1110, "Drive above 72 km/h", NULL); + } else { + nvgText(s->vg, x_pos + 120, 1110, "Drive above 45 mph", NULL); + } + } + } + + nvgRestore(s->vg); + + + if (!ui_alert_active(s) && !scene->frontview) { + // draw top bar + + const int bar_x = box_x; + const int bar_y = box_y; + const int bar_width = box_width; + const int bar_height = 250 - box_y; + + assert(s->status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[s->status]; + + nvgBeginPath(s->vg); + nvgRect(s->vg, bar_x, bar_y, bar_width, bar_height); + nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); + nvgFill(s->vg); + + const int message_y = box_y; + const int message_height = bar_height; + const int message_width = 800; + const int message_x = box_x + box_width / 2 - message_width / 2; + + // message background + nvgBeginPath(s->vg); + NVGpaint bg = nvgLinearGradient(s->vg, message_x, message_y, message_x, message_y+message_height, + nvgRGBAf(0.0, 0.0, 0.0, 0.0), nvgRGBAf(0.0, 0.0, 0.0, 0.1)); + nvgFillPaint(s->vg, bg); + nvgRect(s->vg, message_x, message_y, message_width, message_height); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + + if (s->passive) { + if (s->scene.started_ts > 0) { + // draw drive time when passive + + uint64_t dt = nanos_since_boot() - s->scene.started_ts; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + char time_str[64]; + if (dt > 60*60*1000000000ULL) { + // hours + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d:%02d", + (int)(dt/(60*60*1000000000ULL)), + (int)((dt%(60*60*1000000000ULL))/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } else { + snprintf(time_str, sizeof(time_str), "Drive time: %d:%02d", + (int)(dt/(60*1000000000ULL)), + (int)(dt%(60*1000000000ULL)/1000000000ULL)); + } + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, time_str, NULL); + } + } else { + // status text + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 48*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + if (s->scene.alert_size == cereal_Live100Data_AlertSize_small) { + nvgFontSize(s->vg, 40*2.5); + nvgText(s->vg, message_x+message_width/2, 115, s->scene.alert_text1, NULL); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, message_x+message_width/2, 185, s->scene.alert_text2, NULL); + } else if (s->status == STATUS_DISENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "DISENGAGED", NULL); + } else if (s->status == STATUS_ENGAGED) { + nvgText(s->vg, message_x+message_width/2, message_y+message_height/2+15, "ENGAGED", NULL); + } + } + + // set speed + const int left_x = bar_x; + const int left_y = bar_y; + const int left_width = (bar_width - message_width) / 2; + const int left_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->v_cruise != 255 && scene->v_cruise != 0) { + char speed_str[16]; + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%3d kph", + (int)(scene->v_cruise + 0.5)); + } else { + /* Convert KPH to MPH. Using an approximated mph to kph + conversion factor of 1.609 because this is what the Honda + hud seems to be using */ + snprintf(speed_str, sizeof(speed_str), "%3d mph", + (int)(scene->v_cruise * 0.621504 + 0.5)); + } + nvgText(s->vg, left_x+left_width/2, 115, speed_str, NULL); + } else { + nvgText(s->vg, left_x+left_width/2, 115, "N/A", NULL); + } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, left_x+left_width/2, 185, "SET SPEED", NULL); + + // lead car + const int right_y = bar_y; + const int right_width = (bar_width - message_width) / 2; + const int right_x = bar_x+bar_width-right_width; + const int right_height = bar_height; + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 40*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + if (scene->lead_status) { + char radar_str[16]; + // lead car is always in meters + if (s->is_metric || true) { + snprintf(radar_str, sizeof(radar_str), "%d m", (int)scene->lead_d_rel); + } else { + snprintf(radar_str, sizeof(radar_str), "%d ft", (int)(scene->lead_d_rel * 3.28084)); + } + nvgText(s->vg, right_x+right_width/2, 115, radar_str, NULL); + } else { + nvgText(s->vg, right_x+right_width/2, 115, "N/A", NULL); } + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 26*2.5); + nvgText(s->vg, right_x+right_width/2, 185, "LEAD CAR", NULL); } nvgEndFrame(s->vg); @@ -887,39 +1025,41 @@ static void ui_draw_vision(UIState *s) { static void ui_draw_alerts(UIState *s) { const UIScene *scene = &s->scene; - // dont draw alerts that are outdated by > 20 secs - if ((nanos_since_boot() - scene->alert_ts) >= 20000000000ULL) { - return; - } + if (!ui_alert_active(s)) return; - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + assert(s->status < ARRAYSIZE(alert_colors)); + const uint8_t *color = alert_colors[s->status]; - glClear(GL_STENCIL_BUFFER_BIT); + char alert_text1_upper[1024] = {0}; + for (int i=0; scene->alert_text1[i] && i < sizeof(alert_text1_upper)-1; i++) { + alert_text1_upper[i] = toupper(scene->alert_text1[i]); + } - nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + nvgBeginPath(s->vg); + nvgRect(s->vg, box_x, box_y, box_width, box_height); + nvgFillColor(s->vg, nvgRGBA(color[0], color[1], color[2], color[3])); + nvgFill(s->vg); - // draw alert text - if (strlen(scene->alert_text1) > 0) { - nvgBeginPath(s->vg); - nvgRoundedRect(s->vg, 100, 200, 1700, 800, 40); - nvgFillColor(s->vg, nvgRGBA(10, 10, 10, 220)); - nvgFill(s->vg); - nvgFontSize(s->vg, 200.0f); - nvgFillColor(s->vg, nvgRGBA(255, 0, 0, 255)); - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); - nvgTextBox(s->vg, 100 + 50, 200 + 50, 1700 - 50, scene->alert_text1, - NULL); - if (strlen(scene->alert_text2) > 0) { - nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); - nvgFontSize(s->vg, 100.0f); - nvgTextBox(s->vg, 100 + 50, 200 + 550, 1700 - 2*50, scene->alert_text2, NULL); - } + nvgFontFace(s->vg, "sans-semibold"); + + if (strlen(alert_text1_upper) > 15) { + nvgFontSize(s->vg, 72.0*2.5); + } else { + nvgFontSize(s->vg, 96.0*2.5); } + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, box_x + 50, box_y + 287, box_width - 50, alert_text1_upper, NULL); - nvgEndFrame(s->vg); - glDisable(GL_BLEND); + if (strlen(scene->alert_text2) > 0) { + + nvgFontFace(s->vg, "sans-regular"); + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 44.0*2.5); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BOTTOM); + nvgTextBox(s->vg, box_x + 50, box_y + box_height - 250, box_width - 50, scene->alert_text2, NULL); + } } static void ui_draw_blank(UIState *s) { @@ -927,14 +1067,64 @@ static void ui_draw_blank(UIState *s) { glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } +static void ui_draw_aside(UIState *s) { + char speed_str[32]; + float speed; + + bool is_cruise_set = (s->scene.v_cruise != 0 && s->scene.v_cruise != 255); + unsigned long last_cruise_update_dt = (nanos_since_boot() - s->scene.v_cruise_update_ts); + bool should_draw_cruise_speed = is_cruise_set && last_cruise_update_dt < 2000000000ULL; + if (should_draw_cruise_speed) { + speed = s->scene.v_cruise / 3.6; + nvgFillColor(s->vg, nvgRGBA(0xFF, 0xD8, 0xAC, 0xFF)); + } else { + speed = s->scene.v_ego; + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + } + + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE); + + nvgFontFace(s->vg, "sans-semibold"); + nvgFontSize(s->vg, 110); + if (s->is_metric) { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 3.6 + 0.5)); + } else { + snprintf(speed_str, sizeof(speed_str), "%d", (int)(speed * 2.2374144 + 0.5)); + } + nvgText(s->vg, 150, 762, speed_str, NULL); + + nvgFontFace(s->vg, "sans-regular"); + nvgFontSize(s->vg, 70); + + if (s->is_metric) { + nvgText(s->vg, 150, 817, "kph", NULL); + } else { + nvgText(s->vg, 150, 817, "mph", NULL); + } +} + static void ui_draw(UIState *s) { - if (s->vision_connected) { + if (s->vision_connected && s->plus_state == 0) { ui_draw_vision(s); } else { ui_draw_blank(s); } - ui_draw_alerts(s); + { + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glClear(GL_STENCIL_BUFFER_BIT); + + nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f); + + if (s->vision_connected) { + ui_draw_aside(s); + } + ui_draw_alerts(s); + + nvgEndFrame(s->vg); + glDisable(GL_BLEND); + } eglSwapBuffers(s->display, s->surface); assert(glGetError() == GL_NO_ERROR); @@ -977,6 +1167,14 @@ static ModelData read_model(cereal_ModelData_ptr modelp) { return d; } +static void update_status(UIState *s, int status) { + if (s->status != status) { + s->status = status; + // wake up bg thread to change + pthread_cond_signal(&s->bg_cond); + } +} + static void ui_update(UIState *s) { int err; @@ -1021,7 +1219,7 @@ static void ui_update(UIState *s) { // poll for events while (true) { - zmq_pollitem_t polls[6] = {{0}}; + zmq_pollitem_t polls[8] = {{0}}; polls[0].socket = s->live100_sock_raw; polls[0].events = ZMQ_POLLIN; polls[1].socket = s->livecalibration_sock_raw; @@ -1032,17 +1230,20 @@ static void ui_update(UIState *s) { polls[3].events = ZMQ_POLLIN; polls[4].socket = s->livempc_sock_raw; polls[4].events = ZMQ_POLLIN; + polls[5].socket = s->thermal_sock_raw; + polls[5].events = ZMQ_POLLIN; + + polls[6].socket = s->plus_sock_raw; + polls[6].events = ZMQ_POLLIN; - int num_polls = 5; + int num_polls = 7; if (s->vision_connected) { assert(s->ipc_fd >= 0); - polls[5].fd = s->ipc_fd; - polls[5].events = ZMQ_POLLIN; + polls[7].fd = s->ipc_fd; + polls[7].events = ZMQ_POLLIN; num_polls++; } - - int ret = zmq_poll(polls, num_polls, 0); if (ret < 0) { LOGW("poll failed (%d)", ret); @@ -1052,10 +1253,13 @@ static void ui_update(UIState *s) { break; } - // awake on any activity - set_awake(s, true); + if (polls[0].revents || polls[1].revents || polls[2].revents || + polls[3].revents || polls[4].revents) { + // awake on any (old) activity + set_awake(s, true); + } - if (s->vision_connected && polls[5].revents) { + if (s->vision_connected && polls[7].revents) { // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1104,10 +1308,25 @@ static void ui_update(UIState *s) { } else { assert(false); } + } else if (polls[6].revents) { + // plus socket + + zmq_msg_t msg; + err = zmq_msg_init(&msg); + assert(err == 0); + err = zmq_msg_recv(&msg, s->plus_sock_raw, 0); + assert(err >= 0); + + assert(zmq_msg_size(&msg) == 1); + + s->plus_state = ((char*)zmq_msg_data(&msg))[0]; + + zmq_msg_close(&msg); + } else { // zmq messages void* which = NULL; - for (int i=0; i<5; i++) { + for (int i=0; i<6; i++) { if (polls[i].revents) { which = polls[i].socket; break; @@ -1135,9 +1354,12 @@ static void ui_update(UIState *s) { struct cereal_Live100Data datad; cereal_read_Live100Data(&datad, eventd.live100); + if (datad.vCruise != s->scene.v_cruise) { + s->scene.v_cruise_update_ts = eventd.logMonoTime; + } s->scene.v_cruise = datad.vCruise; s->scene.v_ego = datad.vEgo; - s->scene.angle_steers = datad.angleSteers; + s->scene.curvature = datad.curvature; s->scene.engaged = datad.enabled; // printf("recv %f\n", datad.vEgo); @@ -1156,6 +1378,18 @@ static void ui_update(UIState *s) { s->scene.alert_ts = eventd.logMonoTime; + s->scene.alert_size = datad.alertSize; + + if (datad.alertStatus == cereal_Live100Data_AlertStatus_userPrompt) { + update_status(s, STATUS_WARNING); + } else if (datad.alertStatus == cereal_Live100Data_AlertStatus_critical) { + update_status(s, STATUS_ALERT); + } else if (datad.enabled) { + update_status(s, STATUS_ENGAGED); + } else { + update_status(s, STATUS_DISENGAGED); + } + } else if (eventd.which == cereal_Event_live20) { struct cereal_Live20Data datad; cereal_read_Live20Data(&datad, eventd.live20); @@ -1206,6 +1440,18 @@ static void ui_update(UIState *s) { for (int i = 0; i < 50; i++){ s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); } + } else if (eventd.which == cereal_Event_thermal) { + struct cereal_ThermalData datad; + cereal_read_ThermalData(&datad, eventd.thermal); + + if (!datad.started) { + update_status(s, STATUS_STOPPED); + } else if (s->status == STATUS_STOPPED) { + // car is started but controls doesn't have fingerprint yet + update_status(s, STATUS_DISENGAGED); + } + + s->scene.started_ts = datad.startedTs; } capn_free(&ctx); @@ -1217,12 +1463,6 @@ static void ui_update(UIState *s) { } -volatile int do_exit = 0; -static void set_do_exit(int sig) { - do_exit = 1; -} - - static void* vision_connect_thread(void *args) { int err; @@ -1290,6 +1530,87 @@ static void* vision_connect_thread(void *args) { return NULL; } +#include +#include + +#define SENSOR_LIGHT 7 + +static void* light_sensor_thread(void *args) { + int err; + + UIState *s = args; + s->light_sensor = 0.0; + + struct sensors_poll_device_t* device; + struct sensors_module_t* module; + + hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module); + sensors_open(&module->common, &device); + + // need to do this + struct sensor_t const* list; + int count = module->get_sensors_list(module, &list); + + device->activate(device, SENSOR_LIGHT, 0); + device->activate(device, SENSOR_LIGHT, 1); + device->setDelay(device, SENSOR_LIGHT, ms2ns(100)); + + while (!do_exit) { + static const size_t numEvents = 1; + sensors_event_t buffer[numEvents]; + + int n = device->poll(device, buffer, numEvents); + if (n < 0) { + LOG_100("light_sensor_poll failed: %d", n); + } + if (n > 0) { + s->light_sensor = buffer[0].light; + //printf("new light sensor value: %f\n", s->light_sensor); + } + } + + return NULL; +} + + +static void* bg_thread(void* args) { + UIState *s = args; + + EGLDisplay bg_display; + EGLSurface bg_surface; + + FramebufferState *bg_fb = framebuffer_init("bg", 0x00001000, false, + &bg_display, &bg_surface, NULL, NULL); + assert(bg_fb); + + bool first = true; + while(!do_exit) { + pthread_mutex_lock(&s->lock); + + if (first) { + first = false; + } else { + pthread_cond_wait(&s->bg_cond, &s->lock); + } + + assert(s->status < ARRAYSIZE(bg_colors)); + const uint8_t *color = bg_colors[s->status]; + + pthread_mutex_unlock(&s->lock); + + glClearColor(color[0]/256.0, color[1]/256.0, color[2]/256.0, 0.0); + glClear(GL_COLOR_BUFFER_BIT); + + + eglSwapBuffers(bg_display, bg_surface); + assert(glGetError() == GL_NO_ERROR); + + } + + return NULL; +} + + int main() { int err; @@ -1305,12 +1626,43 @@ int main() { vision_connect_thread, s); assert(err == 0); + pthread_t light_sensor_thread_handle; + err = pthread_create(&light_sensor_thread_handle, NULL, + light_sensor_thread, s); + assert(err == 0); + + pthread_t bg_thread_handle; + err = pthread_create(&bg_thread_handle, NULL, + bg_thread, s); + assert(err == 0); + TouchState touch = {0}; touch_init(&touch); + // light sensor scaling params + #define LIGHT_SENSOR_M 1.3 + #define LIGHT_SENSOR_B 5.0 + + #define NEO_BRIGHTNESS 100 + + float smooth_light_sensor = LIGHT_SENSOR_B; + + const int EON = (access("/EON", F_OK) != -1); + while (!do_exit) { pthread_mutex_lock(&s->lock); + if (EON) { + // light sensor is only exposed on EONs + float clipped_light_sensor = (s->light_sensor*LIGHT_SENSOR_M) + LIGHT_SENSOR_B; + if (clipped_light_sensor > 255) clipped_light_sensor = 255; + smooth_light_sensor = clipped_light_sensor * 0.01 + smooth_light_sensor * 0.99; + set_brightness((int)smooth_light_sensor); + } else { + // compromise for bright and dark envs + set_brightness(NEO_BRIGHTNESS); + } + ui_update(s); if (s->awake) { ui_draw(s); @@ -1339,6 +1691,13 @@ int main() { set_awake(s, true); + // wake up bg thread to exit + pthread_mutex_lock(&s->lock); + pthread_cond_signal(&s->bg_cond); + pthread_mutex_unlock(&s->lock); + err = pthread_join(bg_thread_handle, NULL); + assert(err == 0); + err = pthread_join(connect_thread_handle, NULL); assert(err == 0); diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 2790342b6ce565..b87b0a9162c76e 100644 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -7,9 +7,6 @@ import subprocess def main(gctx=None): - if not os.getenv("CLEAN"): - return - while True: # try network r = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 7a98efab048d92..1a1b65f2119c93 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ