Merge remote-tracking branch 'upstream/master' into disengage-on-gas

.github/PULL_REQUEST_TEMPLATE/car_port.md

@@ -9,7 +9,7 @@ assignees: ''
 **Checklist**
 
 - [ ] added to README
-- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_models.py)
+- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py)
 - [ ] route with openpilot:
 - [ ] route with stock system:
 - [ ] car harness used (if comma doesn't sell it, put N/A):

.github/workflows/selfdrive_tests.yaml

@@ -245,7 +245,6 @@ jobs:
     - name: Run unit tests
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --test && \
-                        coverage run selfdrive/test/test_fingerprints.py && \
                         $UNIT_TEST common && \
                         $UNIT_TEST opendbc/can && \
                         $UNIT_TEST selfdrive/boardd && \
@@ -384,7 +383,7 @@ jobs:
       uses: actions/cache@v2
       with:
         path: /tmp/comma_download_cache
-        key: car_models-${{ hashFiles('selfdrive/test/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
+        key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
     - name: Cache scons
       id: scons-cache
       # TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
@@ -402,7 +401,7 @@ jobs:
     - name: Test car models
       run: |
         ${{ env.RUN }} "scons -j$(nproc) --test && \
-                        FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \
+                        FILEREADER_CACHE=1 coverage run -m pytest selfdrive/car/tests/test_models.py && \
                         coverage xml && \
                         chmod -R 777 /tmp/comma_download_cache"
       env:

SConstruct

@@ -89,7 +89,6 @@ if arch == "aarch64" or arch == "larch64":
     "/usr/local/lib",
     "/usr/lib",
     "/system/vendor/lib64",
-    "/system/comma/usr/lib",
     f"#third_party/acados/{arch}/lib",
   ]
 
@@ -172,8 +171,8 @@ if arch != "Darwin":
   ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
 
 # Enable swaglog include in submodules
-cflags += ["-DSWAGLOG"]
-cxxflags += ["-DSWAGLOG"]
+cflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
+cxxflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
 
 env = Environment(
   ENV=lenv,
@@ -306,11 +305,11 @@ if arch == "Darwin":
   qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
   qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin"))
 elif arch == "aarch64":
-  qt_env['QTDIR'] = "/system/comma/usr"
+  qt_env['QTDIR'] = "/usr"
   qt_dirs = [
-    f"/system/comma/usr/include/qt",
+    f"/usr/include/qt",
   ]
-  qt_dirs += [f"/system/comma/usr/include/qt/Qt{m}" for m in qt_modules]
+  qt_dirs += [f"/usr/include/qt/Qt{m}" for m in qt_modules]
 
   qt_libs = [f"Qt5{m}" for m in qt_modules]
   qt_libs += ['EGL', 'GLESv3', 'c++_shared']

cereal

@@ -1 +1 @@
-Subproject commit 28d458a9af49b38bd0a9052f09fbe927324320fb
+Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b

common/conversions.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+class Conversions:
+  # Speed
+  MPH_TO_KPH = 1.609344
+  KPH_TO_MPH = 1. / MPH_TO_KPH
+  MS_TO_KPH = 3.6
+  KPH_TO_MS = 1. / MS_TO_KPH
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
+  MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+
+  # Angle
+  DEG_TO_RAD = np.pi / 180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
+
+  # Mass
+  LB_TO_KG = 0.453592

docs/CARS.md

@@ -44,9 +44,10 @@
 | Lexus     | RX Hybrid 2020-21             | All               | openpilot        | 0mph               | 0mph              |
 | Lexus     | UX Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
 | Toyota    | Alphard 2019-20               | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Avalon 2016-21                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon 2016-18                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon 2019-21                | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Avalon 2022                   | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
 | Toyota    | Camry 2018-20                 | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
 | Toyota    | Camry 2021-22                 | All               | openpilot        | 0mph<sup>4</sup>   | 0mph              |
 | Toyota    | Camry Hybrid 2018-20          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
@@ -86,7 +87,7 @@
 | Audi      | A3 Sportback e-tron 2017-18     | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | Q2 2018                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Audi      | Q3 2020-21                      | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
-| Audi      | S3 2015                         | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Audi      | S3 2015-17                      | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
 | Cadillac  | Escalade ESV 2016<sup>1</sup>   | ACC + LKAS        | openpilot        | 0mph               | 7mph         |
 | Chevrolet | Volt 2017-18<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
 | Chrysler  | Pacifica 2017-18                | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
@@ -123,7 +124,7 @@
 | Kia       | Forte 2018-21                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | K5 2021-22                      | SCC + LFA         | Stock            | 0mph               | 0mph         |
 | Kia       | Niro EV 2019-22                 | All               | Stock            | 0mph               | 0mph         |
-| Kia       | Niro Hybrid 2021                | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Niro Hybrid 2021-22             | SCC + LKAS        | Stock            | 0mph               | 0mph         |
 | Kia       | Niro PHEV 2019                  | SCC + LKAS        | Stock            | 10mph              | 32mph        |
 | Kia       | Optima 2017                     | SCC + LKAS        | Stock            | 0mph               | 32mph        |
 | Kia       | Optima 2019                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
@@ -169,7 +170,7 @@
 | Volkswagen| T-Cross 2021<sup>4</sup>        | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| T-Roc 2021<sup>4</sup>          | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Taos 2022<sup>4</sup>           | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Tiguan 2020                     | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Tiguan 2020-22<sup>4</sup>      | Driver Assistance | Stock            | 0mph               | 0mph         |
 | Volkswagen| Touran 2017                     | Driver Assistance | Stock            | 0mph               | 0mph         |
 
 <sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />

laika_repo

@@ -1 +1 @@
-Subproject commit dec99a0f77328f7a9f104020d98d7227345d1288
+Subproject commit 94066cb2b4ad5f2bcb8e33ce02fe15a73a00aace

models/big_supercombo.dlc

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
-size 72718099

models/big_supercombo.onnx

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
-size 73147489

models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:209e9544e456dbc2a7d60490da65154e129bc84830909d8d931f97b3df93949b
-size 56684955
+oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
+size 72718099

models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2365bae967cce21ce68707c30bf2981bb7081ee5c3e6a3dff793e660f23ff622
-size 57554657
+oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
+size 73147489

opendbc

@@ -1 +1 @@
-Subproject commit 46a942d6790531cf5b94b14266140e43afcfda3e
+Subproject commit eab58f6f8f41f255920365ab1fd9c75e312a6869

panda

@@ -1 +1 @@
-Subproject commit 51ccb9fbd266796e1bf6ffda8b93c4119ab09ff4
+Subproject commit 5a7af82f06bf5f8039df8f58f9b1114f7d3436ee

pyextra/acados_template/acados_layout.json

@@ -5,6 +5,9 @@
     "acados_include_path": [
         "str"
     ],
+    "cython_include_dirs": [
+        "str"
+    ],
     "model": {
          "name" : [
             "str"
@@ -23,7 +26,15 @@
         ],
         "dyn_disc_fun" : [
             "str"
-        ]
+        ],
+        "gnsf" : {
+            "nontrivial_f_LO": [
+                "int"
+            ],
+            "purely_linear": [
+                "int"
+            ]
+        }
     },
     "parameter_values": [
         "ndarray",
@@ -693,6 +704,18 @@
         "alpha_reduction": [
             "float"
         ],
+        "line_search_use_sufficient_descent": [
+            "int"
+        ],
+        "globalization_use_SOC": [
+            "int"
+        ],
+        "full_step_dual": [
+            "int"
+        ],
+        "eps_sufficient_descent": [
+            "float"
+        ],
         "sim_method_num_stages": [
             "ndarray",
             [

pyextra/acados_template/acados_model.py

@@ -78,6 +78,13 @@ class AcadosModel():
         self.dyn_disc_fun_jac = None  #: name of function discrete dyanamics + jacobian; Default: :code:`None`
         self.dyn_disc_fun = None  #: name of function discrete dyanamics; Default: :code:`None`
 
+        # for GNSF models
+        self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
+        """
+        dictionary containing information on GNSF structure needed when rendering templates.
+        Contains integers `nontrivial_f_LO`, `purely_linear`.
+        """
+
         ## for OCP
         # constraints
         self.con_h_expr = None  #: CasADi expression for the constraint :math:`h`; Default: :code:`None`

pyextra/acados_template/acados_ocp.py

@@ -270,28 +270,28 @@ class AcadosOcpDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -312,49 +312,49 @@ class AcadosOcpDims:
 
     @ny_e.setter
     def ny_e(self, ny_e):
-        if type(ny_e) == int and ny_e > -1:
+        if isinstance(ny_e, int) and ny_e > -1:
             self.__ny_e = ny_e
         else:
             raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
 
     @nr.setter
     def nr(self, nr):
-        if type(nr) == int and nr > -1:
+        if isinstance(nr, int) and nr > -1:
             self.__nr = nr
         else:
             raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
 
     @nr_e.setter
     def nr_e(self, nr_e):
-        if type(nr_e) == int and nr_e > -1:
+        if isinstance(nr_e, int) and nr_e > -1:
             self.__nr_e = nr_e
         else:
             raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
 
     @nh.setter
     def nh(self, nh):
-        if type(nh) == int and nh > -1:
+        if isinstance(nh, int) and nh > -1:
             self.__nh = nh
         else:
             raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
 
     @nh_e.setter
     def nh_e(self, nh_e):
-        if type(nh_e) == int and nh_e > -1:
+        if isinstance(nh_e, int) and nh_e > -1:
             self.__nh_e = nh_e
         else:
             raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
 
     @nphi.setter
     def nphi(self, nphi):
-        if type(nphi) == int and nphi > -1:
+        if isinstance(nphi, int) and nphi > -1:
             self.__nphi = nphi
         else:
             raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
 
     @nphi_e.setter
     def nphi_e(self, nphi_e):
-        if type(nphi_e) == int and nphi_e > -1:
+        if isinstance(nphi_e, int) and nphi_e > -1:
             self.__nphi_e = nphi_e
         else:
             raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
@@ -375,42 +375,42 @@ class AcadosOcpDims:
 
     @nbx_0.setter
     def nbx_0(self, nbx_0):
-        if type(nbx_0) == int and nbx_0 > -1:
+        if isinstance(nbx_0, int) and nbx_0 > -1:
             self.__nbx_0 = nbx_0
         else:
             raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
 
     @nbx_e.setter
     def nbx_e(self, nbx_e):
-        if type(nbx_e) == int and nbx_e > -1:
+        if isinstance(nbx_e, int) and nbx_e > -1:
             self.__nbx_e = nbx_e
         else:
             raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
 
     @nbu.setter
     def nbu(self, nbu):
-        if type(nbu) == int and nbu > -1:
+        if isinstance(nbu, int) and nbu > -1:
             self.__nbu = nbu
         else:
             raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
 
     @nsbx.setter
     def nsbx(self, nsbx):
-        if type(nsbx) == int and nsbx > -1:
+        if isinstance(nsbx, int) and nsbx > -1:
             self.__nsbx = nsbx
         else:
             raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
 
     @nsbx_e.setter
     def nsbx_e(self, nsbx_e):
-        if type(nsbx_e) == int and nsbx_e > -1:
+        if isinstance(nsbx_e, int) and nsbx_e > -1:
             self.__nsbx_e = nsbx_e
         else:
             raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
 
     @nsbu.setter
     def nsbu(self, nsbu):
-        if type(nsbu) == int and nsbu > -1:
+        if isinstance(nsbu, int) and nsbu > -1:
             self.__nsbu = nsbu
         else:
             raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
@@ -1592,14 +1592,14 @@ class AcadosOcpConstraints:
     # initial x
     @lbx_0.setter
     def lbx_0(self, lbx_0):
-        if type(lbx_0) == np.ndarray:
+        if isinstance(lbx_0, np.ndarray):
             self.__lbx_0 = lbx_0
         else:
             raise Exception('Invalid lbx_0 value. Exiting.')
 
     @ubx_0.setter
     def ubx_0(self, ubx_0):
-        if type(ubx_0) == np.ndarray:
+        if isinstance(ubx_0, np.ndarray):
             self.__ubx_0 = ubx_0
         else:
             raise Exception('Invalid ubx_0 value. Exiting.')
@@ -1613,7 +1613,7 @@ class AcadosOcpConstraints:
 
     @Jbx_0.setter
     def Jbx_0(self, Jbx_0):
-        if type(Jbx_0) == np.ndarray:
+        if isinstance(Jbx_0, np.ndarray):
             self.__idxbx_0 = J_to_idx(Jbx_0)
         else:
             raise Exception('Invalid Jbx_0 value. Exiting.')
@@ -1639,28 +1639,28 @@ class AcadosOcpConstraints:
     # bounds on x
     @lbx.setter
     def lbx(self, lbx):
-        if type(lbx) == np.ndarray:
+        if isinstance(lbx, np.ndarray):
             self.__lbx = lbx
         else:
             raise Exception('Invalid lbx value. Exiting.')
 
     @ubx.setter
     def ubx(self, ubx):
-        if type(ubx) == np.ndarray:
+        if isinstance(ubx, np.ndarray):
             self.__ubx = ubx
         else:
             raise Exception('Invalid ubx value. Exiting.')
 
     @idxbx.setter
     def idxbx(self, idxbx):
-        if type(idxbx) == np.ndarray:
+        if isinstance(idxbx, np.ndarray):
             self.__idxbx = idxbx
         else:
             raise Exception('Invalid idxbx value. Exiting.')
 
     @Jbx.setter
     def Jbx(self, Jbx):
-        if type(Jbx) == np.ndarray:
+        if isinstance(Jbx, np.ndarray):
             self.__idxbx = J_to_idx(Jbx)
         else:
             raise Exception('Invalid Jbx value. Exiting.')
@@ -1668,28 +1668,28 @@ class AcadosOcpConstraints:
     # bounds on u
     @lbu.setter
     def lbu(self, lbu):
-        if type(lbu) == np.ndarray:
+        if isinstance(lbu, np.ndarray):
             self.__lbu = lbu
         else:
             raise Exception('Invalid lbu value. Exiting.')
 
     @ubu.setter
     def ubu(self, ubu):
-        if type(ubu) == np.ndarray:
+        if isinstance(ubu, np.ndarray):
             self.__ubu = ubu
         else:
             raise Exception('Invalid ubu value. Exiting.')
 
     @idxbu.setter
     def idxbu(self, idxbu):
-        if type(idxbu) == np.ndarray:
+        if isinstance(idxbu, np.ndarray):
             self.__idxbu = idxbu
         else:
             raise Exception('Invalid idxbu value. Exiting.')
 
     @Jbu.setter
     def Jbu(self, Jbu):
-        if type(Jbu) == np.ndarray:
+        if isinstance(Jbu, np.ndarray):
             self.__idxbu = J_to_idx(Jbu)
         else:
             raise Exception('Invalid Jbu value. Exiting.')
@@ -1697,28 +1697,28 @@ class AcadosOcpConstraints:
     # bounds on x at shooting node N
     @lbx_e.setter
     def lbx_e(self, lbx_e):
-        if type(lbx_e) == np.ndarray:
+        if isinstance(lbx_e, np.ndarray):
             self.__lbx_e = lbx_e
         else:
             raise Exception('Invalid lbx_e value. Exiting.')
 
     @ubx_e.setter
     def ubx_e(self, ubx_e):
-        if type(ubx_e) == np.ndarray:
+        if isinstance(ubx_e, np.ndarray):
             self.__ubx_e = ubx_e
         else:
             raise Exception('Invalid ubx_e value. Exiting.')
 
     @idxbx_e.setter
     def idxbx_e(self, idxbx_e):
-        if type(idxbx_e) == np.ndarray:
+        if isinstance(idxbx_e, np.ndarray):
             self.__idxbx_e = idxbx_e
         else:
             raise Exception('Invalid idxbx_e value. Exiting.')
 
     @Jbx_e.setter
     def Jbx_e(self, Jbx_e):
-        if type(Jbx_e) == np.ndarray:
+        if isinstance(Jbx_e, np.ndarray):
             self.__idxbx_e = J_to_idx(Jbx_e)
         else:
             raise Exception('Invalid Jbx_e value. Exiting.')
@@ -1742,14 +1742,14 @@ class AcadosOcpConstraints:
 
     @lg.setter
     def lg(self, lg):
-        if type(lg) == np.ndarray:
+        if isinstance(lg, np.ndarray):
             self.__lg = lg
         else:
             raise Exception('Invalid lg value. Exiting.')
 
     @ug.setter
     def ug(self, ug):
-        if type(ug) == np.ndarray:
+        if isinstance(ug, np.ndarray):
             self.__ug = ug
         else:
             raise Exception('Invalid ug value. Exiting.')
@@ -1765,14 +1765,14 @@ class AcadosOcpConstraints:
 
     @lg_e.setter
     def lg_e(self, lg_e):
-        if type(lg_e) == np.ndarray:
+        if isinstance(lg_e, np.ndarray):
             self.__lg_e = lg_e
         else:
             raise Exception('Invalid lg_e value. Exiting.')
 
     @ug_e.setter
     def ug_e(self, ug_e):
-        if type(ug_e) == np.ndarray:
+        if isinstance(ug_e, np.ndarray):
             self.__ug_e = ug_e
         else:
             raise Exception('Invalid ug_e value. Exiting.')
@@ -1780,14 +1780,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints
     @lh.setter
     def lh(self, lh):
-        if type(lh) == np.ndarray:
+        if isinstance(lh, np.ndarray):
             self.__lh = lh
         else:
             raise Exception('Invalid lh value. Exiting.')
 
     @uh.setter
     def uh(self, uh):
-        if type(uh) == np.ndarray:
+        if isinstance(uh, np.ndarray):
             self.__uh = uh
         else:
             raise Exception('Invalid uh value. Exiting.')
@@ -1795,14 +1795,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints
     @lphi.setter
     def lphi(self, lphi):
-        if type(lphi) == np.ndarray:
+        if isinstance(lphi, np.ndarray):
             self.__lphi = lphi
         else:
             raise Exception('Invalid lphi value. Exiting.')
 
     @uphi.setter
     def uphi(self, uphi):
-        if type(uphi) == np.ndarray:
+        if isinstance(uphi, np.ndarray):
             self.__uphi = uphi
         else:
             raise Exception('Invalid uphi value. Exiting.')
@@ -1810,14 +1810,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints at shooting node N
     @lh_e.setter
     def lh_e(self, lh_e):
-        if type(lh_e) == np.ndarray:
+        if isinstance(lh_e, np.ndarray):
             self.__lh_e = lh_e
         else:
             raise Exception('Invalid lh_e value. Exiting.')
 
     @uh_e.setter
     def uh_e(self, uh_e):
-        if type(uh_e) == np.ndarray:
+        if isinstance(uh_e, np.ndarray):
             self.__uh_e = uh_e
         else:
             raise Exception('Invalid uh_e value. Exiting.')
@@ -1825,14 +1825,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints at shooting node N
     @lphi_e.setter
     def lphi_e(self, lphi_e):
-        if type(lphi_e) == np.ndarray:
+        if isinstance(lphi_e, np.ndarray):
             self.__lphi_e = lphi_e
         else:
             raise Exception('Invalid lphi_e value. Exiting.')
 
     @uphi_e.setter
     def uphi_e(self, uphi_e):
-        if type(uphi_e) == np.ndarray:
+        if isinstance(uphi_e, np.ndarray):
             self.__uphi_e = uphi_e
         else:
             raise Exception('Invalid uphi_e value. Exiting.')
@@ -1841,21 +1841,21 @@ class AcadosOcpConstraints:
     # soft bounds on x
     @lsbx.setter
     def lsbx(self, lsbx):
-        if type(lsbx) == np.ndarray:
+        if isinstance(lsbx, np.ndarray):
             self.__lsbx = lsbx
         else:
             raise Exception('Invalid lsbx value. Exiting.')
 
     @usbx.setter
     def usbx(self, usbx):
-        if type(usbx) == np.ndarray:
+        if isinstance(usbx, np.ndarray):
             self.__usbx = usbx
         else:
             raise Exception('Invalid usbx value. Exiting.')
 
     @idxsbx.setter
     def idxsbx(self, idxsbx):
-        if type(idxsbx) == np.ndarray:
+        if isinstance(idxsbx, np.ndarray):
             self.__idxsbx = idxsbx
         else:
             raise Exception('Invalid idxsbx value. Exiting.')
@@ -1870,28 +1870,28 @@ class AcadosOcpConstraints:
     # soft bounds on u
     @lsbu.setter
     def lsbu(self, lsbu):
-        if type(lsbu) == np.ndarray:
+        if isinstance(lsbu, np.ndarray):
             self.__lsbu = lsbu
         else:
             raise Exception('Invalid lsbu value. Exiting.')
 
     @usbu.setter
     def usbu(self, usbu):
-        if type(usbu) == np.ndarray:
+        if isinstance(usbu, np.ndarray):
             self.__usbu = usbu
         else:
             raise Exception('Invalid usbu value. Exiting.')
 
     @idxsbu.setter
     def idxsbu(self, idxsbu):
-        if type(idxsbu) == np.ndarray:
+        if isinstance(idxsbu, np.ndarray):
             self.__idxsbu = idxsbu
         else:
             raise Exception('Invalid idxsbu value. Exiting.')
 
     @Jsbu.setter
     def Jsbu(self, Jsbu):
-        if type(Jsbu) == np.ndarray:
+        if isinstance(Jsbu, np.ndarray):
             self.__idxsbu = J_to_idx_slack(Jsbu)
         else:
             raise Exception('Invalid Jsbu value. Exiting.')
@@ -1899,28 +1899,28 @@ class AcadosOcpConstraints:
     # soft bounds on x at shooting node N
     @lsbx_e.setter
     def lsbx_e(self, lsbx_e):
-        if type(lsbx_e) == np.ndarray:
+        if isinstance(lsbx_e, np.ndarray):
             self.__lsbx_e = lsbx_e
         else:
             raise Exception('Invalid lsbx_e value. Exiting.')
 
     @usbx_e.setter
     def usbx_e(self, usbx_e):
-        if type(usbx_e) == np.ndarray:
+        if isinstance(usbx_e, np.ndarray):
             self.__usbx_e = usbx_e
         else:
             raise Exception('Invalid usbx_e value. Exiting.')
 
     @idxsbx_e.setter
     def idxsbx_e(self, idxsbx_e):
-        if type(idxsbx_e) == np.ndarray:
+        if isinstance(idxsbx_e, np.ndarray):
             self.__idxsbx_e = idxsbx_e
         else:
             raise Exception('Invalid idxsbx_e value. Exiting.')
 
     @Jsbx_e.setter
     def Jsbx_e(self, Jsbx_e):
-        if type(Jsbx_e) == np.ndarray:
+        if isinstance(Jsbx_e, np.ndarray):
             self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
         else:
             raise Exception('Invalid Jsbx_e value. Exiting.')
@@ -1959,21 +1959,21 @@ class AcadosOcpConstraints:
     # soft bounds on nonlinear constraints
     @lsh.setter
     def lsh(self, lsh):
-        if type(lsh) == np.ndarray:
+        if isinstance(lsh, np.ndarray):
             self.__lsh = lsh
         else:
             raise Exception('Invalid lsh value. Exiting.')
 
     @ush.setter
     def ush(self, ush):
-        if type(ush) == np.ndarray:
+        if isinstance(ush, np.ndarray):
             self.__ush = ush
         else:
             raise Exception('Invalid ush value. Exiting.')
 
     @idxsh.setter
     def idxsh(self, idxsh):
-        if type(idxsh) == np.ndarray:
+        if isinstance(idxsh, np.ndarray):
             self.__idxsh = idxsh
         else:
             raise Exception('Invalid idxsh value. Exiting.')
@@ -1989,21 +1989,21 @@ class AcadosOcpConstraints:
     # soft bounds on convex-over-nonlinear constraints
     @lsphi.setter
     def lsphi(self, lsphi):
-        if type(lsphi) == np.ndarray:
+        if isinstance(lsphi, np.ndarray):
             self.__lsphi = lsphi
         else:
             raise Exception('Invalid lsphi value. Exiting.')
 
     @usphi.setter
     def usphi(self, usphi):
-        if type(usphi) == np.ndarray:
+        if isinstance(usphi, np.ndarray):
             self.__usphi = usphi
         else:
             raise Exception('Invalid usphi value. Exiting.')
 
     @idxsphi.setter
     def idxsphi(self, idxsphi):
-        if type(idxsphi) == np.ndarray:
+        if isinstance(idxsphi, np.ndarray):
             self.__idxsphi = idxsphi
         else:
             raise Exception('Invalid idxsphi value. Exiting.')
@@ -2151,6 +2151,10 @@ class AcadosOcpOptions:
         self.__ext_cost_num_hess = 0
         self.__alpha_min = 0.05
         self.__alpha_reduction = 0.7
+        self.__line_search_use_sufficient_descent = 0
+        self.__globalization_use_SOC = 0
+        self.__full_step_dual = 0
+        self.__eps_sufficient_descent = 1e-4
 
 
     @property
@@ -2367,6 +2371,43 @@ class AcadosOcpOptions:
         """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
         return self.__alpha_reduction
 
+    @property
+    def line_search_use_sufficient_descent(self):
+        """
+        Determines if sufficient descent (Armijo) condition is used in line search.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__line_search_use_sufficient_descent
+
+    @property
+    def eps_sufficient_descent(self):
+        """
+        Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent.
+        Type: float,
+        default: 1e-4.
+        """
+        return self.__eps_sufficient_descent
+
+    @property
+    def globalization_use_SOC(self):
+        """
+        Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING.
+        SOC is done if preliminary line search does not return full step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__globalization_use_SOC
+
+    @property
+    def full_step_dual(self):
+        """
+        Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__full_step_dual
+
     @property
     def nlp_solver_tol_ineq(self):
         """NLP solver inequality tolerance"""
@@ -2524,12 +2565,23 @@ class AcadosOcpOptions:
 
     @time_steps.setter
     def time_steps(self, time_steps):
-        self.__time_steps = time_steps
+        if isinstance(time_steps, np.ndarray):
+            if len(time_steps.shape) == 1:
+                    self.__time_steps = time_steps
+            else:
+                raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).')
+        else:
+            raise Exception('Invalid time_steps, expected np.ndarray.')
 
     @shooting_nodes.setter
     def shooting_nodes(self, shooting_nodes):
-        self.__shooting_nodes = shooting_nodes
-
+        if isinstance(shooting_nodes, np.ndarray):
+            if len(shooting_nodes.shape) == 1:
+                self.__shooting_nodes = shooting_nodes
+            else:
+                raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).')
+        else:
+            raise Exception('Invalid shooting_nodes, expected np.ndarray.')
 
     @Tsim.setter
     def Tsim(self, Tsim):
@@ -2537,7 +2589,12 @@ class AcadosOcpOptions:
 
     @globalization.setter
     def globalization(self, globalization):
-        self.__globalization = globalization
+        globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP')
+        if globalization in globalization_types:
+            self.__globalization = globalization
+        else:
+            raise Exception('Invalid globalization value. Possible values are:\n\n' \
+                    + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
 
     @alpha_min.setter
     def alpha_min(self, alpha_min):
@@ -2547,10 +2604,38 @@ class AcadosOcpOptions:
     def alpha_reduction(self, alpha_reduction):
         self.__alpha_reduction = alpha_reduction
 
+    @line_search_use_sufficient_descent.setter
+    def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent):
+        if line_search_use_sufficient_descent in [0, 1]:
+            self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent
+        else:
+            raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}')
+
+    @globalization_use_SOC.setter
+    def globalization_use_SOC(self, globalization_use_SOC):
+        if globalization_use_SOC in [0, 1]:
+            self.__globalization_use_SOC = globalization_use_SOC
+        else:
+            raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}')
+
+    @full_step_dual.setter
+    def full_step_dual(self, full_step_dual):
+        if full_step_dual in [0, 1]:
+            self.__full_step_dual = full_step_dual
+        else:
+            raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}')
+
+    @eps_sufficient_descent.setter
+    def eps_sufficient_descent(self, eps_sufficient_descent):
+        if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0:
+            self.__eps_sufficient_descent = eps_sufficient_descent
+        else:
+            raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting')
+
     @sim_method_num_stages.setter
     def sim_method_num_stages(self, sim_method_num_stages):
 
-        # if type(sim_method_num_stages) == int:
+        # if isinstance(sim_method_num_stages, int):
         #     self.__sim_method_num_stages = sim_method_num_stages
         # else:
         #     raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
@@ -2560,7 +2645,7 @@ class AcadosOcpOptions:
     @sim_method_num_steps.setter
     def sim_method_num_steps(self, sim_method_num_steps):
 
-        # if type(sim_method_num_steps) == int:
+        # if isinstance(sim_method_num_steps, int):
         #     self.__sim_method_num_steps = sim_method_num_steps
         # else:
         #     raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
@@ -2570,7 +2655,7 @@ class AcadosOcpOptions:
     @sim_method_newton_iter.setter
     def sim_method_newton_iter(self, sim_method_newton_iter):
 
-        if type(sim_method_newton_iter) == int:
+        if isinstance(sim_method_newton_iter, int):
             self.__sim_method_newton_iter = sim_method_newton_iter
         else:
             raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
@@ -2593,7 +2678,7 @@ class AcadosOcpOptions:
 
     @nlp_solver_step_length.setter
     def nlp_solver_step_length(self, nlp_solver_step_length):
-        if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
+        if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0:
             self.__nlp_solver_step_length = nlp_solver_step_length
         else:
             raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
@@ -2614,7 +2699,7 @@ class AcadosOcpOptions:
 
     @qp_solver_cond_N.setter
     def qp_solver_cond_N(self, qp_solver_cond_N):
-        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
+        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0:
             self.__qp_solver_cond_N = qp_solver_cond_N
         else:
             raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
@@ -2705,21 +2790,21 @@ class AcadosOcpOptions:
     @nlp_solver_max_iter.setter
     def nlp_solver_max_iter(self, nlp_solver_max_iter):
 
-        if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
+        if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0:
             self.__nlp_solver_max_iter = nlp_solver_max_iter
         else:
             raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
 
     @print_level.setter
     def print_level(self, print_level):
-        if type(print_level) == int and print_level >= 0:
+        if isinstance(print_level, int) and print_level >= 0:
             self.__print_level = print_level
         else:
             raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
 
     @model_external_shared_lib_dir.setter
     def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
-        if type(model_external_shared_lib_dir) == str :
+        if isinstance(model_external_shared_lib_dir, str) :
             self.__model_external_shared_lib_dir = model_external_shared_lib_dir
         else:
             raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
@@ -2727,7 +2812,7 @@ class AcadosOcpOptions:
 
     @model_external_shared_lib_name.setter
     def model_external_shared_lib_name(self, model_external_shared_lib_name):
-        if type(model_external_shared_lib_name) == str :
+        if isinstance(model_external_shared_lib_name, str) :
             if model_external_shared_lib_name[-3:] == '.so' : 
                 raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
             + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
@@ -2810,6 +2895,9 @@ class AcadosOcp:
         self.acados_lib_path = f'{acados_path}/lib'
         """Path to where acados library is located, type: string"""
 
+        import numpy
+        self.cython_include_dirs = numpy.get_include()
+
         self.__parameter_values = np.array([])
         self.__problem_class = 'OCP'
 

pyextra/acados_template/acados_ocp_solver.py

@@ -37,7 +37,7 @@ import os
 import json
 import numpy as np
 from datetime import datetime
-import ctypes
+import importlib
 from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
 
 from copy import deepcopy
@@ -51,9 +51,9 @@ from .generate_c_code_nls_cost import generate_c_code_nls_cost
 from .generate_c_code_external_cost import generate_c_code_external_cost
 from .acados_ocp import AcadosOcp
 from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
+from .utils import is_column, is_empty, casadi_length, render_template,\
      format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
+     set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path
 
 
 def make_ocp_dims_consistent(acados_ocp):
@@ -90,7 +90,7 @@ def make_ocp_dims_consistent(acados_ocp):
         raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
             f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
 
-    # cost
+    ## cost
     # initial stage - if not set, copy fields from path constraints
     if cost.cost_type_0 is None:
         cost.cost_type_0 = cost.cost_type
@@ -434,18 +434,14 @@ def make_ocp_dims_consistent(acados_ocp):
         if np.shape(opts.shooting_nodes)[0] != dims.N+1:
             raise Exception('inconsistent dimension N, regarding shooting_nodes.')
 
-        # time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
-        # # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
-        # delta_time_steps = time_steps[1:] - time_steps[0:-1]
-        # avg_time_steps = np.average(time_steps)
-        # # criterion for constant time-step detection: the min/max difference in values normalized by the average
-        # check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
-        # # if the criterion is small, we have a constant time-step
-        # if check_const_time_step < 1e-9:
-        #     time_steps[:] = avg_time_steps  # if we have a constant time-step: apply the average time-step
-        time_steps = np.zeros((dims.N,))
-        for i in range(dims.N):
-            time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
+        time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
+        # identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit
+        avg_time_steps = np.average(time_steps)
+        # criterion for constant time step detection: the min/max difference in values normalized by the average
+        check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps
+        # if the criterion is small, we have a constant time_step
+        if check_const_time_step < 1e-9:
+            time_steps[:] = avg_time_steps  # if we have a constant time_step: apply the average time_step
 
         opts.time_steps = time_steps
 
@@ -525,8 +521,7 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
 
     # strip shooting_nodes
     ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
-
-    dims_dict = acados_class2dict(acados_ocp.dims)
+    dims_dict = format_class_dict(acados_ocp.dims.__dict__)
 
     ocp_check_against_layout(ocp_nlp_dict, dims_dict)
 
@@ -782,8 +777,15 @@ class AcadosOcpSolver:
         dlclose.argtypes = [c_void_p]
 
     @classmethod
-    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
+    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None):
+        """
+        Generates the code for an acados OCP solver, given the description in acados_ocp.
+            :param acados_ocp: type AcadosOcp - description of the OCP for acados
+            :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
+            :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
+        """
         model = acados_ocp.model
