Merge remote-tracking branch 'upstream/master' into controlsd-pedal-logic

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"]
+cflags += ['-DSWAGLOG="\\"selfdrive/common/swaglog.h\\""']
-cxxflags += ["-DSWAGLOG"]
+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 279311d0bde60e814a697da22c55c6abe8a3b03c

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
+oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
-size 56684955
+size 72718099

models/supercombo.onnx

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

opendbc

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

panda

@@ -1 +1 @@
-Subproject commit cb88a1756ba2b8b06e06102691c75f8aa87f687b
+Subproject commit 234e436e93f4dbd084fd912fe9b10d504ca9882e

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]
+        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
+        # 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)
-        # avg_time_steps = np.average(time_steps)
+        # criterion for constant time step detection: the min/max difference in values normalized by the average
-        # # 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
-        # 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 the criterion is small, we have a constant time-step
+        if check_const_time_step < 1e-9:
-        # 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[:] = 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
 
         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 = format_class_dict(acados_ocp.dims.__dict__)
-    dims_dict = acados_class2dict(acados_ocp.dims)
 
     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
+    @classmethod
-          cwd=os.getcwd()
+    def build(cls, code_export_dir, with_cython=False):
-          os.chdir(code_export_dir)
+        """
-          os.system('make clean_ocp_shared_lib')
+        Builds the code for an acados OCP solver, that has been generated in code_export_dir
-          os.system('make ocp_shared_lib')
+            :param code_export_dir: directory in which acados OCP solver has been generated, see generate()
-          os.chdir(cwd)
+            :param with_cython: option indicating if the cython interface is build, default: False.
-
+        """
-    def __init__(self, model_name, N, code_export_dir):
+        cwd=os.getcwd()
-        self.model_name = model_name
+        os.chdir(code_export_dir)
-        self.N = N
+        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)
+        self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
-        return status
+
+        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':
+        if self.solver_options['nlp_solver_type'] == 'SQP':
-            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
+            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
-            if stat.shape[0]>7:
+            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( \
+                print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
-                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
+                      f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
-                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
+                if stat.shape[0]>8:
-                if stat.shape[0]>7:
                     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
+        double_fields = ['time_tot',
-                  'time_lin',  # cpu time for linearization
+                  'time_lin',
-                  'time_sim',  # cpu time for integrator
+                  'time_sim',
-                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
+                  'time_sim_ad',
-                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
+                  'time_sim_la',
-                  'time_qp',   # cpu time qp solution
+                  'time_qp',
-                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+                  'time_qp_solver_call',
                   'time_qp_xcond',
-                  'time_glob',  # cpu time globalization
+                  'time_glob',
-                  'time_reg',  # cpu time regularization
+                  'time_solution_sensitivities',
-                  'sqp_iter',  # number of SQP iterations
+                  'time_reg'
-                  'qp_iter',  # vector of QP iterations for last SQP call
+        ]
-                  'statistics',  # table with info about last iteration
+        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':
+            if self.solver_options['nlp_solver_type'] == 'SQP':
-                out = full_stats[6, :]
+                return full_stats[6, :]
-            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+            elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
-                out = full_stats[2, :]
+                return full_stats[2, :]
 
-        else:
+        elif field_ == 'alpha':
-            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+            full_stats = self.get_stats('statistics')
-            out_data = cast(out.ctypes.data, POINTER(c_double))
+            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':
+        elif field_ == 'residuals':
-            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            return self.get_residuals()
-            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
 
-        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:
+            elif field_ in mem_fields:
-            #     self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                self.shared_lib.ocp_nlp_set.argtypes = \
-            #         self.nlp_solver, stage, field, value_data_p)
+                    [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', \
+            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
-                ' for field "{}" with dimension {} (you have {})'.format( \
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
-                field_, tuple(dims), 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' \
+            raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+                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']
+        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']
+        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:
+            if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                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):
+    cdef str nlp_solver_type
-        self.model_name = model_name
+
-        self.N = N
+    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.data, value.shape[0]) == 0
-            assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], 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', \
+            raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
-                ' for field "{}" with dimension {} (you have {})'.format( \
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
-                field_, tuple(dims), 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' \
+            raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+                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']
+        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']
+        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:
+                if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    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 #}
+# define sources and use make's implicit rules to generate object files (*.o)
-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
 