+        acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory)
 
         if simulink_opts is None:
             simulink_opts = get_simulink_default_opts()
@@ -807,24 +809,91 @@ class AcadosOcpSolver:
         # dump to json
         ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
 
-        code_export_dir = acados_ocp.code_export_directory
         # render templates
         ocp_render_templates(acados_ocp, json_file)
+        acados_ocp.json_file = json_file
 
-        if build:
-          ## Compile solver
-          cwd=os.getcwd()
-          os.chdir(code_export_dir)
-          os.system('make clean_ocp_shared_lib')
-          os.system('make ocp_shared_lib')
-          os.chdir(cwd)
-
-    def __init__(self, model_name, N, code_export_dir):
-        self.model_name = model_name
-        self.N = N
+
+    @classmethod
+    def build(cls, code_export_dir, with_cython=False):
+        """
+        Builds the code for an acados OCP solver, that has been generated in code_export_dir
+            :param code_export_dir: directory in which acados OCP solver has been generated, see generate()
+            :param with_cython: option indicating if the cython interface is build, default: False.
+        """
+        cwd=os.getcwd()
+        os.chdir(code_export_dir)
+        if with_cython:
+            os.system('make clean_ocp_cython')
+            os.system('make ocp_cython')
+        else:
+            os.system('make clean_ocp_shared_lib')
+            os.system('make ocp_shared_lib')
+        os.chdir(cwd)
+
+
+    @classmethod
+    def create_cython_solver(cls, json_file):
+        """
+        Returns an `AcadosOcpSolverCython` object.
+
+        This is an alternative Cython based Python wrapper to the acados OCP solver in C.
+        This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code.
+
+        The default wrapper `AcadosOcpSolver` is based on ctypes.
+        """
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        importlib.invalidate_caches()
+        rel_code_export_directory = os.path.relpath(code_export_directory)
+        acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx')
+
+        AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython')
+        return AcadosOcpSolverCython(acados_ocp_json['model']['name'],
+                    acados_ocp_json['solver_options']['nlp_solver_type'],
+                    acados_ocp_json['dims']['N'])
+
+
+    def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True):
 
         self.solver_created = False
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
+        if generate:
+            self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts)
+
+        # load json, store options in object
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        self.N = acados_ocp_json['dims']['N']
+        self.model_name = acados_ocp_json['model']['name']
+        self.solver_options = acados_ocp_json['solver_options']
+
+        acados_lib_path = acados_ocp_json['acados_lib_path']
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        if build:
+            self.build(code_export_directory, with_cython=False)
+
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
+        self.shared_lib_name = f'{code_export_directory}/libacados_ocp_solver_{self.model_name}.so'
 
         # get shared_lib
         self.shared_lib = CDLL(self.shared_lib_name)
@@ -842,6 +911,8 @@ class AcadosOcpSolver:
         # get pointers solver
         self.__get_pointers_solver()
 
+        self.status = 0
+
 
     def __get_pointers_solver(self):
         """
@@ -864,6 +935,10 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
         self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
 
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p
+        self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule)
+
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
         self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
@@ -872,46 +947,26 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
         self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
 
-        # treat parameters separately
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
-        self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_out_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_set.argtypes = \
-            [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
 
     def solve(self):
         """
         Solve the ocp with current input.
         """
-
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
-        return status
+        self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
+
+        return self.status
 
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
-            .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
+            .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
@@ -921,15 +976,14 @@ class AcadosOcpSolver:
             raise Exception('Solver was not yet created!')
 
         # check if time steps really changed in value
-        if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
+        if np.array_equal(self.solver_options['time_steps'], new_time_steps):
             return
 
         N = new_time_steps.size
-        model = self.acados_ocp.model
         new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
 
         # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
-        if self.acados_ocp.solver_options.time_steps.size == N:
+        if len(self.solver_options['time_steps']) == N:
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
             assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
@@ -941,11 +995,6 @@ class AcadosOcpSolver:
             getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
             getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
 
-            # store N and new time steps
-            self.N = self.acados_ocp.dims.N = N
-            self.acados_ocp.solver_options.time_steps = new_time_steps
-            self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
-
             # create solver with new time steps
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
@@ -956,6 +1005,75 @@ class AcadosOcpSolver:
             # get pointers solver
             self.__get_pointers_solver()
 
+        # store time_steps, N
+        self.solver_options['time_steps'] = new_time_steps
+        self.N = N
+        self.solver_options['Tsim'] = self.solver_options['time_steps'][0]
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        # unlikely but still possible
+        if not self.solver_created:
+            raise Exception('Solver was not yet created!')
+        if self.N < qp_solver_cond_N:
+            raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N:
+            self.solver_created = False
+
+            # recreate the solver
+            fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N'
+            getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int]
+            getattr(self.shared_lib, fun_name).restype = c_int
+            assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0
+
+            # store the new value
+            self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N
+            self.solver_created = True
+
+            # get pointers solver
+            self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.')
+
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+        nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p]
+        self.shared_lib.ocp_nlp_eval_param_sens.restype = None
+        self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
+
 
     def get(self, stage_, field_):
         """
@@ -978,23 +1096,30 @@ class AcadosOcpSolver:
 
         out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
         # mem_fields = ['sl', 'su']
+        sens_fields = ['sens_u', "sens_x"]
+        all_fields = out_fields + sens_fields
+
         field = field_
-        field = field.encode('utf-8')
 
-        if (field_ not in out_fields):
+        if (field_ not in all_fields):
             raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+                    \n Possible values are {}. Exiting.'.format(field_, all_fields))
 
         if not isinstance(stage_, int):
             raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
 
         if stage_ < 0 or stage_ > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
 
         if stage_ == self.N and field_ == 'pi':
             raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage_))
 
+        if field_ in sens_fields:
+            field = field_.replace('sens_', '')
+
+        field = field.encode('utf-8')
+
         self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
             [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
         self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
@@ -1015,6 +1140,11 @@ class AcadosOcpSolver:
         #         [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
         #     self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
         #         self.nlp_dims, self.nlp_solver, stage_, field, out_data)
+        elif field_ in sens_fields:
+            self.shared_lib.ocp_nlp_out_get.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
+                self.nlp_dims, self.sens_out, stage_, field, out_data)
 
         return out
 
@@ -1029,6 +1159,7 @@ class AcadosOcpSolver:
             - res_comp: residual wrt complementarity conditions
             - qp_stat: status of QP solver
             - qp_iter: number of QP iterations
+            - alpha: SQP step size
             - qp_res_stat: stationarity residual of the last QP solution
             - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
             - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
@@ -1036,19 +1167,18 @@ class AcadosOcpSolver:
         """
         stat = self.get_stats("statistics")
 
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
-            if stat.shape[0]>7:
+        if self.solver_options['nlp_solver_type'] == 'SQP':
+            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
+            if stat.shape[0]>8:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
             for jj in range(stat.shape[1]):
-                print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
-                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
-                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
-                if stat.shape[0]>7:
+                print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
+                      f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
+                if stat.shape[0]>8:
                     print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
-                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
+                        stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj]))
             print('\n')
-        elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             print('\niter\tqp_stat\tqp_iter')
             if stat.shape[0]>3:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
@@ -1108,6 +1238,7 @@ class AcadosOcpSolver:
         with open(filename, 'r') as f:
             solution = json.load(f)
 
+        print(f"loading iterate {filename}")
         for key in solution.keys():
             (field, stage) = key.split('_')
             self.set(int(stage), field, np.array(solution[key]))
@@ -1117,62 +1248,99 @@ class AcadosOcpSolver:
         """
         Get the information of the last solver call.
 
-            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha']
+
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
 
-        fields = ['time_tot',  # total cpu time previous call
-                  'time_lin',  # cpu time for linearization
-                  'time_sim',  # cpu time for integrator
-                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
-                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
-                  'time_qp',   # cpu time qp solution
-                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
                   'time_qp_xcond',
-                  'time_glob',  # cpu time globalization
-                  'time_reg',  # cpu time regularization
-                  'sqp_iter',  # number of SQP iterations
-                  'qp_iter',  # vector of QP iterations for last SQP call
-                  'statistics',  # table with info about last iteration
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
                   'stat_m',
-                  'stat_n',]
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
 
-        field = field_
-        field = field.encode('utf-8')
-        if (field_ not in fields):
-            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
-                    \n Possible values are {}. Exiting.'.format(fields, fields))
 
         if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
             out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
             out_data = cast(out.ctypes.data, POINTER(c_int64))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
+
+        # TODO: just return double instead of np.
+        elif field_ in double_fields:
+            out = np.zeros((1,))
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'statistics':
             sqp_iter = self.get_stats("sqp_iter")
             stat_m = self.get_stats("stat_m")
             stat_n = self.get_stats("stat_n")
-
             min_size = min([stat_m, sqp_iter+1])
-
             out = np.ascontiguousarray(
                         np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
             out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'qp_iter':
             full_stats = self.get_stats('statistics')
-            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-                out = full_stats[6, :]
-            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
-                out = full_stats[2, :]
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[6, :]
+            elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                return full_stats[2, :]
 
-        else:
-            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-            out_data = cast(out.ctypes.data, POINTER(c_double))
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[7, :]
+            else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
 
-        if not field_ == 'qp_iter':
-            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+        elif field_ == 'residuals':
+            return self.get_residuals()
 
-        return out
+        else:
+            raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
+                    + f'\n Possible values are {fields}.')
 
 
     def get_cost(self):
@@ -1201,7 +1369,7 @@ class AcadosOcpSolver:
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
         # compute residuals if RTI
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        if self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
             self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
 
@@ -1230,9 +1398,7 @@ class AcadosOcpSolver:
 
 
     # Note: this function should not be used anymore, better use cost_set, constraints_set
-
     def set(self, stage_, field_, value_):
-
         """
         Set numerical data inside the solver.
 
@@ -1253,6 +1419,7 @@ class AcadosOcpSolver:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         # cast value_ to avoid conversion issues
         if isinstance(value_, (float, int)):
@@ -1294,18 +1461,25 @@ class AcadosOcpSolver:
             value_data_p = cast((value_data), c_void_p)
 
             if field_ in constraints_fields:
+                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in cost_fields:
+                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in out_fields:
+                self.shared_lib.ocp_nlp_out_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            # elif field_ in mem_fields:
-            #     self.shared_lib.ocp_nlp_set(self.nlp_config, \
-            #         self.nlp_solver, stage, field, value_data_p)
-
+            elif field_ in mem_fields:
+                self.shared_lib.ocp_nlp_set.argtypes = \
+                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, value_data_p)
         return
 
 
@@ -1364,9 +1538,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1433,8 +1606,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1490,11 +1663,11 @@ class AcadosOcpSolver:
         """
         Set options of the solver.
 
-            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # check field availability and type
@@ -1522,10 +1695,10 @@ class AcadosOcpSolver:
 
         if field_ == 'rti_phase':
             if value_ < 0 or value_ > 2:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only values 0, 1, 2 for SQP-RTI-type solvers')
-            if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+            if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only value 0 for SQP-type solvers')
 
         # encode

pyextra/acados_template/acados_ocp_solver_fast.py

@@ -1,402 +0,0 @@
-import sys
-import os
-import json
-import numpy as np
-from datetime import datetime
-
-from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
-
-from copy import deepcopy
-
-from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
-from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
-from .generate_c_code_gnsf import generate_c_code_gnsf
-from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
-from .generate_c_code_constraint import generate_c_code_constraint
-from .generate_c_code_nls_cost import generate_c_code_nls_cost
-from .generate_c_code_external_cost import generate_c_code_external_cost
-from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
-     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path
-
-
-class AcadosOcpSolverFast:
-    dlclose = CDLL(None).dlclose
-    dlclose.argtypes = [c_void_p]
-
-    def __init__(self, model_name, N, code_export_dir):
-
-        self.solver_created = False
-        self.N = N
-        self.model_name = model_name
-
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
-
-        # get shared_lib
-        self.shared_lib = CDLL(self.shared_lib_name)
-
-        # create capsule
-        getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
-        self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
-
-        # create solver
-        getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
-        assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
-        self.solver_created = True
-
-        # get pointers solver
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
-        self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
-        self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
-        self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
-        self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
-        self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
-        self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
-
-
-    def solve(self):
-        """
-        Solve the ocp with current input.
-        """
-        model_name = self.model_name
-
-        getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
-        return status
-
-    def cost_set(self, start_stage_, field_, value_, api='warn'):
-      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the cost module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to cost_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
-                            ' for field "{}" with dimension {} (you have {})'.format(
-                               field_, expected_shape, value_shape))
-
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    def constraints_set(self, start_stage_, field_, value_, api='warn'):
-      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the constraint module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    # Note: this function should not be used anymore, better use cost_set, constraints_set
-    def set(self, stage_, field_, value_):
-        """
-        Set numerical data inside the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        cost_fields = ['y_ref', 'yref']
-        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
-        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
-        mem_fields = ['sl', 'su']
-
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        model_name = self.model_name
-
-        field = field_
-        field = field.encode('utf-8')
-
-        stage = c_int(stage_)
-
-        # treat parameters separately
-        if field_ == 'p':
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-
-            assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
-        else:
-            if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
-                    \nPossible values are {}. Exiting.".format(field, \
-                    constraints_fields + cost_fields + out_fields + ['p']))
-
-            self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-            self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-            dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, stage_, field)
-
-            if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
-                msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
-                raise Exception(msg)
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-            value_data_p = cast((value_data), c_void_p)
-
-            if field_ in constraints_fields:
-                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in cost_fields:
-                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in out_fields:
-                self.shared_lib.ocp_nlp_out_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            elif field_ in mem_fields:
-                self.shared_lib.ocp_nlp_set.argtypes = \
-                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_set(self.nlp_config, \
-                    self.nlp_solver, stage, field, value_data_p)
-        return
-
-
-    def get_slice(self, start_stage_, end_stage_, field_):
-        """
-        Get the last solution of the solver:
-
-            :param start_stage: integer corresponding to shooting node that indicates start of slice
-            :param end_stage: integer corresponding to shooting node that indicates end of slice
-            :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
-        mem_fields = ['sl', 'su']
-        field = field_
-        field = field.encode('utf-8')
-
-        if (field_ not in out_fields + mem_fields):
-            raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
-
-        if not isinstance(start_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if not isinstance(end_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if start_stage_ >= end_stage_:
-            raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
-
-        if start_stage_ < 0 or end_stage_ > self.N + 1:
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
-        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field)
-
-        out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        if (field_ in out_fields):
-            self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
-        elif field_ in mem_fields:
-            self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
-                self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
-
-        return out
-
-    def get_cost(self):
-        """
-        Returns the cost value of the current solution.
-        """
-        # compute cost internally
-        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
-        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
-
-        # create output array
-        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        # call getter
-        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-
-        field = "cost_value".encode('utf-8')
-        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
-
-        return out[0]

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -39,21 +39,19 @@ cimport cython
 from libc cimport string
 
 cimport acados_solver_common
+# TODO: make this import more clear? it is not a general solver, but problem specific.
 cimport acados_solver
 
 cimport numpy as cnp
 
 import os
+from datetime import datetime
 import numpy as np
 
 
-cdef class AcadosOcpSolverFast:
+cdef class AcadosOcpSolverCython:
     """
     Class to interact with the acados ocp solver C object.
-
-        :param acados_ocp: type AcadosOcp - description of the OCP for acados
-        :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
-        :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
     """
 
     cdef acados_solver.nlp_solver_capsule *capsule
@@ -61,19 +59,26 @@ cdef class AcadosOcpSolverFast:
     cdef acados_solver_common.ocp_nlp_dims *nlp_dims
     cdef acados_solver_common.ocp_nlp_config *nlp_config
     cdef acados_solver_common.ocp_nlp_out *nlp_out
+    cdef acados_solver_common.ocp_nlp_out *sens_out
     cdef acados_solver_common.ocp_nlp_in *nlp_in
     cdef acados_solver_common.ocp_nlp_solver *nlp_solver
 
+    cdef int status
+    cdef bint solver_created
+
     cdef str model_name
     cdef int N
-    cdef bint solver_created
 
-    def __cinit__(self, str model_name, int N, str code_export_dir):
-        self.model_name = model_name
-        self.N = N
+    cdef str nlp_solver_type
+
+    def __cinit__(self, model_name, nlp_solver_type, N):
 
         self.solver_created = False
 
+        self.N = N
+        self.model_name = model_name
+        self.nlp_solver_type = nlp_solver_type
+
         # create capsule
         self.capsule = acados_solver.acados_create_capsule()
 
@@ -81,11 +86,21 @@ cdef class AcadosOcpSolverFast:
         assert acados_solver.acados_create(self.capsule) == 0
         self.solver_created = True
 
+        # get pointers solver
+        self.__get_pointers_solver()
+        self.status = 0
+
+
+    def __get_pointers_solver(self):
+        """
+        Private function to get the pointers for solver
+        """
         # get pointers solver
         self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
         self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
         self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
         self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
+        self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
         self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
         self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
 
@@ -99,15 +114,112 @@ cdef class AcadosOcpSolverFast:
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
             .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
-        raise NotImplementedError()
+
+        raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+
+        # ## check if time steps really changed in value
+        # # get time steps
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
+        # assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
+
+        # if np.array_equal(old_time_steps, new_time_steps):
+        #     return
+
+        # N = new_time_steps.size
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
+
+        # # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
+        # if len(old_time_steps) == N:
+        #     assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
+
+        # else:  # recreate the solver with the new time steps
+        #     self.solver_created = False
+
+        #     # delete old memory (analog to __del__)
+        #     acados_solver.acados_free(self.capsule)
+
+        #     # create solver with new time steps
+        #     assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
+
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+        # # store time_steps, N
+        # self.time_steps = new_time_steps
+        # self.N = N
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
+
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+        # if self.N < qp_solver_cond_N:
+        #     raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        # if self.qp_solver_cond_N != qp_solver_cond_N:
+        #     self.solver_created = False
+
+        #     # recreate the solver
+        #     acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
+
+        #     # store the new value
+        #     self.qp_solver_cond_N = qp_solver_cond_N
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
+
+        cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
 
 
     def get(self, int stage, str field_):
@@ -133,14 +245,14 @@ cdef class AcadosOcpSolverFast:
         field = field_.encode('utf-8')
 
         if field_ not in out_fields:
-            raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
+            raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
                     \n Possible values are {}. Exiting.'.format(field_, out_fields))
 
         if stage < 0 or stage > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
 
         if stage == self.N and field_ == 'pi':
-            raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
+            raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage))
 
         cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
@@ -168,7 +280,7 @@ cdef class AcadosOcpSolverFast:
             - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
             - qp_res_comp: residual wrt complementarity conditions of the last QP solution
         """
-        raise NotImplementedError()
+        acados_solver.acados_print_stats(self.capsule)
 
 
     def store_iterate(self, filename='', overwrite=False):
@@ -178,14 +290,50 @@ cdef class AcadosOcpSolverFast:
             :param filename: if not set, use model_name + timestamp + '.json'
             :param overwrite: if false and filename exists add timestamp to filename
         """
-        raise NotImplementedError()
+        import json
+        if filename == '':
+            filename += self.model_name + '_' + 'iterate' + '.json'
+
+        if not overwrite:
+            # append timestamp
+            if os.path.isfile(filename):
+                filename = filename[:-5]
+                filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
+
+        # get iterate:
+        solution = dict()
+
+        for i in range(self.N+1):
+            solution['x_'+str(i)] = self.get(i,'x')
+            solution['u_'+str(i)] = self.get(i,'u')
+            solution['z_'+str(i)] = self.get(i,'z')
+            solution['lam_'+str(i)] = self.get(i,'lam')
+            solution['t_'+str(i)] = self.get(i, 't')
+            solution['sl_'+str(i)] = self.get(i, 'sl')
+            solution['su_'+str(i)] = self.get(i, 'su')
+        for i in range(self.N):
+            solution['pi_'+str(i)] = self.get(i,'pi')
+
+        # save
+        with open(filename, 'w') as f:
+            json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True)
+        print("stored current iterate in ", os.path.join(os.getcwd(), filename))
 
 
     def load_iterate(self, filename):
         """
         Loads the iterate stored in json file with filename into the ocp solver.
         """
-        raise NotImplementedError()
+        import json
+        if not os.path.isfile(filename):
+            raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
+
+        with open(filename, 'r') as f:
+            solution = json.load(f)
+
+        for key in solution.keys():
+            (field, stage) = key.split('_')
+            self.set(int(stage), field, np.array(solution[key]))
 
 
     def get_stats(self, field_):
@@ -193,8 +341,97 @@ cdef class AcadosOcpSolverFast:
         Get the information of the last solver call.
 
             :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
-        raise NotImplementedError()
+
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
+                  'time_qp_xcond',
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
+                  'stat_m',
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
+
+        if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
+            return self.__get_stat_int(field)
+
+        elif field_ in double_fields:
+            return self.__get_stat_double(field)
+
+        elif field_ == 'statistics':
+            sqp_iter = self.get_stats("sqp_iter")
+            stat_m = self.get_stats("stat_m")
+            stat_n = self.get_stats("stat_n")
+            min_size = min([stat_m, sqp_iter+1])
+            return self.__get_stat_matrix(field, stat_n+1, min_size)
+
+        elif field_ == 'qp_iter':
+            full_stats = self.get_stats('statistics')
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[6, :]
+            elif self.nlp_solver_type == 'SQP_RTI':
+                return full_stats[2, :]
+
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[7, :]
+            else: # self.nlp_solver_type == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
+
+        elif field_ == 'residuals':
+            return self.get_residuals()
+
+        else:
+            raise NotImplementedError("TODO!")
+
+
+    def __get_stat_int(self, field):
+        cdef int out
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &out)
+        return out
+
+    def __get_stat_double(self, field):
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,))
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
+        return out
+
+    def __get_stat_matrix(self, field, n, m):
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64)
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_mat.data)
+        return out_mat
 
 
     def get_cost(self):