-MODEL_OBJ=
+# model
+MODEL_SRC=
 {%- if  solver_options.integrator_type == "ERK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
-{%- endif %}
+	{%- endif %}
 {%- elif solver_options.integrator_type == "IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
-{%- endif %}
+	{%- endif %}
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
-{%- endif %}
+	{%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
+	{% if model.gnsf.purely_linear != 1 %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
+		{% 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" %}
+	{%- if model.dyn_ext_fun_type == "casadi" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
-{%- if hessian_approx == "EXACT" %}
+		{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
-{%- endif %}
+		{%- endif %}
-{%- else %}
+	{%- else %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
-{%- endif %}
+	{%- endif %}
 {%- endif %}
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
-
+# optimal control problem - mostly CasADi exports
-OCP_OBJ=
+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_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
-{%- endif %}
+	{%- 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_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
-{%- if hessian_approx == "EXACT" %}
+	{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
-{%- endif %}
+	{%- endif %}
 {%- endif %}
 
 {%- if cost_type_0 == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
+OCP_SRC+= {{ 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_SRC+= {{ 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_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_0 == "casadi" %}
+	{%- if cost.cost_ext_fun_type_0 == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
+OCP_SRC+= {{ 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_SRC+= {{ 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
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
-{% else %}
+	{%- else %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
-{% endif %}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= {{ 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_hess.c
 {%- elif cost_type == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type == "casadi" %}
+	{%- if cost.cost_ext_fun_type == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
+OCP_SRC+= {{ 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
+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 %}
+	{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
-{% endif %}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type_e == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
+OCP_SRC+= {{ 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_SRC+= {{ 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_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_e == "casadi" %}
+	{%- if cost.cost_ext_fun_type_e == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
+OCP_SRC+= {{ 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_SRC+= {{ 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
+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 %}
+	{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
-{% endif %}
+	{%- 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=
+# for target example
-SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
+EX_SRC= main_{{ model.name }}.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
 
-EX_OBJ=
+# for target example_sim
-EX_OBJ+= main_{{ model.name }}.o
+EX_SIM_SRC= main_sim_{{ model.name }}.c
-
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
-EX_SIM_OBJ=
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
-EX_SIM_OBJ+= main_sim_{{ model.name }}.o
 
+# 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" %}
+# preprocessor flags for make's implicit rules
-all: clean casadi_fun example
+{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-shared_lib: ocp_shared_lib
+CPPFLAGS += -DACADOS_WITH_QPOASES
-{%- 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
 {%- endif %}
-{%- if cost_type_0 == "NONLINEAR_LS" %}
+{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
-CASADI_COST_Y_0_SOURCE=
+CPPFLAGS += -DACADOS_WITH_OSQP
-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
 {%- 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:
+# linker flags
-	{%- if model.dyn_ext_fun_type == "casadi" %}
+LDFLAGS+= -L$(LIB_PATH)
-	( 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 %}
 
-main_sim:
+# link to libraries
-	gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+LDLIBS+= -lacados
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= {{ link_libs }}
 
-ocp_solver:
+# libraries
-	gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
 
-sim_solver:
+# virtual targets
-	gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+.PHONY : all clean
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
 
-example: ocp_solver main
+#all: clean example_sim example shared_lib
-	gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
+{% if solver_options.integrator_type == "DISCRETE" -%}
-	-lacados -lhpipm -lblasfeo \
+all: clean example
-	{{ link_libs }} \
+shared_lib: ocp_shared_lib
-	-lm \
+{%- else %}
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
+all: clean example_sim example
-	-I $(INCLUDE_PATH)/hpipm/include/ \
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
-	-I $(INCLUDE_PATH) \
+{%- endif %}
-	-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/ \
 
-{%- 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
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
-	gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
 
-ocp_shared_lib: casadi_fun ocp_solver
+{% if solver_options.integrator_type != "DISCRETE" -%}
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
+bundled_shared_lib: $(OBJ)
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
-	-I $(INCLUDE_PATH)/hpipm/include/ \
+{%- endif %}
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
 
-{%- 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
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
-	-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 \
 
-{%- 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 \
+	$(LDFLAGS) $(LDLIBS)
-	{{ 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 \
 
 {%- 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) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
-	rm -f *.so
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
-	rm -f main_{{ model.name }}
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
 
 clean_ocp_shared_lib:
-	rm -f libacados_ocp_solver_{{ model.name }}.so
+	$(RM) $(LIBACADOS_OCP_SOLVER)
-	rm -f acados_solver_{{ model.name }}.o
+	$(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');
+                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
-                if size(stat,2)>7
+                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));
+                    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)>7
+                    if size(stat,2)>8
-                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
+                        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
+	{% if model.gnsf.purely_linear != 1 %}
-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);
-
 // 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

@@ -58,7 +58,6 @@ common/transformations/transformations.pyx
 common/api/__init__.py
 
 models/supercombo.dlc
-models/big_supercombo.dlc
 models/dmonitoring_model_q.dlc
 
 release/*
@@ -337,6 +336,7 @@ 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
@@ -424,6 +424,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]
+      metered = sm['deviceState'].networkMetered
-      if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+      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]
+          metered = sm['deviceState'].networkMetered
-          if cell and (not cur_upload_items[tid].allow_cellular) and (not cellular_unmetered):
+          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_qcom2.cc

@@ -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) {
@@ -475,133 +482,133 @@ 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(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[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]) {
+  if (buf_handle[i]) {
-    release(s->multi_cam_state->video0_fd, s->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
+    buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
-    if (dp) s->buf.queue(i);
+    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];
+    sync_destroy.sync_obj = sync_objs[i];
-    ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
+    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.session_hdl = session_handle;
-  req_mgr_sched_request.link_hdl = s->link_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;
+  mem_mgr_map_cmd.fd = 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));
+  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", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
+  // LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
-  s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
+  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;
+     request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
-     enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
+     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];
+  ci = cameras_supported[camera_id];
-  assert(s->ci.frame_width != 0);
+  assert(ci.frame_width != 0);
 
-  s->camera_num = camera_num;
+  camera_num = camera_num_;
 
-  s->request_id_last = 0;
+  request_id_last = 0;
-  s->skipped = true;
+  skipped = true;
 
-  s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
+  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;
+  max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
-  s->target_grey_fraction = 0.3;
+  target_grey_fraction = 0.3;
 
-  s->dc_gain_enabled = false;
+  dc_gain_enabled = false;
-  s->gain_idx = ANALOG_GAIN_REC_IDX;
+  gain_idx = ANALOG_GAIN_REC_IDX;
-  s->exposure_time = 5;
+  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;
+  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) {
+void CameraState::camera_open() {
-  s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
+  sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
-  assert(s->sensor_fd >= 0);
+  assert(sensor_fd >= 0);
-  LOGD("opened sensor for %d", s->camera_num);
+  LOGD("opened sensor for %d", camera_num);
 
   // probe the sensor
-  LOGD("-- Probing sensor %d", s->camera_num);
+  LOGD("-- Probing sensor %d", camera_num);
-  sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->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);
+  auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr);
-  assert(sensor_dev_handle);
+  assert(sensor_dev_handle_);
-  s->sensor_dev_handle = *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,
@@ -649,29 +656,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);
+  auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
-  assert(isp_dev_handle);
+  assert(isp_dev_handle_);
-  s->isp_dev_handle = *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);
+  csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num);
-  assert(s->csiphy_fd >= 0);
+  assert(csiphy_fd >= 0);
-  LOGD("opened csiphy for %d", s->camera_num);
+  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);
+  auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info);
-  assert(csiphy_dev_handle);
+  assert(csiphy_dev_handle_);
-  s->csiphy_dev_handle = *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);
+  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(s, 0, 0, 1, s->buf0_handle, 0);
+  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(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(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(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
 
 
   // config csiphy
@@ -679,7 +686,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 +695,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 +705,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[0] = isp_dev_handle;
-  req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
+  req_mgr_link_info.dev_hdls[1] = 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));
+  ret = do_cam_control(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;
+  link_handle = req_mgr_link_info.link_hdl;
-  LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
+  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;
+  req_mgr_link_control.link_hdls[0] = 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));
+  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,
+  s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
-              VISION_STREAM_RGB_FRONT, 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,
+    s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
-                VISION_STREAM_RGB_BACK, 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,
+    s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
-                VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
     printf("wide road camera initted \n");
   }
 
@@ -768,7 +772,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 +783,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 +798,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;
+  req_mgr_link_control.link_hdls[0] = 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));
+  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.session_hdl = session_handle;
-  req_mgr_unlink_info.link_hdl = s->link_handle;
+  req_mgr_unlink_info.link_hdl = link_handle;
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
+  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};
+  struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle};
-  ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
+  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);
+    s->road_cam.camera_close();
-    camera_close(&s->wide_road_cam);
+    s->wide_road_cam.camera_close();
   }
 
   delete s->sm;
   delete s->pm;
 }
 
-// ******************* just a helper *******************
+void CameraState::handle_camera_event(void *evdat) {
-
-void handle_camera_event(CameraState *s, void *evdat) {
   struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
 
   uint64_t timestamp = event_data->u.frame_msg.timestamp;
@@ -869,53 +871,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);
+      clear_req_queue(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);
+      enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0);
-      s->skipped = true;
+      skipped = true;
-    } else if (main_id == s->frame_id_last + 1) {
+    } else if (main_id == frame_id_last + 1) {
-      s->skipped = false;
+      skipped = false;
     }
 
     // check for dropped requests
-    if (real_id > s->request_id_last + 1) {
+    if (real_id > request_id_last + 1) {
-      enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
+      enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0);
     }
 
     // metas
-    s->frame_id_last = main_id;
+    frame_id_last = main_id;
-    s->request_id_last = real_id;
+    request_id_last = real_id;
 
-    auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
+    auto &meta_data = buf.camera_bufs_metadata[buf_idx];
-    meta_data.frame_id = main_id - s->idx_offset;
+    meta_data.frame_id = main_id - idx_offset;
     meta_data.timestamp_sof = timestamp;
-    s->exp_lock.lock();
+    exp_lock.lock();
-    meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
+    meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac;
-    meta_data.high_conversion_gain = s->dc_gain_enabled;
+    meta_data.high_conversion_gain = dc_gain_enabled;
-    meta_data.integ_lines = s->exposure_time;
+    meta_data.integ_lines = exposure_time;
-    meta_data.measured_grey_fraction = s->measured_grey_fraction;
+    meta_data.measured_grey_fraction = measured_grey_fraction;
-    meta_data.target_grey_fraction = s->target_grey_fraction;
+    meta_data.target_grey_fraction = target_grey_fraction;
-    s->exp_lock.unlock();
+    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) {
+    if (main_id > frame_id_last + 10) {
-      clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
+      clear_req_queue(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);
+      enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0);
-      s->frame_id_last = main_id;
+      frame_id_last = main_id;
-      s->skipped = true;
+      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 +931,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 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 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 +947,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 +956,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 +974,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 +986,41 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
     }
   }
 
-  s->exp_lock.lock();
+  exp_lock.lock();
 
-  s->measured_grey_fraction = grey_frac;
+  measured_grey_fraction = grey_frac;
-  s->target_grey_fraction = target_grey;
+  target_grey_fraction = target_grey;
 
-  s->analog_gain_frac = sensor_analog_gains[new_g];
+  analog_gain_frac = sensor_analog_gains[new_g];
-  s->gain_idx = new_g;
+  gain_idx = new_g;
-  s->exposure_time = new_t;
+  exposure_time = new_t;
-  s->dc_gain_enabled = enable_dc_gain;
+  dc_gain_enabled = enable_dc_gain;
 
-  float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
+  float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0);
-  s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
+  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)},
+                                                  {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
-                                                  {0x3012, (uint16_t)s->exposure_time},
+                                                  {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 +1054,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);
+  s->driver_cam.sensors_start();
-  sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
   if (!env_only_driver) {
-    sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+    s->road_cam.sensors_start();
-    sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+    s->wide_road_cam.sensors_start();
   }
 
   // poll events
@@ -1088,11 +1088,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/car/chrysler/carstate.py

@@ -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

@@ -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

@@ -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

selfdrive/car/gm/carcontroller.py

@@ -41,7 +41,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.active and not (CS.out.steerFaultTemporary or CS.out.steerFaultPermanent) 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)

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.steerFaultTemporary = self.lkas_status == 2
-    ret.steerError = self.lkas_status == 3
+    ret.steerFaultPermanent = self.lkas_status == 3
 
     # 1 - open, 0 - closed
     ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or

selfdrive/car/honda/carcontroller.py

@@ -152,7 +152,7 @@ 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
+    lkas_active = active
 
     # Send CAN commands.
     can_sends = []

selfdrive/car/honda/carstate.py

@@ -197,11 +197,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")
+    ret.steerFaultPermanent = 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")
     # 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"]

selfdrive/car/hyundai/carcontroller.py

@@ -54,7 +54,7 @@ class CarController():
     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
+    lkas_active = c.active and not CS.out.steerFaultTemporary and CS.out.vEgo >= CS.CP.minSteerSpeed
 
     if not lkas_active:
       apply_steer = 0

selfdrive/car/hyundai/carstate.py

@@ -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:

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.IONIQ_EV_LTD, 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.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

@@ -87,7 +87,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,7 +141,7 @@ class CarInterfaceBase(ABC):
 
     # 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
@@ -150,7 +150,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)
 
     # we engage when pcm is active (rising edge)

selfdrive/car/mazda/carstate.py

@@ -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/subaru/carstate.py

@@ -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/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/carstate.py

@@ -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.steerFaultPermanent = steer_status == "EAC_FAULT"
-    ret.steerWarning = self.steer_warning != "EAC_ERROR_IDLE"
+    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/toyota/carstate.py

@@ -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)
 
@@ -58,8 +57,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
+    # On some cars, the angle measurement is non-zero while initializing
-    if abs(torque_sensor_angle_deg) > 1e-3:
+    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 +81,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
@@ -151,6 +150,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

@@ -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

selfdrive/car/toyota/values.py

@@ -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

@@ -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.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerFaultPermanent or CS.out.steerFaultTemporary):
         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

selfdrive/car/volkswagen/carstate.py

@@ -41,8 +41,8 @@ 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.steerFaultPermanent = hca_status in ("DISABLED", "FAULT")
-    ret.steerWarning = hca_status in ("INITIALIZING", "REJECTED")
+    ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED")
 
     # Update gas, brakes, and gearshift.
     ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0

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},

selfdrive/controls/controlsd.py

@@ -305,7 +305,7 @@ class Controls:
         self.events.add(EventName.cruiseMismatch)
 
     # Check for FCW
-    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.5
+    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
     model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
     planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
     if planner_fcw or model_fcw:
@@ -462,7 +462,8 @@ class Controls:
           else:
             self.state = State.enabled
           self.current_alert_types.append(ET.ENABLE)
-          self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
+          if not self.CP.pcmCruise:
+            self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last)
 
     # Check if actuators are enabled
     self.active = self.state == State.enabled or self.state == State.softDisabling
@@ -503,7 +504,7 @@ class Controls:
       actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan)
 
       # Steering PID loop and lateral MPC
-      lat_active = self.active and not CS.steerWarning and not CS.steerError and CS.vEgo > self.CP.minSteerSpeed
+      lat_active = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and CS.vEgo > self.CP.minSteerSpeed
       desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
                                                                              lat_plan.psis,
                                                                              lat_plan.curvatures,

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -43,11 +43,19 @@ generated_files = [
   f'{gen}/lat_cost/lat_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['lat_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["lat_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 lat_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -11,18 +11,19 @@ from selfdrive.modeld.constants import T_IDXS
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolverFast
+  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_lat.json"
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 def gen_lat_model():
   model = AcadosModel()
-  model.name = 'lat'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -58,7 +59,7 @@ def gen_lat_model():
   return model
 
 
-def gen_lat_mpc_solver():
+def gen_lat_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
@@ -103,7 +104,7 @@ def gen_lat_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_iter_max = 1
   ocp.solver_options.qp_solver_cond_N = 1
 
@@ -117,7 +118,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):
@@ -173,5 +174,6 @@ class LateralMpc():
 
 
 if __name__ == "__main__":
-  ocp = gen_lat_mpc_solver()
+  ocp = gen_lat_ocp()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longcontrol.py

@@ -12,14 +12,15 @@ ACCEL_MIN_ISO = -3.5  # m/s^2
 ACCEL_MAX_ISO = 2.0  # m/s^2
 
 
-def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
-                             brake_pressed, cruise_standstill):
+                             v_target_future, brake_pressed, cruise_standstill):
   """Update longitudinal control state machine"""
+  accelerating = v_target_future > v_target
   stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
                        (v_ego < CP.vEgoStopping and
-                        (v_target_future < CP.vEgoStopping or brake_pressed))
+                        ((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed))
 
-  starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
+  starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -83,7 +84,7 @@ class LongControl():
     # Update state machine
     output_accel = self.last_output_accel
     self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
-                                                       v_target_future, CS.brakePressed,
+                                                       v_target, v_target_future, CS.brakePressed,
                                                        CS.cruiseState.standstill)
 
     if self.long_control_state == LongCtrlState.off or CS.gasPressed:

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -28,8 +28,6 @@ casadi_cost_0 = [
 casadi_constraints = [
   f'{gen}/long_constraints/long_constr_h_fun.c',
   f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
-  f'{gen}/long_constraints/long_constr_h_e_fun.c',
-  f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c',
 ]
 
 build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
@@ -47,17 +45,24 @@ generated_files = [
 
   f'{gen}/long_model/long_model.h',
   f'{gen}/long_constraints/long_h_constraint.h',
-  f'{gen}/long_constraints/long_h_e_constraint.h',
   f'{gen}/long_cost/long_cost_y_fun.h',
   f'{gen}/long_cost/long_cost_y_e_fun.h',
   f'{gen}/long_cost/long_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['long_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["long_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 long_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -11,20 +11,20 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
+  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
-  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
 
 from casadi import SX, vertcat
 
+MODEL_NAME = 'long'
 LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_long.json"
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
 
 SOURCES = ['lead0', 'lead1', 'cruise']
 
 X_DIM = 3
 U_DIM = 1
-PARAM_DIM= 4
+PARAM_DIM = 4
 COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4
@@ -34,10 +34,11 @@ X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
-A_CHANGE_COST = .5
+A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -65,7 +66,7 @@ def desired_follow_distance(v_ego, v_lead):
 
 def gen_long_model():
   model = AcadosModel()
-  model.name = 'long'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -97,7 +98,7 @@ def gen_long_model():
   return model
 
 
-def gen_long_mpc_solver():
+def gen_long_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_long_model()
 
@@ -136,7 +137,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
-           20*(a_ego - prev_a),
+           a_ego - prev_a,
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -149,7 +150,6 @@ def gen_long_mpc_solver():
                         (a_max - a_ego),
                         ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
   ocp.model.con_h_expr = constraints
-  ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM))
 
   x0 = np.zeros(X_DIM)
   ocp.constraints.x0 = x0
@@ -163,9 +163,7 @@ def gen_long_mpc_solver():
   ocp.cost.zu = cost_weights
 
   ocp.constraints.lh = np.zeros(CONSTR_DIM)
-  ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
   ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
-  ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
   ocp.constraints.idxsh = np.arange(CONSTR_DIM)
 
   # The HPIPM solver can give decent solutions even when it is stopped early
@@ -175,12 +173,13 @@ def gen_long_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
-  ocp.solver_options.qp_solver_cond_N = N//4
+  ocp.solver_options.qp_solver_cond_N = 1
 
   # More iterations take too much time and less lead to inaccurate convergence in
   # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
   ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
 
   # set prediction horizon
   ocp.solver_options.tf = Tf
@@ -197,7 +196,7 @@ class LongitudinalMpc:
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -215,7 +214,11 @@ class LongitudinalMpc:
     self.status = False
     self.crash_cnt = 0.0
     self.solution_status = 0
+    # timers
     self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
@@ -232,6 +235,7 @@ class LongitudinalMpc:
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
     W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]))
     for i in range(N):
+      # reduce the cost on (a-a_prev) later in the horizon.
       W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -257,14 +261,12 @@ class LongitudinalMpc:
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_cur_state(self, v, a):
-    if abs(self.x0[1] - v) > 2.:
+    v_prev = self.x0[1]
-      self.x0[1] = v
+    self.x0[1] = v
-      self.x0[2] = a
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
       for i in range(0, N+1):
         self.solver.set(i, 'x', self.x0)
-    else:
-      self.x0[1] = v
-      self.x0[2] = a
 
   @staticmethod
   def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
@@ -355,9 +357,17 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "lbx", self.x0)
     self.solver.constraints_set(0, "ubx", self.x0)
 
-    t = sec_since_boot()
     self.solution_status = self.solver.solve()
-    self.solve_time = sec_since_boot() - t
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
+    # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
+    # res = self.solver.get_residuals()
+    # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
+    # self.solver.print_statistics()
 
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
@@ -370,6 +380,7 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
+    t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
@@ -378,5 +389,6 @@ class LongitudinalMpc:
 
 
 if __name__ == "__main__":
-  ocp = gen_long_mpc_solver()
+  ocp = gen_long_ocp()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longitudinal_planner.py

@@ -55,6 +55,7 @@ class Planner:
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
+    self.solverExecutionTime = 0.0
 
   def update(self, sm):
     v_ego = sm['carState'].vEgo

selfdrive/controls/tests/test_following_distance.py

@@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0):
 
 
 class TestFollowingDistance(unittest.TestCase):
-  def test_following_distanc(self):
+  def test_following_distance(self):
     for speed in np.arange(0, 40, 5):
       print(f'Testing {speed} m/s')
       v_lead = float(speed)

selfdrive/debug/can_print_changes.py

@@ -1,46 +1,103 @@
 #!/usr/bin/env python3
+import argparse
 import binascii
-import sys
+import time
 from collections import defaultdict
 
 import cereal.messaging as messaging
-from common.realtime import sec_since_boot
+from selfdrive.debug.can_table import can_table
+from tools.lib.logreader import logreader_from_route_or_segment
 
+RED = '\033[91m'
+CLEAR = '\033[0m'
+
+def update(msgs, bus, dat, low_to_high, high_to_low, quiet=False):
+  for x in msgs:
+    if x.which() != 'can':
+      continue
+
+    for y in x.can:
+      if y.src == bus:
+        dat[y.address] = y.dat
+
+        i = int.from_bytes(y.dat, byteorder='big')
+        l_h = low_to_high[y.address]
+        h_l = high_to_low[y.address]
+
+        change = None
+        if (i | l_h) != l_h:
+          low_to_high[y.address] = i | l_h
+          change = "+"
+
+        if (~i | h_l) != h_l:
+          high_to_low[y.address] = ~i | h_l
+          change = "-"
+
+        if change and not quiet:
+          print(f"{time.monotonic():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}")
 
-def can_printer(bus=0):
-  """Collects messages and prints when a new bit transition is observed.
-  This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt.
-  Leave the script running until no new transitions are seen, then perform the action."""
-  logcan = messaging.sub_sock('can')
 
+def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False):
+  logcan = messaging.sub_sock('can', timeout=10)
+
+  dat = defaultdict(int)
   low_to_high = defaultdict(int)
   high_to_low = defaultdict(int)
 
-  while 1:
+  if init_msgs is not None:
-    can_recv = messaging.drain_sock(logcan, wait_for_one=True)
+    update(init_msgs, bus, dat, low_to_high, high_to_low, quiet=True)
-    for x in can_recv:
-      for y in x.can:
-        if y.src == bus:
-          i = int.from_bytes(y.dat, byteorder='big')
 
-          l_h = low_to_high[y.address]
+  low_to_high_init = low_to_high.copy()
-          h_l = high_to_low[y.address]
+  high_to_low_init = high_to_low.copy()
 
-          change = None
+  if new_msgs is not None:
-          if (i | l_h) != l_h:
+    update(new_msgs, bus, dat, low_to_high, high_to_low)
-            low_to_high[y.address] = i | l_h
+  else:
-            change = "+"
+    # Live mode
+    try:
+      while 1:
+        can_recv = messaging.drain_sock(logcan)
+        update(can_recv, bus, dat, low_to_high, high_to_low)
+        time.sleep(0.02)
+    except KeyboardInterrupt:
+      pass
 
-          if (~i | h_l) != h_l:
+  print("\n\n")
-            high_to_low[y.address] = ~i | h_l
+  tables = ""
-            change = "-"
+  for addr in sorted(dat.keys()):
+    init = low_to_high_init[addr] & high_to_low_init[addr]
+    now = low_to_high[addr] & high_to_low[addr]
+    d = now & ~init
+    if d == 0:
+      continue
+    b = d.to_bytes(len(dat[addr]), byteorder='big')
 
-          if change:
+    byts = ''.join([(c if c == '0' else f'{RED}{c}{CLEAR}') for c in str(binascii.hexlify(b))[2:-1]])
-            print(f"{sec_since_boot():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}")
+    header = f"{hex(addr).ljust(6)}({str(addr).ljust(4)})"
+    print(header, byts)
+    tables += f"{header}\n"
+    tables += can_table(b) + "\n\n"
 
+  if table:
+    print(tables)
 
 if __name__ == "__main__":
-  if len(sys.argv) > 1:
+  desc = """Collects messages and prints when a new bit transition is observed.
-    can_printer(int(sys.argv[1]))
+  This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt.
-  else:
+  Leave the script running until no new transitions are seen, then perform the action."""
-    can_printer()
+  parser = argparse.ArgumentParser(description=desc,
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0)
+  parser.add_argument("--table", action="store_true", help="Print a cabana-like table")
+  parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with")
+  parser.add_argument("comp", type=str, nargs='?', help="Route or segment to compare against init")
+
+  args = parser.parse_args()
+
+  init_lr, new_lr = None, None
+  if args.init:
+    init_lr = logreader_from_route_or_segment(args.init)
+  if args.comp:
+    new_lr = logreader_from_route_or_segment(args.comp)
+
+  can_printer(args.bus, init_msgs=init_lr, new_msgs=new_lr, table=args.table)

selfdrive/debug/can_table.py

@@ -5,6 +5,19 @@ import pandas as pd  # pylint: disable=import-error
 import cereal.messaging as messaging
 
 
+def can_table(dat):
+  rows = []
+  for b in dat:
+    r = list(bin(b).lstrip('0b').zfill(8))
+    r += [hex(b)]
+    rows.append(r)
+
+  df = pd.DataFrame(data=rows)
+  df.columns = [str(n) for n in range(7, -1, -1)] + [' ']
+  table = df.to_markdown(tablefmt='grid')
+  return table
+
+
 if __name__ == "__main__":
   parser = argparse.ArgumentParser(description="Cabana-like table of bits for your terminal",
                                    formatter_class=argparse.ArgumentDefaultsHelpFormatter)
@@ -28,12 +41,5 @@ if __name__ == "__main__":
     if latest is None:
       continue
 
-    rows = []
+    table = can_table(latest.dat)
-    for b in latest.dat:
-      r = list(bin(b).lstrip('0b').zfill(8))
-      r += [hex(b)]
-      rows.append(r)
-
-    df = pd.DataFrame(data=rows)
-    table = df.to_markdown(tablefmt='grid')
     print(f"\n\n{hex(addr)} ({addr}) on bus {args.bus}\n{table}")

selfdrive/hardware/base.py

@@ -2,7 +2,11 @@ from abc import abstractmethod, ABC
 from collections import namedtuple
 from typing import Dict
 
+from cereal import log
+
 ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient', 'pmic'])
+NetworkType = log.DeviceState.NetworkType
+
 
 class HardwareBase(ABC):
   @staticmethod
@@ -67,6 +71,9 @@ class HardwareBase(ABC):
   def get_network_strength(self, network_type):
     pass
 
+  def get_network_metered(self, network_type) -> bool:
+    return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet)
+
   @staticmethod
   def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
     pass

selfdrive/hardware/tici/hardware.py

@@ -13,6 +13,7 @@ from selfdrive.hardware.tici.amplifier import Amplifier
 
 NM = 'org.freedesktop.NetworkManager'
 NM_CON_ACT = NM + '.Connection.Active'
+NM_DEV = NM + '.Device'
 NM_DEV_WL = NM + '.Device.Wireless'
 NM_AP = NM + '.AccessPoint'
 DBUS_PROPS = 'org.freedesktop.DBus.Properties'
@@ -37,6 +38,13 @@ class MM_MODEM_STATE(IntEnum):
        CONNECTING    = 10
        CONNECTED     = 11
 
+class NMMetered(IntEnum):
+  NM_METERED_UNKNOWN = 0
+  NM_METERED_YES = 1
+  NM_METERED_NO = 2
+  NM_METERED_GUESS_YES = 3
+  NM_METERED_GUESS_NO = 4
+
 TIMEOUT = 0.1
 
 NetworkType = log.DeviceState.NetworkType
@@ -91,11 +99,10 @@ class Tici(HardwareBase):
       primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
       primary_connection = self.bus.get_object(NM, primary_connection)
       primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
-      primary_id = primary_connection.Get(NM_CON_ACT, 'Id', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
 
       if primary_type == '802-3-ethernet':
         return NetworkType.ethernet
-      elif primary_type == '802-11-wireless' and primary_id != 'Hotspot':
+      elif primary_type == '802-11-wireless':
         return NetworkType.wifi
       else:
         active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
@@ -218,6 +225,27 @@ class Tici(HardwareBase):
 
     return network_strength
 
+  def get_network_metered(self, network_type) -> bool:
+    try:
+      primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      primary_connection = self.bus.get_object(NM, primary_connection)
+      primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+      for dev in primary_devices:
+        dev_obj = self.bus.get_object(NM, str(dev))
+        metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+        if network_type == NetworkType.wifi:
+          if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]:
+            return True
+        elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]:
+          if metered_prop == NMMetered.NM_METERED_NO:
+            return False
+    except Exception:
+      pass
+
+    return super().get_network_metered(network_type)
+
   @staticmethod
   def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
     upload_speed_kbps = int(upload_speed_kbps)  # Ensure integer value

selfdrive/locationd/locationd.cc

@@ -262,7 +262,6 @@ void Localizer::input_fake_gps_observations(double current_time) {
 
 void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
   // ignore the message if the fix is invalid
-
   bool gps_invalid_flag = (log.getFlags() % 2 == 0);
   bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
@@ -462,9 +461,9 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
 {
   cereal::Event::Builder evt = msg_builder.initEvent();
   evt.setLogMonoTime(logMonoTime);
+  evt.setValid(inputsOK);
   cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
   this->build_live_location(liveLoc);
-  liveLoc.setInputsOK(inputsOK);
   liveLoc.setSensorsOK(sensorsOK);
   liveLoc.setGpsOK(gpsOK);
   return msg_builder.toBytes();
@@ -497,14 +496,19 @@ int Localizer::locationd_thread() {
   PubMaster pm({ "liveLocationKalman" });
   SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
 
+  uint64_t cnt = 0;
+
   while (!do_exit) {
     sm.update();
-    for (const char* service : service_list) {
+    if (sm.allAliveAndValid()){
-      if (sm.updated(service) && sm.valid(service)) {
+      for (const char* service : service_list) {
-        const cereal::Event::Reader log = sm[service];
+        if (sm.updated(service)){
-        this->handle_msg(log);
+          const cereal::Event::Reader log = sm[service];
+          this->handle_msg(log);
+        }
       }
     }
+    
 
     if (sm.updated("cameraOdometry")) {
       uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
@@ -516,7 +520,7 @@ int Localizer::locationd_thread() {
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());
 
-      if (sm.frame % 1200 == 0 && gpsOK) {  // once a minute
+      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
         VectorXd posGeo = this->get_position_geodetic();
         std::string lastGPSPosJSON = util::string_format(
           "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
@@ -525,6 +529,7 @@ int Localizer::locationd_thread() {
           Params().put("LastGPSPosition", gpsjson);
         }, lastGPSPosJSON).detach();
       }
+      cnt++;
     }
   }
   return 0;

selfdrive/locationd/paramsd.py

@@ -62,7 +62,7 @@ class ParamsLearner:
       yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
 
       if self.active:
-        if msg.inputsOK and msg.posenetOK:
+        if msg.posenetOK:
 
           if yaw_rate_valid:
             self.kf.predict_and_observe(t,
@@ -160,10 +160,11 @@ def main(sm=None, pm=None):
 
   while True:
     sm.update()
-    for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
+    if sm.all_alive_and_valid():
-      if sm.updated[which]:
+      for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
-        t = sm.logMonoTime[which] * 1e-9
+        if sm.updated[which]:
-        learner.handle_log(t, which, sm[which])
+          t = sm.logMonoTime[which] * 1e-9
+          learner.handle_log(t, which, sm[which])
 
     if sm.updated['liveLocationKalman']:
       x = learner.kf.x
@@ -198,6 +199,8 @@ def main(sm=None, pm=None):
       liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
       liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
 
+      msg.valid = sm.all_alive_and_valid()
+
       if sm.frame % 1200 == 0:  # once a minute
         params = {
           'carFingerprint': CP.carFingerprint,

selfdrive/locationd/test/test_locationd.py

@@ -4,10 +4,12 @@ import json
 import random
 import unittest
 import time
+import capnp
 from cffi import FFI
 
 from cereal import log
 import cereal.messaging as messaging
+from cereal.services import service_list
 from common.params import Params
 
 from selfdrive.manager.process_config import managed_processes
@@ -103,13 +105,15 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
     ret = self.localizer_get_msg()
     self.assertFalse(ret.liveLocationKalman.posenetOK)
 
+
 class TestLocationdProc(unittest.TestCase):
   MAX_WAITS = 1000
+  LLD_MSGS = {'gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'}
 
   def setUp(self):
     random.seed(123489234)
 
-    self.pm = messaging.PubMaster({'gpsLocationExternal', 'cameraOdometry'})
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
 
     managed_processes['locationd'].prepare()
     managed_processes['locationd'].start()
@@ -127,43 +131,53 @@ class TestLocationdProc(unittest.TestCase):
       waits_left -= 1
     time.sleep(0.0001)
 
-  def test_params_gps(self):
+  def get_fake_msg(self, name, t):
-    # first reset params
+    try:
-    Params().put('LastGPSPosition', json.dumps({"latitude": 0.0, "longitude": 0.0, "altitude": 0.0}))
+      msg = messaging.new_message(name)
-
+    except capnp.lib.capnp.KjException:
-    lat = 30 + (random.random() * 10.0)
+      msg = messaging.new_message(name, 0)
-    lon = -70 + (random.random() * 10.0)
-    alt = 5 + (random.random() * 10.0)
 
-    for _ in range(1000):  # because of kalman filter, send often
+    if name == "gpsLocationExternal":
-      msg = messaging.new_message('gpsLocationExternal')
-      msg.logMonoTime = 0
       msg.gpsLocationExternal.flags = 1
       msg.gpsLocationExternal.verticalAccuracy = 1.0
       msg.gpsLocationExternal.speedAccuracy = 1.0
       msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
       msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
-      msg.gpsLocationExternal.latitude = lat
+      msg.gpsLocationExternal.latitude = self.lat
-      msg.gpsLocationExternal.longitude = lon
+      msg.gpsLocationExternal.longitude = self.lon
-      msg.gpsLocationExternal.altitude = alt
+      msg.gpsLocationExternal.altitude = self.alt
-      self.send_msg(msg)
+    elif name == 'cameraOdometry':
-
-    for _ in range(250):  # params is only written so often
-      msg = messaging.new_message('cameraOdometry')
-      msg.logMonoTime = 0
       msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
       msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
       msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
       msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
-      self.send_msg(msg)
+    msg.logMonoTime = t
+    return msg
 
+  def test_params_gps(self):
+    # first reset params
+    Params().delete('LastGPSPosition')
+
+    self.lat = 30 + (random.random() * 10.0)
+    self.lon = -70 + (random.random() * 10.0)
+    self.alt = 5 + (random.random() * 10.0)
+    self.fake_duration = 90  # secs
+    # get fake messages at the correct frequency, listed in services.py
+    fake_msgs = []
+    for sec in range(self.fake_duration):
+      for name in self.LLD_MSGS:
+        for j in range(int(service_list[name].frequency)):
+          fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9)))
+
+    for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime):
+      self.send_msg(fake_msg)
     time.sleep(1)  # wait for async params write
 
     lastGPS = json.loads(Params().get('LastGPSPosition'))
 
-    self.assertAlmostEqual(lastGPS['latitude'], lat, places=3)
+    self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3)
-    self.assertAlmostEqual(lastGPS['longitude'], lon, places=3)
+    self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3)
-    self.assertAlmostEqual(lastGPS['altitude'], alt, places=3)
+    self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)
 
 
 if __name__ == "__main__":

selfdrive/loggerd/uploader.py

@@ -165,14 +165,14 @@ class Uploader():
 
     return self.last_resp
 
-  def upload(self, key, fn, network_type):
+  def upload(self, key, fn, network_type, metered):
     try:
       sz = os.path.getsize(fn)
     except OSError:
       cloudlog.exception("upload: getsize failed")
       return False
 
-    cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type)
+    cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
 
     if sz == 0:
       try:
@@ -195,10 +195,10 @@ class Uploader():
         self.last_time = time.monotonic() - start_time
         self.last_speed = (sz / 1e6) / self.last_time
         success = True
-        cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
       else:
         success = False
-        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type)
+        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
 
     return success
 
@@ -247,7 +247,7 @@ def uploader_fn(exit_event):
 
     key, fn = d
 
-    success = uploader.upload(key, fn, sm['deviceState'].networkType.raw)
+    success = uploader.upload(key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
     if success:
       backoff = 0.1
     elif allow_sleep:
@@ -257,6 +257,7 @@ def uploader_fn(exit_event):
 
     pm.send("uploaderState", uploader.get_msg())
 
+
 def main():
   uploader_fn(threading.Event())
 

selfdrive/modeld/SConscript

@@ -31,9 +31,6 @@ thneed_src = [
 
 use_thneed = not GetOption('no_thneed')
 
-use_extra = True  # arch == "larch64"
-lenv['CXXFLAGS'].append('-DUSE_EXTRA=true' if use_extra else '-DUSE_EXTRA=false')
-
 if arch == "aarch64" or arch == "larch64":
   libs += ['gsl', 'CB']
   libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl']
@@ -68,16 +65,24 @@ common_model = lenv.Object(common_src)
 
 # build thneed model
 if use_thneed and arch in ("aarch64", "larch64"):
-  fn = "../../models/big_supercombo" if use_extra else "../../models/supercombo"
+  fn = File("#models/supercombo").abspath
   compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
-  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}.thneed --binary"
+  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}_badweights.thneed --binary"
 
   lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"])
   kernel_path = os.path.join(Dir('.').abspath, "thneed", "kernels")
   cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}", 'KERNEL_PATH': kernel_path})
 
   kernels = [os.path.join(kernel_path, x) for x in os.listdir(kernel_path) if x.endswith(".cl")]
-  cenv.Command(fn + ".thneed", [fn + ".dlc", kernels, compiler], cmd)
+  cenv.Command(fn + "_badweights.thneed", [fn + ".dlc", kernels, compiler], cmd)
+
+  from selfdrive.modeld.thneed.weights_fixup import weights_fixup
+  def weights_fixup_action(target, source, env):
+    weights_fixup(target[0].abspath, source[0].abspath, source[1].abspath)
+
+  env = Environment(BUILDERS = {'WeightFixup' : Builder(action = weights_fixup_action)})
+  env.WeightFixup(target=fn + ".thneed", source=[fn+"_badweights.thneed", fn+".dlc"])
+
 
 lenv.Program('_dmonitoringmodeld', [
     "dmonitoringmodeld.cc",

selfdrive/modeld/modeld.cc

@@ -51,7 +51,12 @@ mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera
   return matmul3(yuv_transform, transform);
 }
 
-void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra, bool use_extra_client) {
+static uint64_t get_ts(const VisionIpcBufExtra &extra) {
+  return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof;
+}
+
+
+void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
   // messaging
   PubMaster pm({"modelV2", "cameraOdometry"});
   SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration"});
@@ -74,18 +79,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
   VisionIpcBufExtra meta_extra = {0};
 
   while (!do_exit) {
-    // TODO: change sync logic to use timestamp start of frame in case camerad skips a frame
-    // log frame id in model packet
-
     // Keep receiving frames until we are at least 1 frame ahead of previous extra frame
-    while (meta_main.frame_id <= meta_extra.frame_id) {
+    while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) {
       buf_main = vipc_client_main.recv(&meta_main);
-      if (meta_main.frame_id <= meta_extra.frame_id) {
-        LOGE("main camera behind! main: %d (%.5f), extra: %d (%.5f)",
-          meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
-          meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
-      }
-
       if (buf_main == nullptr)  break;
     }
 
@@ -98,20 +94,14 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       // Keep receiving extra frames until frame id matches main camera
       do {
         buf_extra = vipc_client_extra.recv(&meta_extra);
-
+      } while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
-        if (meta_main.frame_id > meta_extra.frame_id) {
-          LOGE("extra camera behind! main: %d (%.5f), extra: %d (%.5f)",
-            meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
-            meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
-        }
-      } while (buf_extra != nullptr && meta_main.frame_id > meta_extra.frame_id);
 
       if (buf_extra == nullptr) {
         LOGE("vipc_client_extra no frame");
         continue;
       }
 
-      if (meta_main.frame_id != meta_extra.frame_id || std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
+      if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
         LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
           meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
           meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
@@ -134,9 +124,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
       }
 
       model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false);
-      if (use_extra) {
+      model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
-        model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
-      }
       live_calib_seen = true;
     }
 
@@ -161,7 +149,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
 
     float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-    model_publish(pm, meta_main.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
+    model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
                   kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
     posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
 
@@ -181,8 +169,7 @@ int main(int argc, char **argv) {
   }
 
   bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
-  bool use_extra = USE_EXTRA;
+  bool use_extra_client = Hardware::TICI() && !main_wide_camera;
-  bool use_extra_client = Hardware::TICI() && use_extra && !main_wide_camera;
 
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
@@ -190,7 +177,7 @@ int main(int argc, char **argv) {
 
   // init the models
   ModelState model;
-  model_init(&model, device_id, context, use_extra);
+  model_init(&model, device_id, context);
   LOGW("models loaded, modeld starting");
 
   VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
@@ -215,7 +202,7 @@ int main(int argc, char **argv) {
       LOGW("connected extra cam with buffer size: %d (%d x %d)", wb->len, wb->width, wb->height);
     }
 
-    run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra, use_extra_client);
+    run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra_client);
   }
 
   model_free(&model);

selfdrive/modeld/models/driving.cc

@@ -26,20 +26,18 @@ constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size>
   return kj::ArrayPtr(arr.data(), arr.size());
 }
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool use_extra) {
+void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   s->frame = new ModelFrame(device_id, context);
   s->wide_frame = new ModelFrame(device_id, context);
 
 #ifdef USE_THNEED
-  s->m = std::make_unique<ThneedModel>(use_extra ? "../../models/big_supercombo.thneed" : "../../models/supercombo.thneed",
+  s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #elif USE_ONNX_MODEL
-  s->m = std::make_unique<ONNXModel>(use_extra ? "../../models/big_supercombo.onnx" : "../../models/supercombo.onnx",
+  s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #else
-  s->m = std::make_unique<SNPEModel>(use_extra ? "../../models/big_supercombo.dlc" : "../../models/supercombo.dlc",
+  s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc",
-   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #endif
+   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, true);
 
 #ifdef TEMPORAL
   s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
@@ -316,13 +314,14 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
   }
 }
 
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,
                    float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid) {
   const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
   MessageBuilder msg;
   auto framed = msg.initEvent(valid).initModelV2();
   framed.setFrameId(vipc_frame_id);
+  framed.setFrameIdExtra(vipc_frame_id_extra);
   framed.setFrameAge(frame_age);
   framed.setFrameDropPerc(frame_drop * 100);
   framed.setTimestampEof(timestamp_eof);

selfdrive/modeld/models/driving.h

@@ -266,11 +266,11 @@ struct ModelState {
 #endif
 };
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool use_extra);
+void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
                               const mat3 &transform, const mat3 &transform_wide, float *desire_in);
 void model_free(ModelState* s);
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,
                    float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,

selfdrive/modeld/runners/thneedmodel.cc

@@ -47,7 +47,7 @@ void* ThneedModel::getExtraBuf() {
 
 void ThneedModel::execute() {
   if (!recorded) {
-    thneed->record = THNEED_RECORD;
+    thneed->record = true;
     if (use_extra) {
       float *inputs[5] = {recurrent, trafficConvention, desire, extra, input};
       thneed->copy_inputs(inputs);

selfdrive/modeld/thneed/compile.cc

@@ -13,7 +13,7 @@ int main(int argc, char* argv[]) {
   #define OUTPUT_SIZE 0x10000
 
   float *output = (float*)calloc(OUTPUT_SIZE, sizeof(float));
-  SNPEModel mdl(argv[1], output, 0, USE_GPU_RUNTIME, USE_EXTRA);
+  SNPEModel mdl(argv[1], output, 0, USE_GPU_RUNTIME, true);
 
   float state[TEMPORAL_SIZE] = {0};
   float desire[DESIRE_LEN] = {0};
@@ -25,9 +25,7 @@ int main(int argc, char* argv[]) {
   mdl.addDesire(desire, DESIRE_LEN);
   mdl.addTrafficConvention(traffic_convention, TRAFFIC_CONVENTION_LEN);
   mdl.addImage(input, 0);
-  if (USE_EXTRA) {
+  mdl.addExtra(extra, 0);
-    mdl.addExtra(extra, 0);
-  }
 
   // first run
   printf("************** execute 1 **************\n");
@@ -37,6 +35,14 @@ int main(int argc, char* argv[]) {
   // save model
   bool save_binaries = (argc > 3) && (strcmp(argv[3], "--binary") == 0);
   mdl.thneed->save(argv[2], save_binaries);
+
+  // test model
+  auto thneed = new Thneed(true);
+  thneed->record = false;
+  thneed->load(argv[2]);
+  thneed->clexec();
+  thneed->find_inputs_outputs();
+
   return 0;
 }
 

selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads.cl

@@ -1,4 +1,3 @@
 #define SUPPORT_DILATION
 
 __kernel void convolution_horizontal_reduced_reads(
-#include "convolution_.cl"

selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_1x1.cl

@@ -2,4 +2,3 @@
 #define SUPPORT_ACCUMULATION
 
 __kernel void convolution_horizontal_reduced_reads_1x1(
-#include "convolution_.cl"

selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_5_outputs.cl

@@ -1,4 +1,3 @@
 #define NUM_OUTPUTS 5
 
 __kernel void convolution_horizontal_reduced_reads_5_outputs(
-#include "convolution_.cl"

selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise.cl

@@ -2,4 +2,3 @@
 #define SUPPORT_DILATION
 
 __kernel void convolution_horizontal_reduced_reads_depthwise(
-#include "convolution_.cl"

selfdrive/modeld/thneed/kernels/convolution_horizontal_reduced_reads_depthwise_stride_1.cl

@@ -1,4 +1,3 @@
 #define DEPTHWISE
 
 __kernel void convolution_horizontal_reduced_reads_depthwise_stride_1(
-#include "convolution_.cl"

selfdrive/modeld/thneed/lib.py

@@ -0,0 +1,31 @@
+import struct, json
+
+def load_thneed(fn):
+  with open(fn, "rb") as f:
+    json_len = struct.unpack("I", f.read(4))[0]
+    jdat = json.loads(f.read(json_len).decode('latin_1'))
+    weights = f.read()
+  ptr = 0
+  for o in jdat['objects']:
+    if o['needs_load']:
+      nptr = ptr + o['size']
+      o['data'] = weights[ptr:nptr]
+      ptr = nptr
+  for o in jdat['binaries']:
+    nptr = ptr + o['length']
+    o['data'] = weights[ptr:nptr]
+    ptr = nptr
+  return jdat
+
+def save_thneed(jdat, fn):
+  new_weights = []
+  for o in jdat['objects'] + jdat['binaries']:
+    if 'data' in o:
+      new_weights.append(o['data'])
+      del o['data']
+  new_weights = b''.join(new_weights)
+  with open(fn, "wb") as f:
+    j = json.dumps(jdat, ensure_ascii=False).encode('latin_1')
+    f.write(struct.pack("I", len(j)))
+    f.write(j)
+    f.write(new_weights)

selfdrive/modeld/thneed/optimizer.cc

@@ -4,6 +4,9 @@
 #include <assert.h>
 #include "thneed.h"
 
+#include "selfdrive/common/util.h"
+#include "selfdrive/common/clutil.h"
+
 extern map<cl_program, string> g_program_source;
 
 static int is_same_size_image(cl_mem a, cl_mem b) {
@@ -63,6 +66,14 @@ static cl_mem make_image_like(cl_context context, cl_mem val) {
 int Thneed::optimize() {
   const char *kernel_path = getenv("KERNEL_PATH");
   if (!kernel_path) { kernel_path = "/data/openpilot/selfdrive/modeld/thneed/kernels"; printf("no KERNEL_PATH set, defaulting to %s\n", kernel_path); }
+
+  string convolution_;
+  {
+    char fn[0x100];
+    snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, "convolution_");
+    convolution_ = util::read_file(fn);
+  }
+
   // load custom kernels
   map<string, cl_program> g_programs;
   for (auto &k : kq) {
@@ -70,33 +81,17 @@ int Thneed::optimize() {
     if (g_programs.find(k->name) == g_programs.end()) {
       char fn[0x100];
       snprintf(fn, sizeof(fn), "%s/%s.cl", kernel_path, k->name.c_str());
-      FILE *g = fopen(fn, "rb");
+      if (util::file_exists(fn)) {
-      if (g != NULL) {
+        string kernel_src = util::read_file(fn);
-        char *src[0x10000];
+        if (k->name.rfind("convolution_", 0) == 0) {
-        const char *srcs[1]; srcs[0] = (const char *)src;
+          kernel_src += convolution_;
-        memset(src, 0, sizeof(src));
-        size_t length = fread(src, 1, sizeof(src), g);
-        fclose(g);
-
-        printf("building kernel %s\n", k->name.c_str());
-        k->program = clCreateProgramWithSource(context, 1, srcs, &length, NULL);
-        char options[0x100];
-        snprintf(options, sizeof(options)-1, "-I %s", kernel_path);
-        int err = clBuildProgram(k->program, 1, &device_id, options, NULL, NULL);
-
-        if (err != 0) {
-          printf("got err %d\n", err);
-          size_t err_length;
-          char buffer[2048];
-          clGetProgramBuildInfo(k->program, device_id, CL_PROGRAM_BUILD_LOG, sizeof(buffer), buffer, &err_length);
-          buffer[err_length] = '\0';
-          printf("%s\n", buffer);
         }
-        assert(err == 0);
+        printf("building kernel %s with len %lu\n", k->name.c_str(), kernel_src.length());
+        k->program = cl_program_from_source(context, device_id, kernel_src);
 
         // save in cache
         g_programs[k->name] = k->program;
-        g_program_source[k->program] = string((char *)src, length);
+        g_program_source[k->program] = kernel_src;
       } else {
         g_programs[k->name] = NULL;
       }

selfdrive/modeld/thneed/serialize.cc

@@ -63,14 +63,14 @@ void Thneed::load(const char *filename) {
 
   map<string, cl_program> g_programs;
   for (const auto &[name, source] : jdat["programs"].object_items()) {
-    if (record & THNEED_DEBUG) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
+    if (debug >= 1) printf("building %s with size %zu\n", name.c_str(), source.string_value().size());
     g_programs[name] = cl_program_from_source(context, device_id, source.string_value());
   }
 
   for (auto &obj : jdat["binaries"].array_items()) {
     string name = obj["name"].string_value();
     size_t length = obj["length"].int_value();
-    if (record & THNEED_DEBUG) printf("binary %s with size %zu\n", name.c_str(), length);
+    if (debug >= 1) printf("binary %s with size %zu\n", name.c_str(), length);
     g_programs[name] = cl_program_from_binary(context, device_id, (const uint8_t*)&buf[ptr], length);
     ptr += length;
   }

selfdrive/modeld/thneed/thneed.cc

@@ -55,12 +55,12 @@ int ioctl(int filedes, unsigned long request, void *argp) {
   if (thneed != NULL) {
     if (request == IOCTL_KGSL_GPU_COMMAND) {
       struct kgsl_gpu_command *cmd = (struct kgsl_gpu_command *)argp;
-      if (thneed->record & THNEED_RECORD) {
+      if (thneed->record) {
         thneed->timestamp = cmd->timestamp;
         thneed->context_id = cmd->context_id;
         thneed->cmds.push_back(unique_ptr<CachedCommand>(new CachedCommand(thneed, cmd)));
       }
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("IOCTL_KGSL_GPU_COMMAND(%2zu): flags: 0x%lx    context_id: %u  timestamp: %u  numcmds: %d  numobjs: %d\n",
             thneed->cmds.size(),
             cmd->flags,
@@ -70,7 +70,7 @@ int ioctl(int filedes, unsigned long request, void *argp) {
       struct kgsl_gpuobj_sync *cmd = (struct kgsl_gpuobj_sync *)argp;
       struct kgsl_gpuobj_sync_obj *objs = (struct kgsl_gpuobj_sync_obj *)(cmd->objs);
 
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 2) {
         printf("IOCTL_KGSL_GPUOBJ_SYNC count:%d ", cmd->count);
         for (int i = 0; i < cmd->count; i++) {
           printf(" -- offset:0x%lx len:0x%lx id:%d op:%d  ", objs[i].offset, objs[i].length, objs[i].id, objs[i].op);
@@ -78,21 +78,21 @@ int ioctl(int filedes, unsigned long request, void *argp) {
         printf("\n");
       }
 
-      if (thneed->record & THNEED_RECORD) {
+      if (thneed->record) {
         thneed->cmds.push_back(unique_ptr<CachedSync>(new
               CachedSync(thneed, string((char *)objs, sizeof(struct kgsl_gpuobj_sync_obj)*cmd->count))));
       }
     } else if (request == IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID) {
       struct kgsl_device_waittimestamp_ctxtid *cmd = (struct kgsl_device_waittimestamp_ctxtid *)argp;
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID: context_id: %d  timestamp: %d  timeout: %d\n",
             cmd->context_id, cmd->timestamp, cmd->timeout);
       }
     } else if (request == IOCTL_KGSL_SETPROPERTY) {
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         struct kgsl_device_getproperty *prop = (struct kgsl_device_getproperty *)argp;
         printf("IOCTL_KGSL_SETPROPERTY: 0x%x sizebytes:%zu\n", prop->type, prop->sizebytes);
-        if (thneed->record & THNEED_VERBOSE_DEBUG) {
+        if (thneed->debug >= 2) {
           hexdump((uint8_t *)prop->value, prop->sizebytes);
           if (prop->type == KGSL_PROP_PWR_CONSTRAINT) {
             struct kgsl_device_constraint *constraint = (struct kgsl_device_constraint *)prop->value;
@@ -105,7 +105,7 @@ int ioctl(int filedes, unsigned long request, void *argp) {
     } else if (request == IOCTL_KGSL_GPUOBJ_ALLOC || request == IOCTL_KGSL_GPUOBJ_FREE) {
       // this happens
     } else {
-      if (thneed->record & THNEED_DEBUG) {
+      if (thneed->debug >= 1) {
         printf("other ioctl %lx\n", request);
       }
     }
@@ -197,9 +197,9 @@ void CachedCommand::exec() {
   cache.timestamp = ++thneed->timestamp;
   int ret = ioctl(thneed->fd, IOCTL_KGSL_GPU_COMMAND, &cache);
 
-  if (thneed->record & THNEED_DEBUG) printf("CachedCommand::exec got %d\n", ret);
+  if (thneed->debug >= 1) printf("CachedCommand::exec got %d\n", ret);
 
-  if (thneed->record & THNEED_VERBOSE_DEBUG) {
+  if (thneed->debug >= 2) {
     for (auto &it : kq) {
       it->debug_print(false);
     }
@@ -220,9 +220,11 @@ Thneed::Thneed(bool do_clinit) {
   assert(g_fd != -1);
   fd = g_fd;
   ram = make_unique<GPUMalloc>(0x80000, fd);
-  record = THNEED_RECORD;
+  record = true;
   timestamp = -1;
   g_thneed = this;
+  char *thneed_debug_env = getenv("THNEED_DEBUG");
+  debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0;
 }
 
 void Thneed::stop() {
@@ -261,7 +263,7 @@ void Thneed::find_inputs_outputs() {
 void Thneed::copy_inputs(float **finputs) {
   //cl_int ret;
   for (int idx = 0; idx < inputs.size(); ++idx) {
-    if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]);
+    if (debug >= 1) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]);
     if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
   }
 }
@@ -270,7 +272,7 @@ void Thneed::copy_output(float *foutput) {
   if (output != NULL) {
     size_t sz;
     clGetMemObjectInfo(output, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
-    if (record & THNEED_DEBUG) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
+    if (debug >= 1) printf("copying %lu for output %p -> %p\n", sz, output, foutput);
     clEnqueueReadBuffer(command_queue, output, CL_TRUE, 0, sz, foutput, 0, NULL, NULL);
   } else {
     printf("CAUTION: model output is NULL, does it have no outputs?\n");
@@ -287,12 +289,12 @@ void Thneed::wait() {
   int wret = ioctl(fd, IOCTL_KGSL_DEVICE_WAITTIMESTAMP_CTXTID, &wait);
   uint64_t te = nanos_since_boot();
 
-  if (record & THNEED_DEBUG) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
+  if (debug >= 1) printf("wait %d after %lu us\n", wret, (te-tb)/1000);
 }
 
 void Thneed::execute(float **finputs, float *foutput, bool slow) {
   uint64_t tb, te;
-  if (record & THNEED_DEBUG) tb = nanos_since_boot();
+  if (debug >= 1) tb = nanos_since_boot();
 
   // ****** copy inputs
   copy_inputs(finputs);
@@ -319,7 +321,7 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) {
   int i = 0;
   for (auto &it : cmds) {
     ++i;
-    if (record & THNEED_DEBUG) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
+    if (debug >= 1) printf("run %2d @ %7lu us: ", i, (nanos_since_boot()-tb)/1000);
     it->exec();
     if ((i == cmds.size()) || slow) wait();
   }
@@ -335,7 +337,7 @@ void Thneed::execute(float **finputs, float *foutput, bool slow) {
   ret = ioctl(fd, IOCTL_KGSL_SETPROPERTY, &prop);
   assert(ret == 0);
 
-  if (record & THNEED_DEBUG) {
+  if (debug >= 1) {
     te = nanos_since_boot();
     printf("model exec in %lu us\n", (te-tb)/1000);
   }
@@ -353,7 +355,7 @@ void Thneed::clinit() {
 cl_int Thneed::clexec() {
   printf("Thneed::clexec: running %lu queued kernels\n", kq.size());
   for (auto &k : kq) {
-    if (record & THNEED_RECORD) ckq.push_back(k);
+    if (record) ckq.push_back(k);
     cl_int ret = k->exec();
     assert(ret == CL_SUCCESS);
   }
@@ -391,7 +393,7 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue,
   assert(event_wait_list == NULL);
 
   cl_int ret = 0;
-  if (thneed != NULL && thneed->record & THNEED_RECORD) {
+  if (thneed != NULL && thneed->record) {
     if (thneed->context == NULL) {
       thneed->command_queue = command_queue;
       clGetKernelInfo(kernel, CL_KERNEL_CONTEXT, sizeof(thneed->context), &thneed->context, NULL);
@@ -413,7 +415,7 @@ cl_int thneed_clEnqueueNDRangeKernel(cl_command_queue command_queue,
 cl_int thneed_clFinish(cl_command_queue command_queue) {
   Thneed *thneed = g_thneed;
 
-  if (thneed != NULL && thneed->record & THNEED_RECORD) {
+  if (thneed != NULL && thneed->record) {
     #ifdef RUN_OPTIMIZER
       thneed->optimize();
     #endif
@@ -520,8 +522,8 @@ cl_int CLQueuedKernel::exec() {
     }
   }
 
-  if (thneed->record & THNEED_DEBUG) {
+  if (thneed->debug >= 1) {
-    debug_print(thneed->record & THNEED_VERBOSE_DEBUG);
+    debug_print(thneed->debug >= 2);
   }
 
   return clEnqueueNDRangeKernel(thneed->command_queue,

selfdrive/modeld/thneed/thneed.h

@@ -14,10 +14,6 @@
 
 #include "selfdrive/modeld/thneed/include/msm_kgsl.h"
 
-#define THNEED_RECORD 1
-#define THNEED_DEBUG 2
-#define THNEED_VERBOSE_DEBUG 4
-
 using namespace std;
 
 namespace json11 {
@@ -110,7 +106,8 @@ class Thneed {
     int context_id;
 
     // protected?
-    int record;
+    bool record;
+    int debug;
     int timestamp;
     unique_ptr<GPUMalloc> ram;
     vector<unique_ptr<CachedIoctl> > cmds;