@@ -217,7 +454,31 @@ cdef class AcadosOcpSolverFast:
         """
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
-        raise NotImplementedError()
+        # compute residuals if RTI
+        if self.nlp_solver_type == 'SQP_RTI':
+            acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
+        cdef double double_value
+
+        field = "res_stat".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[0] = double_value
+
+        field = "res_eq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[1] = double_value
+
+        field = "res_ineq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[2] = double_value
+
+        field = "res_comp".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
+        out[3] = double_value
+
+        return out
 
 
     # Note: this function should not be used anymore, better use cost_set, constraints_set
@@ -243,18 +504,18 @@ cdef class AcadosOcpSolverFast:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         field = field_.encode('utf-8')
 
-        cdef double[::1] value
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64)
 
         # treat parameters separately
         if field_ == 'p':
-            value = np.ascontiguousarray(value_, dtype=np.double)
-            assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], value.shape[0]) == 0
+            assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
         else:
             if field_ not in constraints_fields + cost_fields + out_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+                raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
                     \nPossible values are {}. Exiting.".format(field, \
                     constraints_fields + cost_fields + out_fields + ['p']))
 
@@ -262,20 +523,22 @@ cdef class AcadosOcpSolverFast:
                 self.nlp_dims, self.nlp_out, stage, field)
 
             if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+                msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
                 msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
                 raise Exception(msg)
 
-            value = np.ascontiguousarray(value_, dtype=np.double)
             if field_ in constraints_fields:
                 acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
             elif field_ in cost_fields:
                 acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
             elif field_ in out_fields:
                 acados_solver_common.ocp_nlp_out_set(self.nlp_config,
-                    self.nlp_dims, self.nlp_out, stage, field, <void *> &value[0])
+                    self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
+            elif field_ in mem_fields:
+                acados_solver_common.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, <void *> value.data)
 
 
     def cost_set(self, int stage, str field_, value_):
@@ -304,9 +567,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -338,8 +600,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -361,7 +623,7 @@ cdef class AcadosOcpSolverFast:
         acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
 
         # create output data
-        out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
 
         # call getter
         acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
@@ -376,8 +638,8 @@ cdef class AcadosOcpSolverFast:
             :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # encode
@@ -394,10 +656,10 @@ cdef class AcadosOcpSolverFast:
 
             if field_ == 'rti_phase':
                 if value_ < 0 or value_ > 2:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only values 0, 1, 2 for SQP-RTI-type solvers')
-                if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only value 0 for SQP-type solvers')
 
             int_value = value_
@@ -418,7 +680,7 @@ cdef class AcadosOcpSolverFast:
             acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
 
         else:
-            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+            raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
                 '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
 
 

pyextra/acados_template/acados_sim.py

@@ -70,28 +70,28 @@ class AcadosSimDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -302,6 +302,7 @@ class AcadosSim:
         self.code_export_directory = 'c_generated_code'
         """Path to where code will be exported. Default: `c_generated_code`."""
 
+        self.cython_include_dirs = ''
         self.__parameter_values = np.array([])
 
     @property

pyextra/acados_template/acados_sim_solver.py

@@ -215,6 +215,24 @@ class AcadosSimSolver:
         model_name = self.sim_struct.model.name
         self.model_name = model_name
 
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
         # Ctypes
         shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
         self.shared_lib = CDLL(shared_lib)

pyextra/acados_template/acados_solver_common.pxd

@@ -95,6 +95,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
 
     # solver
     void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
+    void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
     void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
 
     # get/set

pyextra/acados_template/c_templates_tera/Makefile.in

@@ -125,134 +125,134 @@
 	{%- endif %}
 {%- endif %}
 
-{# acados flags #}
-ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
-{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPOASES
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
-ACADOS_FLAGS += -DACADOS_WITH_OSQP
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
-{%- endif %}
-# # Debugging
-# ACADOS_FLAGS += -g3
+# define sources and use make's implicit rules to generate object files (*.o)
 
-MODEL_OBJ=
+# model
+MODEL_SRC=
 {%- if  solver_options.integrator_type == "ERK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
+	{% if model.gnsf.purely_linear != 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
+		{% if model.gnsf.nontrivial_f_LO == 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
+		{%- endif %}
+	{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
 {%- elif solver_options.integrator_type == "DISCRETE" %}
-{%- if model.dyn_ext_fun_type == "casadi" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
-{%- endif %}
-{%- else %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
-{%- endif %}
+	{%- if model.dyn_ext_fun_type == "casadi" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
+		{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
+		{%- endif %}
+	{%- else %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+	{%- endif %}
 {%- endif %}
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
-
-OCP_OBJ=
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
 {%- if constr_type == "BGP" and dims_nphi > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
 {%- endif %}
 {%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
 {%- endif %}
 
 {%- if constr_type == "BGH" and dims_nh > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if cost_type_0 == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_0 == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
-{% else %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_0 == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
+	{%- else %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
 {%- elif cost_type == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type_e == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_e == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_e == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
+	{%- endif %}
 {%- endif %}
-OCP_OBJ+= acados_solver_{{ model.name }}.o
+OCP_SRC+= acados_solver_{{ model.name }}.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
 
+# for sim solver
+SIM_SRC= acados_sim_solver_{{ model.name }}.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
 
-SIM_OBJ=
-SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
+# for target example
+EX_SRC= main_{{ model.name }}.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
 
-EX_OBJ=
-EX_OBJ+= main_{{ model.name }}.o
-
-EX_SIM_OBJ=
-EX_SIM_OBJ+= main_sim_{{ model.name }}.o
+# for target example_sim
+EX_SIM_SRC= main_sim_{{ model.name }}.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
 
+# combine model, sim and ocp object files
 OBJ=
 OBJ+= $(MODEL_OBJ)
 {%- if solver_options.integrator_type != "DISCRETE" %}
@@ -271,233 +271,103 @@ EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
 INCLUDE_PATH = {{ acados_include_path }}
 LIB_PATH = {{ acados_lib_path }}
 
-{%- if solver_options.integrator_type == "DISCRETE" %}
-all: clean casadi_fun example
-shared_lib: ocp_shared_lib
-{%- else %}
-all: clean casadi_fun example_sim example
-shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
-{%- endif %}
-
-CASADI_MODEL_SOURCE=
-{%- if solver_options.integrator_type == "ERK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "GNSF" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
-{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
-{%- endif %}
-{%- endif %}
-{%- if constr_type == "BGP" and dims_nphi > 0 %}
-CASADI_CON_PHI_SOURCE=
-CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
-{%- endif %}
-{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-CASADI_CON_PHI_E_SOURCE=
-CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
-{%- endif %}
-{%- if constr_type == "BGH" and dims_nh > 0 %}
-CASADI_CON_H_SOURCE=
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
-{%- endif %}
-{%- endif %}
-
-{%- if dims_nh_e > 0 %}
-CASADI_CON_H_E_SOURCE=
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
-{%- endif %}
-{%- endif %}
-
-{%- if cost_type == "NONLINEAR_LS" %}
-CASADI_COST_Y_SOURCE=
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
-{%- endif %}
-{%- if cost_type_e == "NONLINEAR_LS" %}
-CASADI_COST_Y_E_SOURCE=
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
+# preprocessor flags for make's implicit rules
+{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS += -DACADOS_WITH_QPOASES
 {%- endif %}
-{%- if cost_type_0 == "NONLINEAR_LS" %}
-CASADI_COST_Y_0_SOURCE=
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
+{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
+CPPFLAGS += -DACADOS_WITH_OSQP
 {%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
+CPPFLAGS += -DACADOS_WITH_QPDUNES
+{%- endif %}
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+ {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
+ {%- endif %}
+
+{# c-compiler flags #}
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
 
-casadi_fun:
-	{%- if model.dyn_ext_fun_type == "casadi" %}
-	( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGP" and dims_nphi > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGH" and dims_nh > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
-	{%- endif %}
-	{%- if cost_type_e == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type_0 == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
-	{%- endif %}
-
-main:
-	gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
 
-main_sim:
-	gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= {{ link_libs }}
 
-ocp_solver:
-	gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# libraries
+LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
 
-sim_solver:
-	gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+# virtual targets
+.PHONY : all clean
 
-example: ocp_solver main
-	gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
-
-
-example_sim: sim_solver main_sim
-	gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/acados/ \
+#all: clean example_sim example shared_lib
+{% if solver_options.integrator_type == "DISCRETE" -%}
+all: clean example
+shared_lib: ocp_shared_lib
+{%- else %}
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+{%- endif %}
 
-{%- if solver_options.integrator_type != "DISCRETE" %}
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
 
-bundled_shared_lib: casadi_fun ocp_solver sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
 
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+{% if solver_options.integrator_type != "DISCRETE" -%}
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+{%- endif %}
 
-{%- else %}
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
 
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
 
-{%- endif %}
 
+# Cython targets
 ocp_cython_c: ocp_shared_lib
 	cython \
 	-o acados_ocp_solver_pyx.c \
 	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
 	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I {{ code_export_directory }} \
 
 ocp_cython_o: ocp_cython_c
-	clang $(ACADOS_FLAGS) -c -O2 \
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
 	-o acados_ocp_solver_pyx.o \
 	-I /usr/include/python3.8 \
 	-I $(INCLUDE_PATH)/blasfeo/include/ \
 	-I $(INCLUDE_PATH)/hpipm/include/ \
 	-I $(INCLUDE_PATH) \
+	-I {{ cython_include_dirs }} \
 	acados_ocp_solver_pyx.c \
 
 ocp_cython: ocp_cython_o
-	clang $(ACADOS_FLAGS) -shared \
+	$(CC) $(ACADOS_FLAGS) -shared \
 	-o acados_ocp_solver_pyx.so \
 	-Wl,-rpath=$(LIB_PATH) \
 	acados_ocp_solver_pyx.o \
 	$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
-	{{ link_libs }} \
-	-lm \
-
-sim_shared_lib: casadi_fun sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+	$(LDFLAGS) $(LDLIBS)
 
 {%- if os and os == "pc" %}
 
@@ -510,15 +380,27 @@ clean_ocp_shared_lib:
 	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
 	del \Q acados_solver_{{ model.name }}.o 2>nul
 
+clean_ocp_cython:
+	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
+	del \Q acados_solver_{{ model.name }}.o 2>nul
+	del \Q acados_ocp_solver_pyx.so 2>nul
+	del \Q acados_ocp_solver_pyx.o 2>nul
+
 {%- else %}
 
 clean:
-	rm -f *.o
-	rm -f *.so
-	rm -f main_{{ model.name }}
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
 
 clean_ocp_shared_lib:
-	rm -f libacados_ocp_solver_{{ model.name }}.so
-	rm -f acados_solver_{{ model.name }}.o
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_{{ model.name }}.so
+	$(RM) acados_solver_{{ model.name }}.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
 
 {%- endif %}

pyextra/acados_template/c_templates_tera/acados_mex_create.in.c

@@ -63,7 +63,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexPrintf("{{ model.name }}_acados_create() -> success!\n");
 
     // get pointers to nlp solver related objects
-    ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
+    ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
     ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
     ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
     ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
@@ -238,14 +238,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
 {%- endif %}
 {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
     l_ptr = mxGetData(gnsf_phi_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
     l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
     l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
+    {% if model.gnsf.nontrivial_f_LO == 1 %}
     l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
+    {%- endif %}
+    {%- endif %}
     l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
 {% elif solver_options.integrator_type == "DISCRETE" %}

pyextra/acados_template/c_templates_tera/acados_mex_set.in.c

@@ -69,7 +69,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
     // plan
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
-    ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
+    ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
     // config
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
     ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
@@ -404,7 +404,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_z"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -426,7 +426,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_xdot"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -448,7 +448,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_gnsf_phi"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == GNSF)
         {

pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c

@@ -164,12 +164,17 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
 {%- endif %}
 
     {% elif solver_options.integrator_type == "GNSF" -%}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
 
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
     capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
     capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
@@ -194,6 +199,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
     external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
 
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
@@ -201,6 +207,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
     external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
+  {%- endif %}
+  {%- endif %}
 
     capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
     capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
@@ -212,7 +220,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     {% endif %}
 
     // sim plan & config
-    sim_solver_plan plan;
+    sim_solver_plan_t plan;
     plan.sim_solver = {{ solver_options.integrator_type }};
 
     // create correct config based on plan
@@ -307,14 +315,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
                 "expl_ode_hess", capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun", capsule->sim_gnsf_phi_fun);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
 {%- endif %}
@@ -409,10 +421,14 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
     external_function_param_casadi_free(capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
 {% endif %}
 
@@ -445,10 +461,14 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
     capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
     capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
     capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
 {% endif %}
 

pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c

@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/ocp_nlp_interface.h"
 #include "acados_c/external_function_interface.h"
 

pyextra/acados_template/c_templates_tera/acados_solver.in.h

@@ -78,9 +78,10 @@ typedef struct {{ model.name }}_solver_capsule
     // acados objects
     ocp_nlp_in *nlp_in;
     ocp_nlp_out *nlp_out;
+    ocp_nlp_out *sens_out;
     ocp_nlp_solver *nlp_solver;
     void *nlp_opts;
-    ocp_nlp_plan *nlp_solver_plan;
+    ocp_nlp_plan_t *nlp_solver_plan;
     ocp_nlp_config *nlp_config;
     ocp_nlp_dims *nlp_dims;
 
@@ -186,6 +187,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
  * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
  */
 int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
+/**
+ * This function is used for updating an already initialized solver with a different number of qp_cond_N.
+ */
+int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
 int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
 int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
 int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
@@ -193,11 +198,12 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
 
 ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
 void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
 
 #ifdef __cplusplus
 } /* extern "C" */

pyextra/acados_template/c_templates_tera/acados_solver.in.pxd

@@ -1,3 +1,36 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
 cimport acados_solver_common
 
 cdef extern from "acados_solver_{{ model.name }}.h":
@@ -8,6 +41,11 @@ cdef extern from "acados_solver_{{ model.name }}.h":
     int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
 
     int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
     int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
     int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
     int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
@@ -15,6 +53,7 @@ cdef extern from "acados_solver_{{ model.name }}.h":
 
     acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
     void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)

pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c

@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/sim_interface.h"
 #include "acados_c/external_function_interface.h"
 

pyextra/acados_template/c_templates_tera/main.in.c

@@ -156,11 +156,12 @@ int main()
     for (int ii = 0; ii < NTIMINGS; ii++)
     {
         // initialize solution
-        for (int i = 0; i <= nlp_dims->N; i++)
+        for (int i = 0; i < N; i++)
         {
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
         }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
         ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
         status = {{ model.name }}_acados_solve(acados_ocp_capsule);
         ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);

pyextra/acados_template/c_templates_tera/make_sfun.in.m

@@ -46,10 +46,14 @@ SOURCES = { ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
             {%- endif %}
         {%- elif solver_options.integrator_type == "GNSF" %}
+            {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
+            {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
+            {%- endif %}
+            {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
         {%- elif solver_options.integrator_type == "DISCRETE" %}
             '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...

pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m

@@ -47,10 +47,14 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
                 {%- endif %}
             {%- elif solver_options.integrator_type == "GNSF" %}
+                {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
+                {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
+                {%- endif %}
+                {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
             {%- endif %}
           ];

pyextra/acados_template/c_templates_tera/mex_solver.in.m

@@ -125,15 +125,15 @@ classdef {{ model.name }}_mex_solver < handle
             if strcmp(field, 'stat')
                 stat = obj.get('stat');
                 {%- if solver_options.nlp_solver_type == "SQP" %}
-                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
-                if size(stat,2)>7
+                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
+                if size(stat,2)>8
                     fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
                 end
                 fprintf('\n');
                 for jj=1:size(stat,1)
-                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
-                    if size(stat,2)>7
-                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
+                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
+                    if size(stat,2)>8
+                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
                     end
                     fprintf('\n');
                 end

pyextra/acados_template/c_templates_tera/model.in.h

@@ -90,14 +90,7 @@ int {{ model.name }}_impl_dae_hess_n_out(void);
 
 {% elif solver_options.integrator_type == "GNSF" %}
 /* GNSF Functions */
-// used to import model matrices
-int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
-int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
-
+	{% if model.gnsf.purely_linear != 1 %}
 // phi_fun
 int        {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
@@ -121,7 +114,7 @@ const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
 const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
-
+	{% if model.gnsf.nontrivial_f_LO == 1 %}
 // f_lo_fun_jac_x1k1uz
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
@@ -129,6 +122,15 @@ const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
 const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
+	{%- endif %}
+	{%- endif %}
+// used to import model matrices
+int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
 
 {% elif solver_options.integrator_type == "ERK" %}
 /* explicit ODE */

pyextra/acados_template/utils.py

@@ -254,22 +254,6 @@ def format_class_dict(d):
     return out
 
 
-def acados_class2dict(class_instance):
-    """
-    removes the __ artifact from class to dict conversion
-    """
-
-    d = dict(class_instance.__dict__)
-    out = {}
-    for k, v in d.items():
-        if isinstance(v, dict):
-            v = format_class_dict(v)
-
-        out_key = k.split('__', 1)[-1]
-        out[k.replace(k, out_key)] = v
-    return out
-
-
 def get_ocp_nlp_layout():
     python_interface_path = get_python_interface_path()
     abs_path = os.path.join(python_interface_path, 'acados_layout.json')
@@ -433,6 +417,13 @@ def set_up_imported_gnsf_model(acados_formulation):
     acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
     acados_formulation.model.get_matrices_fun = get_matrices_fun
 
+    # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
+    #  {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
+    #   nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
+    get_matrices_out = get_matrices_fun(0)
+    acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
+    acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13])
+
     if "f_lo_fun_jac_x1k1uz" in gnsf:
         f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
         acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz

rednose_repo

@@ -1 +1 @@
-Subproject commit 5b526a8e00bdc1c3922be470af1602cf9dc72dde
+Subproject commit 7663289f1e68860f53dc34337ef080dde69a2586

release/files_common

@@ -17,6 +17,7 @@ site_scons/site_tools/cython.py
 
 common/.gitignore
 common/__init__.py
+common/conversions.py
 common/gpio.py
 common/realtime.py
 common/clock.pyx
@@ -58,7 +59,6 @@ common/transformations/transformations.pyx
 common/api/__init__.py
 
 models/supercombo.dlc
-models/big_supercombo.dlc
 models/dmonitoring_model_q.dlc
 
 release/*
@@ -70,12 +70,10 @@ installer/updater/updater
 selfdrive/version.py
 
 selfdrive/__init__.py
-selfdrive/config.py
 selfdrive/sentry.py
 selfdrive/swaglog.py
 selfdrive/logmessaged.py
 selfdrive/tombstoned.py
-selfdrive/pandad.py
 selfdrive/updated.py
 selfdrive/rtshield.py
 selfdrive/statsd.py
@@ -99,6 +97,7 @@ selfdrive/boardd/panda.h
 selfdrive/boardd/pigeon.cc
 selfdrive/boardd/pigeon.h
 selfdrive/boardd/set_time.py
+selfdrive/boardd/pandad.py
 
 selfdrive/car/__init__.py
 selfdrive/car/car_helpers.py
@@ -337,11 +336,11 @@ selfdrive/sensord/sensord
 
 selfdrive/thermald/thermald.py
 selfdrive/thermald/power_monitoring.py
+selfdrive/thermald/fan_controller.py
 
 selfdrive/test/__init__.py
 selfdrive/test/helpers.py
 selfdrive/test/setup_device_ci.sh
-selfdrive/test/test_fingerprints.py
 selfdrive/test/test_onroad.py
 
 selfdrive/ui/.gitignore
@@ -424,6 +423,7 @@ selfdrive/modeld/transforms/transform.cc
 selfdrive/modeld/transforms/transform.h
 selfdrive/modeld/transforms/transform.cl
 
+selfdrive/modeld/thneed/*.py
 selfdrive/modeld/thneed/thneed.*
 selfdrive/modeld/thneed/serialize.cc
 selfdrive/modeld/thneed/compile.cc

selfdrive/athena/athenad.py

@@ -163,8 +163,6 @@ def upload_handler(end_event: threading.Event) -> None:
   sm = messaging.SubMaster(['deviceState'])
   tid = threading.get_ident()
 
-  cellular_unmetered = Params().get_bool("CellularUnmetered")
-
   while not end_event.is_set():
     cur_upload_items[tid] = None
 
@@ -181,46 +179,45 @@ def upload_handler(end_event: threading.Event) -> None:
         cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True)
         continue
 
-      # Check if uploading over cell is allowed
+      # Check if uploading over metered connection is allowed
       sm.update(0)
-      cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
-      if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+      metered = sm['deviceState'].networkMetered
+      network_type = sm['deviceState'].networkType.raw
+      if metered and (not cur_upload_items[tid].allow_cellular):
         retry_upload(tid, end_event, False)
         continue
 
       try:
         def cb(sz, cur):
-          # Abort transfer if connection changed to cell after starting upload
+          # Abort transfer if connection changed to metered after starting upload
           sm.update(0)
-          cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet]
-          if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+          metered = sm['deviceState'].networkMetered
+          if metered and (not cur_upload_items[tid].allow_cellular):
             raise AbortTransferException
 
           cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
 
-
-        network_type = sm['deviceState'].networkType.raw
         fn = cur_upload_items[tid].path
         try:
           sz = os.path.getsize(fn)
         except OSError:
           sz = -1
 
-        cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type, metered=metered)
         response = _do_upload(cur_upload_items[tid], cb)
 
         if response.status_code not in (200, 201, 403, 412):
-          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type)
+          cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type, metered=metered)
           retry_upload(tid, end_event)
         else:
-          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type)
+          cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type, metered=metered)
 
         UploadQueueCache.cache(upload_queue)
       except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError):
-        cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type, metered=metered)
         retry_upload(tid, end_event)
       except AbortTransferException:
-        cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type, metered=metered)
         retry_upload(tid, end_event, False)
 
     except queue.Empty:
@@ -459,6 +456,12 @@ def getNetworkType():
   return HARDWARE.get_network_type()
 
 
+@dispatcher.add_method
+def getNetworkMetered():
+  network_type = HARDWARE.get_network_type()
+  return HARDWARE.get_network_metered(network_type)
+
+
 @dispatcher.add_method
 def getNetworks():
   return HARDWARE.get_networks()

selfdrive/boardd/boardd.cc

@@ -371,7 +371,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
 
     size_t j = 0;
     for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
-        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_EXTI); f++) {
       if (fault_bits.test(f)) {
         faults.set(j, cereal::PandaState::FaultType(f));
         j++;

selfdrive/boardd/panda.cc

@@ -338,6 +338,10 @@ void Panda::set_power_saving(bool power_saving) {
   usb_write(0xe7, power_saving, 0);
 }
 
+void Panda::enable_deepsleep() {
+  usb_write(0xfb, 0, 0);
+}
+
 void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) {
   usb_write(0xe6, (uint16_t)power_mode, 0);
 }

selfdrive/boardd/panda.h

@@ -85,6 +85,7 @@ class Panda {
   std::optional<std::vector<uint8_t>> get_firmware_version();
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
+  void enable_deepsleep();
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
   void send_heartbeat(bool engaged);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);

selfdrive/camerad/cameras/camera_common.cc

@@ -161,6 +161,8 @@ bool CameraBuf::acquire() {
   cl_mem camrabuf_cl = camera_bufs[cur_buf_idx].buf_cl;
   cl_event event;
 
+  float start_time = millis_since_boot();
+
   if (debayer) {
     float gain = 0.0;
 
@@ -181,6 +183,8 @@ bool CameraBuf::acquire() {
   cur_yuv_buf = vipc_server->get_buffer(yuv_type);
   rgb2yuv->queue(q, cur_rgb_buf->buf_cl, cur_yuv_buf->buf_cl);
 
+  cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
+
   VisionIpcBufExtra extra = {
                         cur_frame_data.frame_id,
                         cur_frame_data.timestamp_sof,
@@ -219,6 +223,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
   framed.setLensPos(frame_data.lens_pos);
   framed.setLensErr(frame_data.lens_err);
   framed.setLensTruePos(frame_data.lens_true_pos);
+  framed.setProcessingTime(frame_data.processing_time);
 }
 
 kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {

selfdrive/camerad/cameras/camera_common.h

@@ -75,6 +75,8 @@ typedef struct FrameMetadata {
   unsigned int lens_pos;
   float lens_err;
   float lens_true_pos;
+
+  float processing_time;
 } FrameMetadata;
 
 typedef struct CameraExpInfo {

selfdrive/camerad/cameras/camera_qcom.cc

@@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
               /*fps*/ 20,
 #endif
               device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
 
   camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
               /*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
               /*max_gain=*/510, 10, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+              VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
 
   s->sm = new SubMaster({"driverState"});
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -26,7 +26,7 @@ extern ExitHandler do_exit;
 
 const size_t FRAME_WIDTH = 1928;
 const size_t FRAME_HEIGHT = 1208;
-const size_t FRAME_STRIDE = 2416;  // for 10 bit output
+const size_t FRAME_STRIDE = 2896;  // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
 
 const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
 
@@ -56,7 +56,7 @@ const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
 const int EXPOSURE_TIME_MAX = 1904; // with HDR, slowest ss
 
 // ************** low level camera helpers ****************
-int cam_control(int fd, int op_code, void *handle, int size) {
+int do_cam_control(int fd, int op_code, void *handle, int size) {
   struct cam_control camcontrol = {0};
   camcontrol.op_code = op_code;
   camcontrol.handle = (uint64_t)handle;
@@ -83,7 +83,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
       .num_resources = (uint32_t)(data ? 1 : 0),
       .resource_hdl = (uint64_t)data,
   };
-  int err = cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
+  int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
   return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
 };
 
@@ -93,13 +93,13 @@ int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t p
       .dev_handle = dev_handle,
       .packet_handle = packet_handle,
   };
-  return cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
+  return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
 }
 
 int device_control(int fd, int op_code, int session_handle, int dev_handle) {
   // start stop and release are all the same
   struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
-  return cam_control(fd, op_code, &cmd, sizeof(cmd));
+  return do_cam_control(fd, op_code, &cmd, sizeof(cmd));
 }
 
 void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
@@ -118,7 +118,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
     mem_mgr_alloc_cmd.num_hdl++;
   }
 
-  cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
+  do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
   *handle = mem_mgr_alloc_cmd.out.buf_handle;
 
   void *ptr = NULL;
@@ -137,7 +137,7 @@ void release(int video0_fd, uint32_t handle) {
   struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
   mem_mgr_release_cmd.buf_handle = handle;
 
-  ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
+  ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
   assert(ret == 0);
 }
 
@@ -153,34 +153,39 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
   req_mgr_flush_request.link_hdl = link_hdl;
   req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
   int ret;
-  ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
+  ret = do_cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
   // LOGD("flushed all req: %d", ret);
 }
 
 // ************** high level camera helpers ****************
 
-void sensors_poke(struct CameraState *s, int request_id) {
+void CameraState::sensors_start() {
+  int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
+  sensors_i2c(start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+}
+
+void CameraState::sensors_poke(int request_id) {
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet);
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 0;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
   pkt->header.op_code = 0x7f;
   pkt->header.request_id = request_id;
 
-  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
+  int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
   assert(ret == 0);
 
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
 
-void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
+void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code) {
   // LOGD("sensors_i2c: %d", len);
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 1;
   pkt->kmd_cmd_buf_index = -1;
   pkt->header.size = size;
@@ -190,7 +195,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
   buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
   buf_desc[0].type = CAM_CMD_BUF_I2C;
 
-  struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+  struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
   i2c_random_wr->header.count = len;
   i2c_random_wr->header.op_code = 1;
   i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
@@ -198,14 +203,15 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
   i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
   memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
 
-  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
+  int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
   assert(ret == 0);
 
   munmap(i2c_random_wr, buf_desc[0].size);
-  release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+  release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
+
 static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
   cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
   unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
@@ -214,7 +220,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
   return (struct cam_cmd_power *)(unconditional_wait + 1);
 };
 
-void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
+void CameraState::sensors_init() {
+  int video0_fd = multi_cam_state->video0_fd;
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
   struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
@@ -333,7 +340,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
   power->power_settings[2].power_seq_type = 3;
 
   LOGD("probing the sensor");
-  int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
+  int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
   assert(ret == 0);
 
   munmap(i2c_info, buf_desc[0].size);
@@ -344,13 +351,13 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
   release_fd(video0_fd, cam_packet_handle);
 }
 
-void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
+void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
   uint32_t cam_packet_handle = 0;
   int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
   if (io_mem_handle != 0) {
     size += sizeof(struct cam_buf_io_cfg);
   }
-  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+  struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 2;
   pkt->kmd_cmd_buf_index = 0;
   // YUV has kmd_cmd_buf_offset = 1780
@@ -445,7 +452,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
   buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
   buf_desc[1].type = CAM_CMD_BUF_GENERIC;
   buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
-  uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
+  uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
   memcpy(buf2, &tmp, sizeof(tmp));
 
   if (io_mem_handle != 0) {
@@ -464,10 +471,10 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
 		 .h_init = 0x0,
 		 .v_init = 0x0,
 		};
-    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;             // CAM_FORMAT_UBWC_TP10 for YUV
+    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12;             // CAM_FORMAT_UBWC_TP10 for YUV
     io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;          // CAM_COLOR_SPACE_BT601_FULL for YUV
     io_cfg[0].color_pattern = 0x5;                         // 0x0 for YUV
-    io_cfg[0].bpp = 0xa;
+    io_cfg[0].bpp = 0xc;
     io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;   // CAM_ISP_IFE_OUT_RES_FULL for YUV
     io_cfg[0].fence = fence;
     io_cfg[0].direction = CAM_BUF_OUTPUT;
@@ -475,142 +482,141 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
     io_cfg[0].framedrop_pattern = 0x1;
   }
 
-  int ret = device_config(s->multi_cam_state->isp_fd, s->session_handle, s->isp_dev_handle, cam_packet_handle);
+  int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
   assert(ret == 0);
   if (ret != 0) {
     printf("ISP CONFIG FAILED\n");
   }
 
   munmap(buf2, buf_desc[1].size);
-  release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
-  // release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+  release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle);
+  // release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
   munmap(pkt, size);
-  release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+  release_fd(multi_cam_state->video0_fd, cam_packet_handle);
 }
 
-void enqueue_buffer(struct CameraState *s, int i, bool dp) {
+void CameraState::enqueue_buffer(int i, bool dp) {
   int ret;
-  int request_id = s->request_ids[i];
+  int request_id = request_ids[i];
 
-  if (s->buf_handle[i]) {
-    release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
+  if (buf_handle[i]) {
+    release(multi_cam_state->video0_fd, buf_handle[i]);
     // wait
     struct cam_sync_wait sync_wait = {0};
-    sync_wait.sync_obj = s->sync_objs[i];
+    sync_wait.sync_obj = sync_objs[i];
     sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
-    ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
+    ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
     // LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
 
-    s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
-    if (dp) s->buf.queue(i);
+    buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
+    if (dp) buf.queue(i);
 
     // destroy old output fence
     struct cam_sync_info sync_destroy = {0};
     strcpy(sync_destroy.name, "NodeOutputPortFence");
-    sync_destroy.sync_obj = s->sync_objs[i];
-    ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
+    sync_destroy.sync_obj = sync_objs[i];
+    ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
     // LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
   }
 
   // do stuff
   struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
-  req_mgr_sched_request.session_hdl = s->session_handle;
-  req_mgr_sched_request.link_hdl = s->link_handle;
+  req_mgr_sched_request.session_hdl = session_handle;
+  req_mgr_sched_request.link_hdl = link_handle;
   req_mgr_sched_request.req_id = request_id;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
   // LOGD("sched req: %d %d", ret, request_id);
 
   // create output fence
   struct cam_sync_info sync_create = {0};
   strcpy(sync_create.name, "NodeOutputPortFence");
-  ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
+  ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
   // LOGD("fence req: %d %d", ret, sync_create.sync_obj);
-  s->sync_objs[i] = sync_create.sync_obj;
+  sync_objs[i] = sync_create.sync_obj;
 
   // configure ISP to put the image in place
   struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
-  mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
+  mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
   mem_mgr_map_cmd.num_hdl = 1;
   mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
-  mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
-  // LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
-  s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
+  mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
+  // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
+  buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
 
   // poke sensor
-  sensors_poke(s, request_id);
+  sensors_poke(request_id);
   // LOGD("Poked sensor");
 
   // push the buffer
-  config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1));
+  config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
 }
 
-void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
+void CameraState::enqueue_req_multi(int start, int n, bool dp) {
    for (int i=start;i<start+n;++i) {
-     s->request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
-     enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
+     request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
+     enqueue_buffer((i - 1) % FRAME_BUF_COUNT, dp);
    }
 }
 
 // ******************* camera *******************
 
-static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
+void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
   LOGD("camera init %d", camera_num);
-  s->multi_cam_state = multi_cam_state;
+  multi_cam_state = multi_cam_state_;
   assert(camera_id < std::size(cameras_supported));
-  s->ci = cameras_supported[camera_id];
-  assert(s->ci.frame_width != 0);
+  ci = cameras_supported[camera_id];
+  assert(ci.frame_width != 0);
 
-  s->camera_num = camera_num;
+  camera_num = camera_num_;
 
-  s->request_id_last = 0;
-  s->skipped = true;
+  request_id_last = 0;
+  skipped = true;
 
-  s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
-  s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
-  s->target_grey_fraction = 0.3;
+  min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
+  max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
+  target_grey_fraction = 0.3;
 
-  s->dc_gain_enabled = false;
-  s->gain_idx = ANALOG_GAIN_REC_IDX;
-  s->exposure_time = 5;
-  s->cur_ev[0] = s->cur_ev[1] = s->cur_ev[2] = (s->dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[s->gain_idx] * s->exposure_time;
+  dc_gain_enabled = false;
+  gain_idx = ANALOG_GAIN_REC_IDX;
+  exposure_time = 5;
+  cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time;
 
-  s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
+  buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
 }
 
-static void camera_open(CameraState *s) {
-  s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
-  assert(s->sensor_fd >= 0);
-  LOGD("opened sensor for %d", s->camera_num);
+void CameraState::camera_open() {
+  sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
+  assert(sensor_fd >= 0);
+  LOGD("opened sensor for %d", camera_num);
 
   // probe the sensor
-  LOGD("-- Probing sensor %d", s->camera_num);
-  sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
+  LOGD("-- Probing sensor %d", camera_num);
+  sensors_init();
 
   // create session
   struct cam_req_mgr_session_info session_info = {};
-  int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
+  int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
   LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
-  s->session_handle = session_info.session_hdl;
+  session_handle = session_info.session_hdl;
 
   // access the sensor
   LOGD("-- Accessing sensor");
-  auto sensor_dev_handle = device_acquire(s->sensor_fd, s->session_handle, nullptr);
-  assert(sensor_dev_handle);
-  s->sensor_dev_handle = *sensor_dev_handle;
+  auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr);
+  assert(sensor_dev_handle_);
+  sensor_dev_handle = *sensor_dev_handle_;
   LOGD("acquire sensor dev");
 
   struct cam_isp_in_port_info in_port_info = {
-      .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[s->camera_num],
+      .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
 
       .lane_type = CAM_ISP_LANE_TYPE_DPHY,
       .lane_num = 4,
       .lane_cfg = 0x3210,
 
       .vc = 0x0,
-      // .dt = 0x2C; //CSI_RAW12
-      .dt = 0x2B,  //CSI_RAW10
-      .format = CAM_FORMAT_MIPI_RAW_10,
+      .dt = 0x2C,  // CSI_RAW12
+      .format = CAM_FORMAT_MIPI_RAW_12,
 
       .test_pattern = 0x2,  // 0x3?
       .usage_type = 0x0,
@@ -636,7 +642,7 @@ static void camera_open(CameraState *s) {
       .num_out_res = 0x1,
       .data[0] = (struct cam_isp_out_port_info){
           .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
-          .format = CAM_FORMAT_MIPI_RAW_10,
+          .format = CAM_FORMAT_MIPI_RAW_12,
           .width = FRAME_WIDTH,
           .height = FRAME_HEIGHT,
           .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
@@ -649,29 +655,29 @@ static void camera_open(CameraState *s) {
       .length = sizeof(in_port_info),
   };
 
-  auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
-  assert(isp_dev_handle);
-  s->isp_dev_handle = *isp_dev_handle;
+  auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
+  assert(isp_dev_handle_);
+  isp_dev_handle = *isp_dev_handle_;
   LOGD("acquire isp dev");
 
-  s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
-  assert(s->csiphy_fd >= 0);
-  LOGD("opened csiphy for %d", s->camera_num);
+  csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num);
+  assert(csiphy_fd >= 0);
+  LOGD("opened csiphy for %d", camera_num);
 
   struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
-  auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
-  assert(csiphy_dev_handle);
-  s->csiphy_dev_handle = *csiphy_dev_handle;
+  auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info);
+  assert(csiphy_dev_handle_);
+  csiphy_dev_handle = *csiphy_dev_handle_;
   LOGD("acquire csiphy dev");
 
   // config ISP
-  alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
-  config_isp(s, 0, 0, 1, s->buf0_handle, 0);
+  alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu);
+  config_isp(0, 0, 1, buf0_handle, 0);
 
   LOG("-- Configuring sensor");
-  sensors_i2c(s, init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-  //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
-  //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
+  sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  //sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
+  //sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
 
 
   // config csiphy
@@ -679,7 +685,7 @@ static void camera_open(CameraState *s) {
   {
     uint32_t cam_packet_handle = 0;
     int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
-    struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
+    struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
     pkt->num_cmd_buf = 1;
     pkt->kmd_cmd_buf_index = -1;
     pkt->header.size = size;
@@ -688,7 +694,7 @@ static void camera_open(CameraState *s) {
     buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
     buf_desc[0].type = CAM_CMD_BUF_GENERIC;
 
-    struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
+    struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
     csiphy_info->lane_mask = 0x1f;
     csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
     csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
@@ -698,54 +704,51 @@ static void camera_open(CameraState *s) {
     csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
     csiphy_info->data_rate = 48000000;  // Calculated by camera_freqs.py
 
-    int ret_ = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
+    int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
     assert(ret_ == 0);
 
     munmap(csiphy_info, buf_desc[0].size);
-    release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
+    release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
     munmap(pkt, size);
-    release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
+    release_fd(multi_cam_state->video0_fd, cam_packet_handle);
   }
 
   // link devices
   LOG("-- Link devices");
   struct cam_req_mgr_link_info req_mgr_link_info = {0};
-  req_mgr_link_info.session_hdl = s->session_handle;
+  req_mgr_link_info.session_hdl = session_handle;
   req_mgr_link_info.num_devices = 2;
-  req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
-  req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
-  s->link_handle = req_mgr_link_info.link_hdl;
-  LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
+  req_mgr_link_info.dev_hdls[0] = isp_dev_handle;
+  req_mgr_link_info.dev_hdls[1] = sensor_dev_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
+  link_handle = req_mgr_link_info.link_hdl;
+  LOGD("link: %d hdl: 0x%X", ret, link_handle);
 
   struct cam_req_mgr_link_control req_mgr_link_control = {0};
   req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
-  req_mgr_link_control.session_hdl = s->session_handle;
+  req_mgr_link_control.session_hdl = session_handle;
   req_mgr_link_control.num_links = 1;
-  req_mgr_link_control.link_hdls[0] = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
+  req_mgr_link_control.link_hdls[0] = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
   LOGD("link control: %d", ret);
 
-  ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle);
   LOGD("start csiphy: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
   LOGD("start isp: %d", ret);
-  ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
+  ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle);
   LOGD("start sensor: %d", ret);
 
-  enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0);
+  enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
 }
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
-  camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+  s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
   printf("driver camera initted \n");
   if (!env_only_driver) {
-    camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
-                VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
+    s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD); // swap left/right
     printf("road camera initted \n");
-    camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
-                VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
+    s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE_ROAD, VISION_STREAM_WIDE_ROAD);
     printf("wide road camera initted \n");
   }
 
@@ -768,7 +771,7 @@ void cameras_open(MultiCameraState *s) {
   LOGD("opened video1");
 
   // looks like there's only one of these
-  s->isp_fd = HANDLE_EINTR(open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK));
+  s->isp_fd = open_v4l_by_name_and_index("cam-isp");
   assert(s->isp_fd >= 0);
   LOGD("opened isp");
 
@@ -779,7 +782,7 @@ void cameras_open(MultiCameraState *s) {
   query_cap_cmd.handle_type = 1;
   query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd;
   query_cap_cmd.size = sizeof(isp_query_cap_cmd);
-  ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
+  ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
   assert(ret == 0);
   LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
   LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
@@ -794,74 +797,72 @@ void cameras_open(MultiCameraState *s) {
   ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
   printf("req mgr subscribe: %d\n", ret);
 
-  camera_open(&s->driver_cam);
+  s->driver_cam.camera_open();
   printf("driver camera opened \n");
   if (!env_only_driver) {
-    camera_open(&s->road_cam);
+    s->road_cam.camera_open();
     printf("road camera opened \n");
-    camera_open(&s->wide_road_cam);
+    s->wide_road_cam.camera_open();
     printf("wide road camera opened \n");
   }
 }
 
-static void camera_close(CameraState *s) {
+void CameraState::camera_close() {
   int ret;
 
   // stop devices
   LOG("-- Stop devices");
-  // ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
+  // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle);
   // LOGD("stop sensor: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle);
   LOGD("stop isp: %d", ret);
-  ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle);
   LOGD("stop csiphy: %d", ret);
   // link control stop
   LOG("-- Stop link control");
   static struct cam_req_mgr_link_control req_mgr_link_control = {0};
   req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE;
-  req_mgr_link_control.session_hdl = s->session_handle;
+  req_mgr_link_control.session_hdl = session_handle;
   req_mgr_link_control.num_links = 1;
-  req_mgr_link_control.link_hdls[0] = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
+  req_mgr_link_control.link_hdls[0] = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
   LOGD("link control stop: %d", ret);
 
   // unlink
   LOG("-- Unlink");
   static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
-  req_mgr_unlink_info.session_hdl = s->session_handle;
-  req_mgr_unlink_info.link_hdl = s->link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
+  req_mgr_unlink_info.session_hdl = session_handle;
+  req_mgr_unlink_info.link_hdl = link_handle;
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
   LOGD("unlink: %d", ret);
 
   // release devices
   LOGD("-- Release devices");
-  ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
+  ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle);
   LOGD("release sensor: %d", ret);
-  ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
+  ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle);
   LOGD("release isp: %d", ret);
-  ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
+  ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle);
   LOGD("release csiphy: %d", ret);
 
   // destroyed session
-  struct cam_req_mgr_session_info session_info = {.session_hdl = s->session_handle};
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
+  struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle};
+  ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
   LOGD("destroyed session: %d", ret);
 }
 
 void cameras_close(MultiCameraState *s) {
-  camera_close(&s->driver_cam);
+  s->driver_cam.camera_close();
   if (!env_only_driver) {
-    camera_close(&s->road_cam);
-    camera_close(&s->wide_road_cam);
+    s->road_cam.camera_close();
+    s->wide_road_cam.camera_close();
   }
 
   delete s->sm;
   delete s->pm;
 }
 
-// ******************* just a helper *******************
-
-void handle_camera_event(CameraState *s, void *evdat) {
+void CameraState::handle_camera_event(void *evdat) {
   struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
 
   uint64_t timestamp = event_data->u.frame_msg.timestamp;
@@ -869,53 +870,53 @@ void handle_camera_event(CameraState *s, void *evdat) {
   int real_id = event_data->u.frame_msg.request_id;
 
   if (real_id != 0) { // next ready
-    if (real_id == 1) {s->idx_offset = main_id;}
+    if (real_id == 1) {idx_offset = main_id;}
     int buf_idx = (real_id - 1) % FRAME_BUF_COUNT;
 
     // check for skipped frames
-    if (main_id > s->frame_id_last + 1 && !s->skipped) {
+    if (main_id > frame_id_last + 1 && !skipped) {
       // realign
-      clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
-      enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
-      s->skipped = true;
-    } else if (main_id == s->frame_id_last + 1) {
-      s->skipped = false;
+      clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
+      enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0);
+      skipped = true;
+    } else if (main_id == frame_id_last + 1) {
+      skipped = false;
     }
 
     // check for dropped requests
-    if (real_id > s->request_id_last + 1) {
-      enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
+    if (real_id > request_id_last + 1) {
+      enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0);
     }
 
     // metas
-    s->frame_id_last = main_id;
-    s->request_id_last = real_id;
+    frame_id_last = main_id;
+    request_id_last = real_id;
 
-    auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
-    meta_data.frame_id = main_id - s->idx_offset;
+    auto &meta_data = buf.camera_bufs_metadata[buf_idx];
+    meta_data.frame_id = main_id - idx_offset;
     meta_data.timestamp_sof = timestamp;
-    s->exp_lock.lock();
-    meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
-    meta_data.high_conversion_gain = s->dc_gain_enabled;
-    meta_data.integ_lines = s->exposure_time;
-    meta_data.measured_grey_fraction = s->measured_grey_fraction;
-    meta_data.target_grey_fraction = s->target_grey_fraction;
-    s->exp_lock.unlock();
+    exp_lock.lock();
+    meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac;
+    meta_data.high_conversion_gain = dc_gain_enabled;
+    meta_data.integ_lines = exposure_time;
+    meta_data.measured_grey_fraction = measured_grey_fraction;
+    meta_data.target_grey_fraction = target_grey_fraction;
+    exp_lock.unlock();
 
     // dispatch
-    enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
+    enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1);
   } else { // not ready
     // reset after half second of no response
-    if (main_id > s->frame_id_last + 10) {
-      clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
-      enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
-      s->frame_id_last = main_id;
-      s->skipped = true;
+    if (main_id > frame_id_last + 10) {
+      clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
+      enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0);
+      frame_id_last = main_id;
+      skipped = true;
     }
   }
 }
 
-static void set_camera_exposure(CameraState *s, float grey_frac) {
+void CameraState::set_camera_exposure(float grey_frac) {
   const float dt = 0.05;
 
   const float ts_grey = 10.0;
@@ -929,15 +930,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
   // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
   // TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
 
-  const float cur_ev = s->cur_ev[s->buf.cur_frame_data.frame_id % 3];
+  const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3];
 
   // Scale target grey between 0.1 and 0.4 depending on lighting conditions
-  float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev) / log2(6000.0), 0.1, 0.4);
-  float target_grey = (1.0 - k_grey) * s->target_grey_fraction + k_grey * new_target_grey;
+  float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4);
+  float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
 
-  float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->max_ev);
+  float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev);
   float k = (1.0 - k_ev) / 3.0;
-  desired_ev = (k * s->cur_ev[0]) + (k * s->cur_ev[1]) + (k * s->cur_ev[2]) + (k_ev * desired_ev);
+  desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
 
   float best_ev_score = 1e6;
   int new_g = 0;
@@ -945,7 +946,7 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
 
   // Hysteresis around high conversion gain
   // We usually want this on since it results in lower noise, but turn off in very bright day scenes
-  bool enable_dc_gain = s->dc_gain_enabled;
+  bool enable_dc_gain = dc_gain_enabled;
   if (!enable_dc_gain && target_grey < 0.2) {
     enable_dc_gain = true;
   } else if (enable_dc_gain && target_grey > 0.3) {
@@ -954,14 +955,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
 
   // Simple brute force optimizer to choose sensor parameters
   // to reach desired EV
-  for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, s->gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, s->gain_idx + 1); g++) {
+  for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) {
     float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1);
 
     // Compute optimal time for given gain
     int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX);
 
     // Only go below recomended gain when absolutely necessary to not overexpose
-    if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < s->gain_idx) {
+    if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) {
       continue;
     }
 
@@ -972,10 +973,10 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
     float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1;
     score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m;
 
-    // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s->camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s->gain_idx) * (score + 1.0) / 10.0, desired_ev, s->min_ev);
+    // LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
 
     // Small penalty on changing gain
-    score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0;
+    score += std::abs(g - gain_idx) * (score + 1.0) / 10.0;
 
     if (score < best_ev_score) {
       new_t = t;
@@ -984,42 +985,41 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
     }
   }
 
-  s->exp_lock.lock();
+  exp_lock.lock();
 
-  s->measured_grey_fraction = grey_frac;
-  s->target_grey_fraction = target_grey;
+  measured_grey_fraction = grey_frac;
+  target_grey_fraction = target_grey;
 
-  s->analog_gain_frac = sensor_analog_gains[new_g];
-  s->gain_idx = new_g;
-  s->exposure_time = new_t;
-  s->dc_gain_enabled = enable_dc_gain;
+  analog_gain_frac = sensor_analog_gains[new_g];
+  gain_idx = new_g;
+  exposure_time = new_t;
+  dc_gain_enabled = enable_dc_gain;
 
-  float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
-  s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
+  float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0);
+  cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
 
-  s->exp_lock.unlock();
+  exp_lock.unlock();
 
   // Processing a frame takes right about 50ms, so we need to wait a few ms
   // so we don't send i2c commands around the frame start.
-  int ms = (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof) / 1000000;
+  int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000;
   if (ms < 60) {
     util::sleep_for(60 - ms);
   }
-  // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s->camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s->buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof));
+  // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
 
   uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
   struct i2c_random_wr_payload exp_reg_array[] = {
                                                   {0x3366, analog_gain_reg},
-                                                  {0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)},
-                                                  {0x3012, (uint16_t)s->exposure_time},
+                                                  {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
+                                                  {0x3012, (uint16_t)exposure_time},
                                                 };
-  sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
+  sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
               CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-
 }
 
 void camera_autoexposure(CameraState *s, float grey_frac) {
-  set_camera_exposure(s, grey_frac);
+  s->set_camera_exposure(grey_frac);
 }
 
 // called by processing_thread
@@ -1053,11 +1053,10 @@ void cameras_run(MultiCameraState *s) {
 
   // start devices
   LOG("-- Starting devices");
-  int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
-  sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  s->driver_cam.sensors_start();
   if (!env_only_driver) {
-    sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-    sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+    s->road_cam.sensors_start();
+    s->wide_road_cam.sensors_start();
   }
 
   // poll events
@@ -1088,11 +1087,11 @@ void cameras_run(MultiCameraState *s) {
         }
 
         if (event_data->session_hdl == s->road_cam.session_handle) {
-          handle_camera_event(&s->road_cam, event_data);
+          s->road_cam.handle_camera_event(event_data);
         } else if (event_data->session_hdl == s->wide_road_cam.session_handle) {
-          handle_camera_event(&s->wide_road_cam, event_data);
+          s->wide_road_cam.handle_camera_event(event_data);
         } else if (event_data->session_hdl == s->driver_cam.session_handle) {
-          handle_camera_event(&s->driver_cam, event_data);
+          s->driver_cam.handle_camera_event(event_data);
         } else {
           printf("Unknown vidioc event source\n");
           assert(false);

selfdrive/camerad/cameras/camera_qcom2.h

@@ -9,7 +9,8 @@
 
 #define FRAME_BUF_COUNT 4
 
-typedef struct CameraState {
+class CameraState {
+public:
   MultiCameraState *multi_cam_state;
   CameraInfo ci;
 
@@ -31,6 +32,21 @@ typedef struct CameraState {
 
   int camera_num;
 
+  void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
+  void enqueue_req_multi(int start, int n, bool dp);
+  void enqueue_buffer(int i, bool dp);
+  void handle_camera_event(void *evdat);
+  void set_camera_exposure(float grey_frac);
+
+  void sensors_start();
+  void sensors_poke(int request_id);
+  void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code);
+  void sensors_init();
+
+  void camera_open();
+  void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type);
+  void camera_close();
+
   int32_t session_handle;
   int32_t sensor_dev_handle;
   int32_t isp_dev_handle;
@@ -48,7 +64,7 @@ typedef struct CameraState {
   bool skipped;
 
   CameraBuf buf;
-} CameraState;
+};
 
 typedef struct MultiCameraState {
   unique_fd video0_fd;

selfdrive/camerad/cameras/camera_replay.cc

@@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
   camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
   // camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
-  //             VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
+  //             VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
 }
 

selfdrive/camerad/cameras/camera_webcam.cc

@@ -141,9 +141,9 @@ void driver_camera_thread(CameraState *s) {
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
   camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
+              VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
   camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
-              VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
+              VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
 }
 

selfdrive/camerad/cameras/real_debayer.cl

@@ -40,12 +40,12 @@ half3 color_correct(half3 rgb) {
 }
 
 half val_from_10(const uchar * source, int gx, int gy) {
-  // parse 10bit
-  int start = gy * FRAME_STRIDE + (5 * (gx / 4));
-  int offset = gx % 4;
-  uint major = (uint)source[start + offset] << 2;
-  uint minor = (source[start + 4] >> (2 * offset)) & 3;
-  half pv = (half)(major + minor);
+  // parse 12bit
+  int start = gy * FRAME_STRIDE + (3 * (gx / 2));
+  int offset = gx % 2;
+  uint major = (uint)source[start + offset] << 4;
+  uint minor = (source[start + 2] >> (4 * offset)) & 0xf;
+  half pv = (half)((major + minor)/4);
 
   // normalize
   pv = max(0.0h, pv - black_level);

selfdrive/camerad/cameras/sensor2_i2c.h

@@ -9,7 +9,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
   {0x302C, 0x0001}, // VT_SYS_CLK_DIV
   {0x302E, 0x0002}, // PRE_PLL_CLK_DIV
   {0x3030, 0x0032}, // PLL_MULTIPLIER
-  {0x3036, 0x000A}, // OP_WORD_CLK_DIV
+  {0x3036, 0x000C}, // OP_WORD_CLK_DIV
   {0x3038, 0x0001}, // OP_SYS_CLK_DIV
 
   // FORMAT
@@ -46,11 +46,11 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
 
   // Readout Settings
   {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
-  {0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10
-  {0x3342, 0x122B}, // MIPI_F1_PDT_EDT
-  {0x3346, 0x122B}, // MIPI_F2_PDT_EDT
-  {0x334A, 0x122B}, // MIPI_F3_PDT_EDT
-  {0x334E, 0x122B}, // MIPI_F4_PDT_EDT
+  {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
+  {0x3342, 0x122C}, // MIPI_F1_PDT_EDT
+  {0x3346, 0x122C}, // MIPI_F2_PDT_EDT
+  {0x334A, 0x122C}, // MIPI_F3_PDT_EDT
+  {0x334E, 0x122C}, // MIPI_F4_PDT_EDT
   {0x3344, 0x0011}, // MIPI_F1_VDT_VC
   {0x3348, 0x0111}, // MIPI_F2_VDT_VC
   {0x334C, 0x0211}, // MIPI_F3_VDT_VC

selfdrive/camerad/snapshot/snapshot.py

@@ -17,9 +17,9 @@ from selfdrive.manager.process_config import managed_processes
 LM_THRESH = 120  # defined in selfdrive/camerad/imgproc/utils.h
 
 VISION_STREAMS = {
-  "roadCameraState": VisionStreamType.VISION_STREAM_RGB_BACK,
-  "driverCameraState": VisionStreamType.VISION_STREAM_RGB_FRONT,
-  "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE,
+  "roadCameraState": VisionStreamType.VISION_STREAM_RGB_ROAD,
+  "driverCameraState": VisionStreamType.VISION_STREAM_RGB_DRIVER,
+  "wideRoadCameraState": VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD,
 }
 
 

selfdrive/car/chrysler/carstate.py

@@ -1,7 +1,7 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
 
@@ -58,7 +58,7 @@ class CarState(CarStateBase):
     ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
     steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
-    ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
+    ret.steerFaultPermanent = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
 
     ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]["HIGH_BEAM_FLASH"])
 

selfdrive/car/ford/carstate.py

@@ -1,7 +1,7 @@
 from cereal import car
-from opendbc.can.parser import CANParser
+from common.conversions import Conversions as CV
 from common.numpy_fast import mean
-from selfdrive.config import Conversions as CV
+from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.ford.values import DBC
 
@@ -23,7 +23,7 @@ class CarState(CarStateBase):
     ret.standstill = not ret.vEgoRaw > 0.001
     ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"]
     ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
-    ret.steerError = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
+    ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
     ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS
     ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3))
     ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0

selfdrive/car/ford/interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.ford.values import MAX_ANGLE
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
@@ -25,6 +25,8 @@ class CarInterface(CarInterfaceBase):
     ret.steerRateCost = 1.0
     ret.centerToFront = ret.wheelbase * 0.44
     tire_stiffness_factor = 0.5328
+    # TODO: add minSteerSpeed
+    ret.minEnableSpeed = 12. * CV.MPH_TO_MS
 
     # TODO: get actual value, for now starting with reasonable value for
     # civic and scaling by mass and wheelbase
@@ -64,7 +66,7 @@ class CarInterface(CarInterfaceBase):
   def apply(self, c):
 
     ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.hudControl.visualAlert, c.cruiseControl.cancel)
+                         c.hudControl.visualAlert, c.cruiseControl.cancel)
 
     self.frame += 1
     return ret

selfdrive/car/ford/radar_interface.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.ford.values import DBC
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_MSGS = list(range(0x500, 0x540))

selfdrive/car/gm/carcontroller.py

@@ -1,11 +1,11 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from common.realtime import DT_CTRL
 from common.numpy_fast import interp
-from selfdrive.config import Conversions as CV
+from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.gm import gmcan
 from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams
-from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -27,8 +27,7 @@ class CarController():
     self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar'])
     self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis'])
 
-  def update(self, c, enabled, CS, frame, actuators,
-             hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
+  def update(self, c, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
 
@@ -41,7 +40,7 @@ class CarController():
     if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last:
       self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter
     elif (frame % P.STEER_STEP) == 0:
-      lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED
+      lkas_enabled = c.latActive and CS.out.vEgo > P.MIN_STEER_SPEED
       if lkas_enabled:
         new_steer = int(round(actuators.steer * P.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
@@ -58,7 +57,7 @@ class CarController():
 
     # Gas/regen and brakes - all at 25Hz
     if (frame % 4) == 0:
-      if not c.active:
+      if not c.longActive:
         # Stock ECU sends max regen when not enabled.
         self.apply_gas = P.MAX_ACC_REGEN
         self.apply_brake = 0
@@ -68,15 +67,15 @@ class CarController():
 
       idx = (frame // 4) % 4
 
-      at_full_stop = enabled and CS.out.standstill
-      near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
+      at_full_stop = c.longActive and CS.out.standstill
+      near_stop = c.longActive and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE)
       can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop))
-      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, enabled, at_full_stop))
+      can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, c.longActive, at_full_stop))
 
     # Send dashboard UI commands (ACC status), 25hz
     if (frame % 4) == 0:
       send_fcw = hud_alert == VisualAlert.fcw
-      can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
+      can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, c.longActive, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw))
 
     # Radar needs to know current speed and yaw rate (50hz),
     # and that ADAS is alive (10hz)

selfdrive/car/gm/carstate.py

@@ -45,8 +45,8 @@ class CarState(CarStateBase):
 
     # 0 inactive, 1 active, 2 temporarily limited, 3 failed
     self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
-    ret.steerWarning = self.lkas_status == 2
-    ret.steerError = self.lkas_status == 3
+    ret.steerFaultTemporary = self.lkas_status == 2
+    ret.steerFaultPermanent = self.lkas_status == 3
 
     # 1 - open, 0 - closed
     ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
@@ -59,7 +59,7 @@ class CarState(CarStateBase):
     ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
     ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
 
-    self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
+    ret.parkingBrake = pt_cp.vl["EPBStatus"]["EPBClosed"] == 1
     ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
     ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
     self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]

selfdrive/car/gm/interface.py

@@ -1,10 +1,11 @@
 #!/usr/bin/env python3
 from cereal import car
 from math import fabs
-from selfdrive.config import Conversions as CV
+
+from common.conversions import Conversions as CV
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.gm.values import CAR, CruiseButtons, \
                                     AccState, CarControllerParams
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -46,7 +47,7 @@ class CarInterface(CarInterfaceBase):
 
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
-    # added to selfdrive/test/test_routes, we can remove it from this list.
+    # added to selfdrive/car/tests/routes.py, we can remove it from this list.
     ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL}
 
     # Presence of a camera on the object bus is ok.
@@ -191,8 +192,6 @@ class CarInterface(CarInterfaceBase):
 
     if ret.vEgo < self.CP.minEnableSpeed:
       events.add(EventName.belowEngageSpeed)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
     if ret.cruiseState.standstill:
       events.add(EventName.resumeRequired)
     if self.CS.pcm_acc_status == AccState.FAULTED:
@@ -222,12 +221,7 @@ class CarInterface(CarInterfaceBase):
     if hud_v_cruise > 70:
       hud_v_cruise = 0
 
-    # For Openpilot, "enabled" includes pre-enable.
-    # In GM, PCM faults out if ACC command overlaps user gas.
-    enabled = c.enabled and not self.CS.out.gasPressed
-
-    ret = self.CC.update(c, enabled, self.CS, self.frame,
-                         c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          hud_v_cruise, hud_control.lanesVisible,
                          hud_control.leadVisible, hud_control.visualAlert)
 

selfdrive/car/gm/radar_interface.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 import math
 from cereal import car
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.gm.values import DBC, CAR, CanBus
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import RadarInterfaceBase
 
 RADAR_HEADER_MSG = 1120

selfdrive/car/honda/carcontroller.py

@@ -112,12 +112,12 @@ class CarController():
 
     self.params = CarControllerParams(CP)
 
-  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
 
-    if active:
+    if c.longActive:
       accel = actuators.accel
       gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
     else:
@@ -136,7 +136,7 @@ class CarController():
     else:
       hud_lanes = 0
 
-    if enabled:
+    if c.enabled:
       if hud_show_car:
         hud_car = 2
       else:
@@ -152,8 +152,6 @@ class CarController():
     # steer torque is converted back to CAN reference (positive when steering right)
     apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
 
-    lkas_active = active and not CS.steer_not_allowed
-
     # Send CAN commands.
     can_sends = []
 
@@ -165,7 +163,7 @@ class CarController():
     # Send steering command.
     idx = frame % 4
     can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
-      lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
+      c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
 
     stopping = actuators.longControlState == LongCtrlState.stopping
 
@@ -217,7 +215,7 @@ class CarController():
         if CS.CP.carFingerprint in HONDA_BOSCH:
           self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
           self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
+          can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
         else:
           apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
           apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
@@ -236,7 +234,7 @@ class CarController():
             # This prevents unexpected pedal range rescaling
             # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
             # when you do enable.
-            if active:
+            if c.longActive:
               self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
             else:
               self.gas = 0.0

selfdrive/car/honda/carstate.py

@@ -1,9 +1,10 @@
-from cereal import car
 from collections import defaultdict
+
+from cereal import car
+from common.conversions import Conversions as CV
 from common.numpy_fast import interp
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
-from selfdrive.config import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 
@@ -197,11 +198,10 @@ class CarState(CarStateBase):
     ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
 
     steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
-    ret.steerError = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
-    # NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
-    self.steer_not_allowed = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_2")
+    ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
     # LOW_SPEED_LOCKOUT is not worth a warning
-    ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
+    # NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
+    ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
 
     if self.CP.openpilotLongitudinalControl:
       self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
@@ -230,11 +230,10 @@ class CarState(CarStateBase):
       250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
     ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
 
+    # TODO: set for all cars
     if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
                                   CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
-      self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
-    else:
-      self.park_brake = 0  # TODO
+      ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
 
     gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))

selfdrive/car/honda/hondacan.py

@@ -1,5 +1,5 @@
+from common.conversions import Conversions as CV
 from selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, CAR, CarControllerParams
-from selfdrive.config import Conversions as CV
 
 # CAN bus layout with relay
 # 0 = ACC-CAN - radar side

selfdrive/car/honda/interface.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
+from common.conversions import Conversions as CV
 from common.numpy_fast import interp
 from common.params import Params
 from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 from selfdrive.car.disable_ecu import disable_ecu
-from selfdrive.config import Conversions as CV
 
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -380,8 +380,6 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, pcm_enable=False)
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
 
     if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
       events.add(EventName.belowEngageSpeed)
@@ -427,11 +425,9 @@ class CarInterface(CarInterfaceBase):
     else:
       hud_v_cruise = 255
 
-    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
-                         c.actuators,
-                         c.cruiseControl.cancel,
-                         hud_v_cruise,
-                         hud_control.lanesVisible,
+    ret = self.CC.update(c, self.CS, self.frame,
+                         c.actuators, c.cruiseControl.cancel,
+                         hud_v_cruise, hud_control.lanesVisible,
                          hud_show_car=hud_control.leadVisible,
                          hud_alert=hud_control.visualAlert)
 

selfdrive/car/hyundai/carcontroller.py

@@ -1,7 +1,7 @@
 from cereal import car
 from common.realtime import DT_CTRL
 from common.numpy_fast import clip, interp
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
 from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
@@ -46,23 +46,20 @@ class CarController():
     self.last_resume_frame = 0
     self.accel = 0
 
-  def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
     # Steering Torque
     new_steer = int(round(actuators.steer * self.p.STEER_MAX))
     apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
     self.steer_rate_limited = new_steer != apply_steer
 
-    # disable when temp fault is active, or below LKA minimum speed
-    lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed
-
-    if not lkas_active:
+    if not c.latActive:
       apply_steer = 0
 
     self.apply_steer_last = apply_steer
 
     sys_warning, sys_state, left_lane_warning, right_lane_warning = \
-      process_hud_alert(enabled, self.car_fingerprint, visual_alert,
+      process_hud_alert(c.enabled, self.car_fingerprint, visual_alert,
                         left_lane, right_lane, left_lane_depart, right_lane_depart)
 
     can_sends = []
@@ -72,8 +69,8 @@ class CarController():
       if (frame % 100) == 0:
         can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
 
-    can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
-                                   CS.lkas11, sys_warning, sys_state, enabled,
+    can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive,
+                                   CS.lkas11, sys_warning, sys_state, c.enabled,
                                    left_lane, right_lane,
                                    left_lane_warning, right_lane_warning))
 
@@ -89,7 +86,7 @@ class CarController():
 
     if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
       lead_visible = False
-      accel = actuators.accel if c.active else 0
+      accel = actuators.accel if c.longActive else 0
 
       jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
 
@@ -100,7 +97,7 @@ class CarController():
 
       stopping = (actuators.longControlState == LongCtrlState.stopping)
       set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
-      can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
+      can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
       self.accel = accel
 
     # 20 Hz LFA MFA message
@@ -108,7 +105,7 @@ class CarController():
                                                    CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
                                                    CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
                                                    CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
-      can_sends.append(create_lfahda_mfc(self.packer, enabled))
+      can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
 
     # 5 Hz ACC options
     if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:

selfdrive/car/hyundai/carstate.py

@@ -1,10 +1,10 @@
 import copy
 from cereal import car
-from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
-from selfdrive.car.interfaces import CarStateBase
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
+from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES, EV_CAR, HYBRID_CAR
+from selfdrive.car.interfaces import CarStateBase
 
 
 class CarState(CarStateBase):
@@ -47,7 +47,7 @@ class CarState(CarStateBase):
     ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
     ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
-    ret.steerWarning = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
+    ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
 
     # cruise state
     if self.CP.openpilotLongitudinalControl:
@@ -70,6 +70,7 @@ class CarState(CarStateBase):
     ret.brake = 0
     ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
     ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
+    ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
 
     if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
       if self.CP.carFingerprint in HYBRID_CAR:
@@ -109,7 +110,6 @@ class CarState(CarStateBase):
     # save the entire LKAS11 and CLU11
     self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
     self.clu11 = copy.copy(cp.vl["CLU11"])
-    self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
     self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"]  # 0 NOT ACTIVE, 1 ACTIVE
     self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
     self.prev_cruise_buttons = self.cruise_buttons

selfdrive/car/hyundai/interface.py

@@ -2,7 +2,7 @@
 from cereal import car
 from panda import Panda
 from common.params import Params
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, Buttons, CarControllerParams
 from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@@ -32,7 +32,7 @@ class CarInterface(CarInterfaceBase):
 
     # These cars have been put into dashcam only due to both a lack of users and test coverage.
     # These cars likely still work fine. Once a user confirms each car works and a test route is
-    # added to selfdrive/test/test_routes, we can remove it from this list.
+    # added to selfdrive/car/tests/routes.py, we can remove it from this list.
     ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30}
 
     ret.steerActuatorDelay = 0.1  # Default delay
@@ -302,8 +302,6 @@ class CarInterface(CarInterfaceBase):
 
     if self.CS.brake_error:
       events.add(EventName.brakeUnavailable)
-    if self.CS.park_brake:
-      events.add(EventName.parkBrake)
 
     if self.CS.CP.openpilotLongitudinalControl:
       buttonEvents = []
@@ -352,8 +350,8 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
-                         c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible,
-                         hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert,
+                         hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
+                         hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1
     return ret

selfdrive/car/hyundai/values.py

@@ -11,8 +11,8 @@ class CarControllerParams:
     # To determine the limit for your car, find the maximum value that the stock LKAS will request.
     # If the max stock LKAS request is <384, add your car to this list.
     if CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.HYUNDAI_GENESIS, CAR.ELANTRA_GT_I30, CAR.IONIQ,
-                             CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
-                             CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
+                             CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_HEV,
+                             CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA, CAR.KIA_SORENTO, CAR.KIA_STINGER):
       self.STEER_MAX = 255
     else:
       self.STEER_MAX = 384
@@ -184,21 +184,27 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\000AEhe SCC FHCUP      1.00 1.02 99110-G2100         ',
       b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2200         ',
+      b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2600         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\000AE  MDPS C 1.00 1.01 56310/G2510 4APHC101',
       b'\xf1\x00AE  MDPS C 1.00 1.01 56310/G2560 4APHC101',
+      b'\xf1\x00AE  MDPS C 1.00 1.01 56310G2510\x00 4APHC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\000AEP MFC  AT USA LHD 1.00 1.01 95740-G2600 190819',
       b'\xf1\x00AEP MFC  AT EUR RHD 1.00 1.01 95740-G2600 190819',
+      b'\xf1\x00AEP MFC  AT USA LHD 1.00 1.00 95740-G2700 201027',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2',
       b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00',
+      b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt',
+      b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00',
     ],
   },
   CAR.IONIQ_EV_2020: {
@@ -460,6 +466,7 @@ FW_VERSIONS = {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
       b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
+      b'\xf1\x8799110S1500\xf1\x00TM__ SCC FHCUP      1.00 1.00 99110-S1500         ',
     ],
     (Ecu.esp, 0x7d1, None): [
       b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
@@ -467,11 +474,14 @@ FW_VERSIONS = {
       b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
       b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
       b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0',
+      b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x82TACVN5GMI3XXXH0A',
       b'\xf1\x82TMBZN5TMD3XXXG2E',
       b'\xf1\x82TACVN5GSI3XXXH0A',
       b'\xf1\x82TMCFD5MMCXXXXG0A',
+      b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00TM  MDPS C 1.00 1.02 56370-S2AA0 0B19',
@@ -481,12 +491,15 @@ FW_VERSIONS = {
       b'\xf1\x00TMA MFC  AT MEX LHD 1.00 1.01 99211-S2500 210205',
       b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.00 99211-S2500 200720',
       b'\xf1\x00TM  MFC  AT EUR LHD 1.00 1.03 99211-S1500 210224',
+      b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.01 99211-S2500 210205',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
       b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
       b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6',
     ],
   },
   CAR.SANTA_FE_HEV_2022: {
@@ -855,6 +868,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00DEH MFC  AT USA LHD 1.00 1.07 99211-G5000 201221',
+      b'\xf1\x00DEH MFC  AT USA LHD 1.00 1.00 99211-G5500 210428',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\x00DEhe SCC FHCUP      1.00 1.00 99110-G5600         ',
@@ -905,6 +919,7 @@ FW_VERSIONS = {
       b'\xf1\x00CN7_ SCC F-CUP      1.00 1.01 99110-AA000         ',
       b'\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         ',
       b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         ',
+      b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP      1.00 1.01 99110-AA000         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106',
@@ -927,7 +942,8 @@ FW_VERSIONS = {
       b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\177\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87CXMQFM1916035JB2\x88vvgg\x87Wuwgev\xa9\x98\x88\x98h\x99\x9f\xffh\xff\xff\xff\xa5\xee\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87CXLQF40189012JL2f\x88\x86\x88\x88vUex\xb8\x88\x88\x88\x87\x88\x89fh?\xffz\xff\xff\xff\x08z\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
-      b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00'
+      b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87CXMQFM3806705JB2\x89\x87wwx\x88g\x86\x99\x87\x86xwwv\x88yv\x7f\xffz\xff\xff\xffV\x15\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x82CNCWD0AMFCXCSFFA',

selfdrive/car/interfaces.py

@@ -7,7 +7,7 @@ from cereal import car
 from common.kalman.simple_kalman import KF1D
 from common.realtime import DT_CTRL
 from selfdrive.car import gen_empty_fingerprint
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from selfdrive.controls.lib.events import Events
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -90,7 +90,7 @@ class CarInterfaceBase(ABC):
     ret.stopAccel = -2.0
     ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
     ret.vEgoStopping = 0.5
-    ret.vEgoStarting = 0.5  # needs to be >= vEgoStopping to avoid state transition oscillation
+    ret.vEgoStarting = 0.5
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]
@@ -141,11 +141,13 @@ class CarInterfaceBase(ABC):
       events.add(EventName.wrongCruiseMode)
     if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
       events.add(EventName.brakeHold)
+    if cs_out.parkingBrake:
+      events.add(EventName.parkBrake)
 
 
     # Handle permanent and temporary steering faults
     self.steering_unpressed = 0 if cs_out.steeringPressed else self.steering_unpressed + 1
-    if cs_out.steerWarning:
+    if cs_out.steerFaultTemporary:
       # if the user overrode recently, show a less harsh alert
       if self.silent_steer_warning or cs_out.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
         self.silent_steer_warning = True
@@ -154,7 +156,7 @@ class CarInterfaceBase(ABC):
         events.add(EventName.steerTempUnavailable)
     else:
       self.silent_steer_warning = False
-    if cs_out.steerError:
+    if cs_out.steerFaultPermanent:
       events.add(EventName.steerUnavailable)
 
     # Disable on rising edge of gas or brake. Also disable on brake when speed > 0.

selfdrive/car/mazda/carcontroller.py

@@ -19,7 +19,7 @@ class CarController():
     apply_steer = 0
     self.steer_rate_limited = False
 
-    if c.active:
+    if c.latActive:
       # calculate steer and also set limits due to driver torque
       new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
@@ -32,7 +32,7 @@ class CarController():
         # TODO: improve the resume trigger logic by looking at actual radar data
         can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
 
-    if c.cruiseControl.cancel or (CS.out.cruiseState.enabled and not c.enabled):
+    if c.cruiseControl.cancel:
       # If brake is pressed, let us wait >70ms before trying to disable crz to avoid
       # a race condition with the stock system, where the second cancel from openpilot
       # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to

selfdrive/car/mazda/carstate.py

@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
@@ -88,7 +88,7 @@ class CarState(CarStateBase):
     # Check if LKAS is disabled due to lack of driver torque when all other states indicate
     # it should be enabled (steer lockout). Don't warn until we actually get lkas active
     # and lose it again, i.e, after initial lkas activation
-    ret.steerWarning = self.lkas_allowed_speed and lkas_blocked
+    ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
 
     self.acc_active_last = ret.cruiseState.enabled
 
@@ -98,7 +98,7 @@ class CarState(CarStateBase):
     self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
     self.cam_lkas = cp_cam.vl["CAM_LKAS"]
     self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
-    ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
+    ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
 
     return ret
 

selfdrive/car/mazda/interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase

selfdrive/car/mock/interface.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 import math
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.car import gen_empty_fingerprint, get_safety_config

selfdrive/car/nissan/carcontroller.py

@@ -18,20 +18,19 @@ class CarController():
 
     self.packer = CANPacker(dbc_name)
 
-  def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert,
+  def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert,
              left_line, right_line, left_lane_depart, right_lane_depart):
 
     can_sends = []
 
     ### STEER ###
-    acc_active = CS.out.cruiseState.enabled
     lkas_hud_msg = CS.lkas_hud_msg
     lkas_hud_info_msg = CS.lkas_hud_info_msg
     apply_angle = actuators.steeringAngleDeg
 
     steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
 
-    if c.active:
+    if c.latActive:
       # # windup slower
       if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
         angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
@@ -58,10 +57,6 @@ class CarController():
 
     self.last_angle = apply_angle
 
-    if not enabled and acc_active:
-      # send acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
-      cruise_cancel = 1
-
     if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel:
         can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
 
@@ -73,12 +68,12 @@ class CarController():
         can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
 
     can_sends.append(nissancan.create_steering_control(
-        self.packer, apply_angle, frame, enabled, self.lkas_max_torque))
+        self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))
 
     if lkas_hud_msg and lkas_hud_info_msg:
       if frame % 2 == 0:
         can_sends.append(nissancan.create_lkas_hud_msg(
-          self.packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart))
+          self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart))
 
       if frame % 50 == 0:
         can_sends.append(nissancan.create_lkas_hud_info_msg(

selfdrive/car/nissan/carstate.py

@@ -3,7 +3,7 @@ from collections import deque
 from cereal import car
 from opendbc.can.can_define import CANDefine
 from selfdrive.car.interfaces import CarStateBase
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from opendbc.can.parser import CANParser
 from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
 

selfdrive/car/nissan/interface.py

@@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          c.cruiseControl.cancel, hud_control.visualAlert,
                          hud_control.leftLaneVisible, hud_control.rightLaneVisible,
                          hud_control.leftLaneDepart, hud_control.rightLaneDepart)

selfdrive/car/subaru/carcontroller.py

@@ -15,7 +15,7 @@ class CarController():
     self.p = CarControllerParams(CP)
     self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
 
-  def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
 
     can_sends = []
 
@@ -30,7 +30,7 @@ class CarController():
       apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
       self.steer_rate_limited = new_steer != apply_steer
 
-      if not c.active:
+      if not c.latActive:
         apply_steer = 0
 
       if CS.CP.carFingerprint in PREGLOBAL_CARS:
@@ -69,7 +69,7 @@ class CarController():
         self.es_distance_cnt = CS.es_distance_msg["Counter"]
 
       if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
-        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
+        can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
         self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
 
     new_actuators = actuators.copy()

selfdrive/car/subaru/carstate.py

@@ -1,7 +1,7 @@
 import copy
 from cereal import car
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CARS
@@ -62,13 +62,13 @@ class CarState(CarStateBase):
                         cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
                         cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
                         cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
-    ret.steerError = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
+    ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
 
     if self.car_fingerprint in PREGLOBAL_CARS:
       self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"]
       self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
     else:
-      ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
+      ret.steerFaultTemporary = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
       ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
       self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
     self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])

selfdrive/car/subaru/interface.py

@@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators,
                          c.cruiseControl.cancel, hud_control.visualAlert,
                          hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
     self.frame += 1

selfdrive/car/subaru/values.py

@@ -74,12 +74,14 @@ FW_VERSIONS = {
       b'\x7a\x94\x3f\x90\x00',
       b'\xa2 \x185\x00',
       b'\xa2 \x193\x00',
+      b'\xa2 \x194\x00',
       b'z\x94.\x90\x00',
       b'z\x94\b\x90\x01',
       b'\xa2 \x19`\x00',
       b'z\x94\f\x90\001',
       b'z\x9c\x19\x80\x01',
       b'z\x94\x08\x90\x00',
+      b'z\x84\x19\x90\x00',
     ],
     (Ecu.eps, 0x746, None): [
       b'\x7a\xc0\x0c\x00',

selfdrive/car/tesla/carcontroller.py

@@ -12,12 +12,12 @@ class CarController():
     self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
     self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
 
-  def update(self, c, enabled, CS, frame, actuators, cruise_cancel):
+  def update(self, c, CS, frame, actuators, cruise_cancel):
     can_sends = []
 
     # Temp disable steering on a hands_on_fault, and allow for user override
     hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3)
-    lkas_enabled = c.active and (not hands_on_fault)
+    lkas_enabled = c.latActive and (not hands_on_fault)
 
     if lkas_enabled:
       apply_angle = actuators.steeringAngleDeg
@@ -50,10 +50,6 @@ class CarController():
     if hands_on_fault:
       cruise_cancel = True
 
-    # Cancel when openpilot is not enabled anymore
-    if not enabled and bool(CS.out.cruiseState.enabled):
-      cruise_cancel = True
-
     if ((frame % 10) == 0 and cruise_cancel):
       # Spam every possible counter value, otherwise it might not be accepted
       for counter in range(16):

selfdrive/car/tesla/carstate.py

@@ -1,10 +1,10 @@
 import copy
 from cereal import car
+from common.conversions import Conversions as CV
 from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.config import Conversions as CV
 
 class CarState(CarStateBase):
   def __init__(self, CP):
@@ -43,8 +43,8 @@ class CarState(CarStateBase):
     ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
     ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
     ret.steeringPressed = (self.hands_on_level > 0)
-    ret.steerError = steer_status == "EAC_FAULT"
-    ret.steerWarning = self.steer_warning != "EAC_ERROR_IDLE"
+    ret.steerFaultPermanent = steer_status == "EAC_FAULT"
+    ret.steerFaultTemporary = self.steer_warning != "EAC_ERROR_IDLE"
 
     # Cruise state
     cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)

selfdrive/car/tesla/interface.py

@@ -71,6 +71,6 @@ class CarInterface(CarInterfaceBase):
     return self.CS.out
 
   def apply(self, c):
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
+    ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel)
     self.frame += 1
     return ret

selfdrive/car/tesla/teslacan.py

@@ -1,6 +1,6 @@
 import copy
 import crcmod
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.tesla.values import CANBUS, CarControllerParams
 
 

selfdrive/car/tests/routes.py

@@ -69,7 +69,7 @@ routes = [
   TestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS),
   TestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70),
   TestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80),
-  TestRoute("38bfd238edecbcd7|2018-08-29--22-02-15", HYUNDAI.SANTA_FE),
+  TestRoute("4dbd55df87507948|2022-03-01--09-45-38", HYUNDAI.SANTA_FE),
   TestRoute("bf43d9df2b660eb0|2021-09-23--14-16-37", HYUNDAI.SANTA_FE_2022),
   TestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.SANTA_FE_HEV_2022),
   TestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.SANTA_FE_PHEV_2022),

selfdrive/car/tests/test_car_interfaces.py

@@ -12,7 +12,6 @@ class TestCarInterfaces(unittest.TestCase):
 
   @parameterized.expand([(car,) for car in all_known_cars()])
   def test_car_interfaces(self, car_name):
-    print(car_name)
     if car_name in FINGERPRINTS:
       fingerprint = FINGERPRINTS[car_name][0]
     else:

selfdrive/car/tests/test_models.py

@@ -17,7 +17,7 @@ from selfdrive.car.gm.values import CAR as GM
 from selfdrive.car.honda.values import CAR as HONDA, HONDA_BOSCH
 from selfdrive.car.hyundai.values import CAR as HYUNDAI
 from selfdrive.car.toyota.values import CAR as TOYOTA
-from selfdrive.test.test_routes import routes, non_tested_cars
+from selfdrive.car.tests.routes import routes, non_tested_cars
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader
 
@@ -29,11 +29,6 @@ PandaType = log.PandaState.PandaType
 NUM_JOBS = int(os.environ.get("NUM_JOBS", "1"))
 JOB_ID = int(os.environ.get("JOB_ID", "0"))
 
-# TODO: get updated routes for these cars
-ignore_can_valid = [
-  HYUNDAI.SANTA_FE,
-]
-
 ignore_addr_checks_valid = [
   GM.BUICK_REGAL,
   HYUNDAI.GENESIS_G70_2020,
@@ -135,8 +130,7 @@ class TestCarModel(unittest.TestCase):
       if i > 200:
         can_invalid_cnt += not CS.canValid
 
-    if self.car_model not in ignore_can_valid:
-      self.assertLess(can_invalid_cnt, 50)
+    self.assertLess(can_invalid_cnt, 50)
 
   def test_radar_interface(self):
     os.environ['NO_RADAR_SLEEP'] = "1"

selfdrive/car/toyota/carcontroller.py

@@ -22,11 +22,11 @@ class CarController():
     self.gas = 0
     self.accel = 0
 
-  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
+  def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
     # gas and brake
-    if CS.CP.enableGasInterceptor and active:
+    if CS.CP.enableGasInterceptor and c.longActive:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -49,7 +49,7 @@ class CarController():
     self.steer_rate_limited = new_steer != apply_steer
 
     # Cut steering while we're in a known fault state (2s)
-    if not active or CS.steer_state in (9, 25):
+    if not c.latActive or CS.steer_state in (9, 25):
       apply_steer = 0
       apply_steer_req = 0
     else:
@@ -57,7 +57,7 @@ class CarController():
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
-    if not enabled and CS.pcm_acc_status:
+    if not c.enabled and CS.pcm_acc_status:
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
@@ -122,7 +122,7 @@ class CarController():
       send_ui = True
 
     if (frame % 100 == 0 or send_ui):
-      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, enabled))
+      can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled))
 
     if frame % 100 == 0 and CS.CP.enableDsu:
       can_sends.append(create_fcw_command(self.packer, fcw_alert))

selfdrive/car/toyota/carstate.py

@@ -1,11 +1,11 @@
 from cereal import car
+from common.conversions import Conversions as CV
 from common.numpy_fast import mean
 from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
-from selfdrive.config import Conversions as CV
+from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE
 
 
@@ -19,7 +19,6 @@ class CarState(CarStateBase):
     # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
     # the signal is zeroed to where the steering angle is at start.
     # Need to apply an offset as soon as the steering angle measurements are both received
-    self.needs_angle_offset = True
     self.accurate_steer_angle_seen = False
     self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
 
@@ -32,6 +31,7 @@ class CarState(CarStateBase):
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
     ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
+    ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
 
     ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
     ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
@@ -58,8 +58,8 @@ class CarState(CarStateBase):
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
-    # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-    if abs(torque_sensor_angle_deg) > 1e-3:
+    # On some cars, the angle measurement is non-zero while initializing
+    if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
       self.accurate_steer_angle_seen = True
 
     if self.accurate_steer_angle_seen:
@@ -82,7 +82,7 @@ class CarState(CarStateBase):
     ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
     # we could use the override bit from dbc, but it's triggered at too high torque values
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
-    ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5)
+    ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5)
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -141,6 +141,7 @@ class CarState(CarStateBase):
       ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"),
       ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"),
       ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"),
+      ("PARKING_BRAKE", "BODY_CONTROL_STATE"),
       ("TC_DISABLED", "ESP_CONTROL"),
       ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"),
       ("STEER_FRACTION", "STEER_ANGLE_SENSOR"),
@@ -151,6 +152,7 @@ class CarState(CarStateBase):
       ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"),
       ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"),
       ("STEER_ANGLE", "STEER_TORQUE_SENSOR"),
+      ("STEER_ANGLE_INITIALIZING", "STEER_TORQUE_SENSOR"),
       ("TURN_SIGNALS", "BLINKERS_STATE"),
       ("LKA_STATE", "EPS_STATUS"),
       ("AUTO_HIGH_BEAM", "LIGHT_STALK"),

selfdrive/car/toyota/interface.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune
 from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, EV_HYBRID_CAR, CarControllerParams
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
@@ -103,6 +103,9 @@ class CarInterface(CarInterfaceBase):
       set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
 
     elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2):
+      # starting from 2019, all Avalon variants have stop and go
+      # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
+      stop_and_go = candidate != CAR.AVALON
       ret.wheelbase = 2.82
       ret.steerRatio = 14.8  # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
       tire_stiffness_factor = 0.7983
@@ -270,7 +273,7 @@ class CarInterface(CarInterfaceBase):
   # to be called @ 100hz
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c.enabled, c.active, self.CS, self.frame,
+    ret = self.CC.update(c, self.CS, self.frame,
                          c.actuators, c.cruiseControl.cancel,
                          hud_control.visualAlert, hud_control.leftLaneVisible,
                          hud_control.rightLaneVisible, hud_control.leadVisible,

selfdrive/car/toyota/values.py

@@ -2,8 +2,8 @@ from collections import defaultdict
 from enum import IntFlag
 
 from cereal import car
+from common.conversions import Conversions as CV
 from selfdrive.car import dbc_dict
-from selfdrive.config import Conversions as CV
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
@@ -30,7 +30,7 @@ class CAR:
   AVALON = "TOYOTA AVALON 2016"
   AVALON_2019 = "TOYOTA AVALON 2019"
   AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
-  AVALON_TSS2 = "TOYOTA AVALON 2022"
+  AVALON_TSS2 = "TOYOTA AVALON 2022"  # TSS 2.5
   CAMRY = "TOYOTA CAMRY 2018"
   CAMRYH = "TOYOTA CAMRY HYBRID 2018"
   CAMRY_TSS2 = "TOYOTA CAMRY 2021"  # TSS 2.5
@@ -1388,6 +1388,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x700, None): [
       b'\x018966378B2100\x00\x00\x00\x00',
       b'\x018966378G3000\x00\x00\x00\x00',
+      b'\x018966378B3000\x00\x00\x00\x00'
     ],
     (Ecu.esp, 0x7b0, None): [
       b'\x01F152678221\x00\x00\x00\x00\x00\x00',
@@ -1397,6 +1398,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b"\x018821F3301400\x00\x00\x00\x00",
+      b'\x018821F3301200\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',

selfdrive/car/volkswagen/carcontroller.py

@@ -21,7 +21,7 @@ class CarController():
 
     self.steer_rate_limited = False
 
-  def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
+  def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
     """ Controls thread """
 
     can_sends = []
@@ -39,7 +39,7 @@ class CarController():
       # torque value. Do that anytime we happen to have 0 torque, or failing that,
       # when exceeding ~1/3 the 360 second timer.
 
-      if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
+      if c.latActive:
         new_steer = int(round(actuators.steer * P.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
         self.steer_rate_limited = new_steer != apply_steer
@@ -77,7 +77,7 @@ class CarController():
       else:
         hud_alert = MQB_LDW_MESSAGES["none"]
 
-      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
+      can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled,
                                                             CS.out.steeringPressed, hud_alert, left_lane_visible,
                                                             right_lane_visible, CS.ldw_stock_values,
                                                             left_lane_depart, right_lane_depart))
@@ -88,11 +88,11 @@ class CarController():
 
     if CS.CP.pcmCruise:
       if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
-        if not enabled and CS.out.cruiseState.enabled:
+        if c.cruiseControl.cancel:
           # Cancel ACC if it's engaged with OP disengaged.
           self.graButtonStatesToSend = BUTTON_STATES.copy()
           self.graButtonStatesToSend["cancel"] = True
-        elif enabled and CS.esp_hold_confirmation:
+        elif c.enabled and CS.esp_hold_confirmation:
           # Blip the Resume button if we're engaged at standstill.
           # FIXME: This is a naive implementation, improve with visiond or radar input.
           self.graButtonStatesToSend = BUTTON_STATES.copy()

selfdrive/car/volkswagen/carstate.py

@@ -1,6 +1,6 @@
 import numpy as np
 from cereal import car
-from selfdrive.config import Conversions as CV
+from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -41,14 +41,15 @@ class CarState(CarStateBase):
 
     # Verify EPS readiness to accept steering commands
     hca_status = self.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"])
-    ret.steerError = hca_status in ("DISABLED", "FAULT")
-    ret.steerWarning = hca_status in ("INITIALIZING", "REJECTED")
+    ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT")
+    ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED")
 
     # Update gas, brakes, and gearshift.
     ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0
     ret.gasPressed = ret.gas > 0
     ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0  # FIXME: this is pressure in Bar, not sure what OP expects
     ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
+    ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])  # FIXME: need to include an EPB check as well
     self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]
 
     # Update gear and/or clutch position data.
@@ -140,7 +141,6 @@ class CarState(CarStateBase):
     self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"]
 
     # Additional safety checks performed in CarInterface.
-    self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])  # FIXME: need to include an EPB check as well
     ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
 
     return ret

selfdrive/car/volkswagen/interface.py

@@ -186,8 +186,6 @@ class CarInterface(CarInterfaceBase):
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
 
     # Vehicle health and operation safety checks
-    if self.CS.parkingBrakeSet:
-      events.add(EventName.parkBrake)
     if self.CS.tsk_status in (6, 7):
       events.add(EventName.accFaulted)
 
@@ -211,7 +209,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     hud_control = c.hudControl
-    ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators,
+    ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators,
                          hud_control.visualAlert,
                          hud_control.leftLaneVisible,
                          hud_control.rightLaneVisible,

selfdrive/car/volkswagen/values.py

@@ -301,6 +301,7 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906024AK\xf1\x899937',
       b'\xf1\x8704E906024AS\xf1\x899912',
+      b'\xf1\x8704E906024BC\xf1\x899971',
       b'\xf1\x8704E906024B \xf1\x895594',
       b'\xf1\x8704E906024C \xf1\x899970',
       b'\xf1\x8704E906024L \xf1\x895595',
@@ -311,6 +312,7 @@ FW_VERSIONS = {
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158BQ\xf1\x893545',
       b'\xf1\x8709S927158BS\xf1\x893642',
+      b'\xf1\x8709S927158BS\xf1\x893694',
       b'\xf1\x8709S927158R \xf1\x893552',
       b'\xf1\x8709S927158R \xf1\x893587',
       b'\xf1\x870GC300020N \xf1\x892803',
@@ -426,9 +428,11 @@ FW_VERSIONS = {
       b'\xf1\x8704L906027G \xf1\x899893',
       b'\xf1\x875N0906259  \xf1\x890002',
       b'\xf1\x8783A907115B \xf1\x890005',
+      b'\xf1\x8783A907115G \xf1\x890001',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x8709G927158DT\xf1\x893698',
+      b'\xf1\x8709G927158GD\xf1\x893820',
       b'\xf1\x870DL300011N \xf1\x892001',
       b'\xf1\x870DL300011N \xf1\x892012',
       b'\xf1\x870DL300013A \xf1\x893005',
@@ -439,6 +443,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655BM\xf1\x890403\xf1\x82\02316143231313500314641011750179333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02312110031333300314240583752379333423100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\02331310031333336313140013950399333423100',
+      b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1316143231313500314647021750179333613100',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820529A6060603',
@@ -450,6 +455,7 @@ FW_VERSIONS = {
       b'\xf1\x872Q0907572J \xf1\x890156',
       b'\xf1\x872Q0907572Q \xf1\x890342',
       b'\xf1\x872Q0907572R \xf1\x890372',
+      b'\xf1\x872Q0907572T \xf1\x890383',
     ],
   },
   CAR.TOURAN_MK2: {
@@ -516,6 +522,7 @@ FW_VERSIONS = {
       b'\xf1\x875G0906259L \xf1\x890002',
       b'\xf1\x875G0906259Q \xf1\x890002',
       b'\xf1\x878V0906259F \xf1\x890002',
+      b'\xf1\x878V0906259K \xf1\x890001',
       b'\xf1\x878V0906264B \xf1\x890003',
       b'\xf1\x878V0907115B \xf1\x890007',
     ],
@@ -525,6 +532,7 @@ FW_VERSIONS = {
       b'\xf1\x870D9300012  \xf1\x894912',
       b'\xf1\x870D9300013B \xf1\x894931',
       b'\xf1\x870D9300041N \xf1\x894512',
+      b'\xf1\x870D9300043T \xf1\x899699',
       b'\xf1\x870DD300046A \xf1\x891602',
       b'\xf1\x870DD300046F \xf1\x891602',
       b'\xf1\x870DD300046G \xf1\x891601',
@@ -542,6 +550,7 @@ FW_VERSIONS = {
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1',
+      b'\xf1\x873Q0909144K \xf1\x895072\xf1\x82\x0571G0HA16A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0',
@@ -705,11 +714,13 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906016ER\xf1\x895823',
       b'\xf1\x8704E906027HD\xf1\x893742',
+      b'\xf1\x8704E906027MH\xf1\x894786',
       b'\xf1\x8704L906021DT\xf1\x898127',
       b'\xf1\x8704L906026BS\xf1\x891541',
       b'\xf1\x875G0906259C \xf1\x890002',
     ],
     (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870CW300041L \xf1\x891601',
       b'\xf1\x870CW300041N \xf1\x891605',
       b'\xf1\x870CW300043B \xf1\x891601',
       b'\xf1\x870D9300041C \xf1\x894936',
@@ -722,11 +733,13 @@ FW_VERSIONS = {
       b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200',
       b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100',
       b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100',
+      b'\xf1\x875QD959655  \xf1\x890388\xf1\x82\x111101000011110006110411111111119111',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A01513A1',
       b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521T00403A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1',
+      b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1',
       b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
@@ -734,6 +747,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
       b'\xf1\x875Q0907572J \xf1\x890654',
       b'\xf1\x875Q0907572P \xf1\x890682',
+      b'\xf1\x875Q0907572R \xf1\x890771',
     ],
   },
   CAR.SKODA_SCALA_MK1: {

selfdrive/common/params.cc

@@ -91,7 +91,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CarParamsCache", CLEAR_ON_MANAGER_START},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
-    {"CellularUnmetered", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},