mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-21 14:02:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot 2023-02-07T02:56:39 for EON/C2

Browse Source 
version: dragonpilot v0.9.1 beta for EON/C2
date: 2023-02-07T02:56:39
dp-dev(priv2) master commit: 459790d03b790e5f3a8cff18710ac4d44841a16a
This commit is contained in:
Dragonpilot Team
2023-02-07 02:48:45 +00:00
committed by Comma Device
parent 1f2e3aa8b6
commit 8555e48fc9
79 changed files with 6825 additions and 3185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+4
View File
@@ -8,6 +8,10 @@ dragonpilot [Latest] - EON/C2 Release
* NEW: Dynamic End-to-End can be turned on when lead car is going below x km/h. (Useful for stop and go traffic.)
* NEW: Toggle to adapt Dynamic End-to-End to Following Dist. Mode.
* NEW: Toggle to adapt Dynamic End-to-End to Accel Mode.
* NEW: Always On Lateral v2 (For Toyota/VAG/HKG/Honda).
* NEW: VAG - vw-spam-resume (Thanks to @jyoung8607)
* FIXED: Bugs in updated.
* FIXED: Issue in C2 fan control.
* TWEAK: Bug fixes and improvement.
dragonpilot 2022.11.09 - EON/C2 Release
cereal/messaging/messaging_pyx.cpp
+23 -122
View File
@@ -1,4 +1,4 @@
/* Generated by Cython 0.29.26 */
/* Generated by Cython 0.29.24 */
/* BEGIN: Cython Metadata
{
@@ -28,8 +28,8 @@ END: Cython Metadata */
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_26"
#define CYTHON_HEX_VERSION 0x001D1AF0
#define CYTHON_ABI "0_29_24"
#define CYTHON_HEX_VERSION 0x001D18F0
#define CYTHON_FUTURE_DIVISION 1
#include <stddef.h>
#ifndef offsetof
@@ -176,7 +176,7 @@ END: Cython Metadata */
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -195,7 +195,7 @@ END: Cython Metadata */
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -214,9 +214,7 @@ END: Cython Metadata */
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#if PY_MAJOR_VERSION < 3
#include "longintrepr.h"
#endif
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
@@ -347,68 +345,9 @@ class __Pyx_FakeReference {
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#define __Pyx_DefaultClassType PyType_Type
#if PY_VERSION_HEX >= 0x030B00A1
static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
PyObject *code, PyObject *c, PyObject* n, PyObject *v,
PyObject *fv, PyObject *cell, PyObject* fn,
PyObject *name, int fline, PyObject *lnos) {
PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
const char *fn_cstr=NULL;
const char *name_cstr=NULL;
PyCodeObject* co=NULL;
PyObject *type, *value, *traceback;
PyErr_Fetch(&type, &value, &traceback);
if (!(kwds=PyDict_New())) goto end;
if (!(argcount=PyLong_FromLong(a))) goto end;
if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
if (!(nlocals=PyLong_FromLong(l))) goto end;
if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
if (!(stacksize=PyLong_FromLong(s))) goto end;
if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
if (!(flags=PyLong_FromLong(f))) goto end;
if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
Py_XDECREF((PyObject*)co);
co = (PyCodeObject*)call_result;
call_result = NULL;
if (0) {
cleanup_code_too:
Py_XDECREF((PyObject*)co);
co = NULL;
}
end:
Py_XDECREF(kwds);
Py_XDECREF(argcount);
Py_XDECREF(posonlyargcount);
Py_XDECREF(kwonlyargcount);
Py_XDECREF(nlocals);
Py_XDECREF(stacksize);
Py_XDECREF(replace);
Py_XDECREF(call_result);
Py_XDECREF(empty);
if (type) {
PyErr_Restore(type, value, traceback);
}
return co;
}
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -646,10 +585,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsHash_t
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -814,7 +753,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
@@ -1182,7 +1120,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
#ifndef Py_MEMBER_SIZE
#define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
#endif
#if CYTHON_FAST_PYCALL
static size_t __pyx_pyframe_localsplus_offset = 0;
#include "frameobject.h"
#define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1190,7 +1127,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
(void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
#define __Pyx_PyFrame_GetLocalsplus(frame)\
(assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
#endif // CYTHON_FAST_PYCALL
#endif
/* PyObjectCallMethO.proto */
@@ -3986,9 +3922,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_Context = {
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static PyObject *__pyx_tp_new_6cereal_9messaging_13messaging_pyx_Poller(PyTypeObject *t, CYTHON_UNUSED PyObject *a, CYTHON_UNUSED PyObject *k) {
@@ -4122,9 +4055,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_Poller = {
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static struct __pyx_vtabstruct_6cereal_9messaging_13messaging_pyx_SubSocket __pyx_vtable_6cereal_9messaging_13messaging_pyx_SubSocket;
@@ -4239,9 +4169,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_SubSocket = {
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static PyObject *__pyx_tp_new_6cereal_9messaging_13messaging_pyx_PubSocket(PyTypeObject *t, CYTHON_UNUSED PyObject *a, CYTHON_UNUSED PyObject *k) {
@@ -4352,9 +4279,6 @@ static PyTypeObject __pyx_type_6cereal_9messaging_13messaging_pyx_PubSocket = {
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static PyMethodDef __pyx_methods[] = {
@@ -6156,7 +6080,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
}
if (!use_cline) {
c_line = 0;
(void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
}
else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
c_line = 0;
@@ -6253,31 +6177,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
const char *funcname, int c_line,
int py_line, const char *filename) {
PyCodeObject *py_code = NULL;
PyObject *py_funcname = NULL;
PyCodeObject *py_code = 0;
PyObject *py_srcfile = 0;
PyObject *py_funcname = 0;
#if PY_MAJOR_VERSION < 3
PyObject *py_srcfile = NULL;
py_srcfile = PyString_FromString(filename);
if (!py_srcfile) goto bad;
#else
py_srcfile = PyUnicode_FromString(filename);
#endif
if (!py_srcfile) goto bad;
if (c_line) {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
funcname = PyUnicode_AsUTF8(py_funcname);
if (!funcname) goto bad;
#endif
}
else {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromString(funcname);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromString(funcname);
#endif
}
#if PY_MAJOR_VERSION < 3
if (!py_funcname) goto bad;
py_code = __Pyx_PyCode_New(
0,
0,
@@ -6296,16 +6219,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
__pyx_empty_bytes /*PyObject *lnotab*/
);
Py_DECREF(py_srcfile);
#else
py_code = PyCode_NewEmpty(filename, funcname, py_line);
#endif
Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline
Py_DECREF(py_funcname);
return py_code;
bad:
Py_XDECREF(py_funcname);
#if PY_MAJOR_VERSION < 3
Py_XDECREF(py_srcfile);
#endif
Py_XDECREF(py_funcname);
return NULL;
}
static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -7153,23 +7071,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
Py_DECREF(x);
return ival;
}
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
#if PY_MAJOR_VERSION < 3
} else if (likely(PyInt_CheckExact(o))) {
return PyInt_AS_LONG(o);
#endif
} else {
Py_ssize_t ival;
PyObject *x;
x = PyNumber_Index(o);
if (!x) return -1;
ival = PyInt_AsLong(x);
Py_DECREF(x);
return ival;
}
}
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
}
cereal/messaging/messaging_pyx.so
BIN
View File
Binary file not shown.
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/clock.cpp
+23 -108
View File
@@ -1,4 +1,4 @@
/* Generated by Cython 0.29.26 */
/* Generated by Cython 0.29.24 */
/* BEGIN: Cython Metadata
{
@@ -23,8 +23,8 @@ END: Cython Metadata */
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_26"
#define CYTHON_HEX_VERSION 0x001D1AF0
#define CYTHON_ABI "0_29_24"
#define CYTHON_HEX_VERSION 0x001D18F0
#define CYTHON_FUTURE_DIVISION 1
#include <stddef.h>
#ifndef offsetof
@@ -171,7 +171,7 @@ END: Cython Metadata */
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -190,7 +190,7 @@ END: Cython Metadata */
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -209,9 +209,7 @@ END: Cython Metadata */
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#if PY_MAJOR_VERSION < 3
#include "longintrepr.h"
#endif
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
@@ -342,68 +340,9 @@ class __Pyx_FakeReference {
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#define __Pyx_DefaultClassType PyType_Type
#if PY_VERSION_HEX >= 0x030B00A1
static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
PyObject *code, PyObject *c, PyObject* n, PyObject *v,
PyObject *fv, PyObject *cell, PyObject* fn,
PyObject *name, int fline, PyObject *lnos) {
PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
const char *fn_cstr=NULL;
const char *name_cstr=NULL;
PyCodeObject* co=NULL;
PyObject *type, *value, *traceback;
PyErr_Fetch(&type, &value, &traceback);
if (!(kwds=PyDict_New())) goto end;
if (!(argcount=PyLong_FromLong(a))) goto end;
if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
if (!(nlocals=PyLong_FromLong(l))) goto end;
if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
if (!(stacksize=PyLong_FromLong(s))) goto end;
if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
if (!(flags=PyLong_FromLong(f))) goto end;
if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
Py_XDECREF((PyObject*)co);
co = (PyCodeObject*)call_result;
call_result = NULL;
if (0) {
cleanup_code_too:
Py_XDECREF((PyObject*)co);
co = NULL;
}
end:
Py_XDECREF(kwds);
Py_XDECREF(argcount);
Py_XDECREF(posonlyargcount);
Py_XDECREF(kwonlyargcount);
Py_XDECREF(nlocals);
Py_XDECREF(stacksize);
Py_XDECREF(replace);
Py_XDECREF(call_result);
Py_XDECREF(empty);
if (type) {
PyErr_Restore(type, value, traceback);
}
return co;
}
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -641,10 +580,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsHash_t
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -803,7 +742,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
@@ -1864,7 +1802,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
}
if (!use_cline) {
c_line = 0;
(void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
}
else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
c_line = 0;
@@ -1961,31 +1899,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
const char *funcname, int c_line,
int py_line, const char *filename) {
PyCodeObject *py_code = NULL;
PyObject *py_funcname = NULL;
PyCodeObject *py_code = 0;
PyObject *py_srcfile = 0;
PyObject *py_funcname = 0;
#if PY_MAJOR_VERSION < 3
PyObject *py_srcfile = NULL;
py_srcfile = PyString_FromString(filename);
if (!py_srcfile) goto bad;
#else
py_srcfile = PyUnicode_FromString(filename);
#endif
if (!py_srcfile) goto bad;
if (c_line) {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
funcname = PyUnicode_AsUTF8(py_funcname);
if (!funcname) goto bad;
#endif
}
else {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromString(funcname);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromString(funcname);
#endif
}
#if PY_MAJOR_VERSION < 3
if (!py_funcname) goto bad;
py_code = __Pyx_PyCode_New(
0,
0,
@@ -2004,16 +1941,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
__pyx_empty_bytes /*PyObject *lnotab*/
);
Py_DECREF(py_srcfile);
#else
py_code = PyCode_NewEmpty(filename, funcname, py_line);
#endif
Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline
Py_DECREF(py_funcname);
return py_code;
bad:
Py_XDECREF(py_funcname);
#if PY_MAJOR_VERSION < 3
Py_XDECREF(py_srcfile);
#endif
Py_XDECREF(py_funcname);
return NULL;
}
static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -2861,23 +2793,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
Py_DECREF(x);
return ival;
}
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
#if PY_MAJOR_VERSION < 3
} else if (likely(PyInt_CheckExact(o))) {
return PyInt_AS_LONG(o);
#endif
} else {
Py_ssize_t ival;
PyObject *x;
x = PyNumber_Index(o);
if (!x) return -1;
ival = PyInt_AsLong(x);
Py_DECREF(x);
return ival;
}
}
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
}
common/clock.so
BIN
View File
Binary file not shown.
common/kalman/simple_kalman_impl.cpp
+23 -113
View File
@@ -1,4 +1,4 @@
/* Generated by Cython 0.29.26 */
/* Generated by Cython 0.29.24 */
/* BEGIN: Cython Metadata
{
@@ -22,8 +22,8 @@ END: Cython Metadata */
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_26"
#define CYTHON_HEX_VERSION 0x001D1AF0
#define CYTHON_ABI "0_29_24"
#define CYTHON_HEX_VERSION 0x001D18F0
#define CYTHON_FUTURE_DIVISION 1
#include <stddef.h>
#ifndef offsetof
@@ -170,7 +170,7 @@ END: Cython Metadata */
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -189,7 +189,7 @@ END: Cython Metadata */
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -208,9 +208,7 @@ END: Cython Metadata */
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#if PY_MAJOR_VERSION < 3
#include "longintrepr.h"
#endif
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
@@ -341,68 +339,9 @@ class __Pyx_FakeReference {
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#define __Pyx_DefaultClassType PyType_Type
#if PY_VERSION_HEX >= 0x030B00A1
static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
PyObject *code, PyObject *c, PyObject* n, PyObject *v,
PyObject *fv, PyObject *cell, PyObject* fn,
PyObject *name, int fline, PyObject *lnos) {
PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
const char *fn_cstr=NULL;
const char *name_cstr=NULL;
PyCodeObject* co=NULL;
PyObject *type, *value, *traceback;
PyErr_Fetch(&type, &value, &traceback);
if (!(kwds=PyDict_New())) goto end;
if (!(argcount=PyLong_FromLong(a))) goto end;
if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
if (!(nlocals=PyLong_FromLong(l))) goto end;
if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
if (!(stacksize=PyLong_FromLong(s))) goto end;
if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
if (!(flags=PyLong_FromLong(f))) goto end;
if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
Py_XDECREF((PyObject*)co);
co = (PyCodeObject*)call_result;
call_result = NULL;
if (0) {
cleanup_code_too:
Py_XDECREF((PyObject*)co);
co = NULL;
}
end:
Py_XDECREF(kwds);
Py_XDECREF(argcount);
Py_XDECREF(posonlyargcount);
Py_XDECREF(kwonlyargcount);
Py_XDECREF(nlocals);
Py_XDECREF(stacksize);
Py_XDECREF(replace);
Py_XDECREF(call_result);
Py_XDECREF(empty);
if (type) {
PyErr_Restore(type, value, traceback);
}
return co;
}
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -640,10 +579,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsHash_t
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -799,7 +738,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
@@ -1178,7 +1116,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
#ifndef Py_MEMBER_SIZE
#define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
#endif
#if CYTHON_FAST_PYCALL
static size_t __pyx_pyframe_localsplus_offset = 0;
#include "frameobject.h"
#define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1186,7 +1123,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
(void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
#define __Pyx_PyFrame_GetLocalsplus(frame)\
(assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
#endif // CYTHON_FAST_PYCALL
#endif
/* PyObjectCall.proto */
@@ -4393,9 +4329,6 @@ static PyTypeObject __pyx_type_6common_6kalman_18simple_kalman_impl_KF1D = {
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static PyMethodDef __pyx_methods[] = {
@@ -5940,7 +5873,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
}
if (!use_cline) {
c_line = 0;
(void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
}
else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
c_line = 0;
@@ -6037,31 +5970,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
const char *funcname, int c_line,
int py_line, const char *filename) {
PyCodeObject *py_code = NULL;
PyObject *py_funcname = NULL;
PyCodeObject *py_code = 0;
PyObject *py_srcfile = 0;
PyObject *py_funcname = 0;
#if PY_MAJOR_VERSION < 3
PyObject *py_srcfile = NULL;
py_srcfile = PyString_FromString(filename);
if (!py_srcfile) goto bad;
#else
py_srcfile = PyUnicode_FromString(filename);
#endif
if (!py_srcfile) goto bad;
if (c_line) {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
funcname = PyUnicode_AsUTF8(py_funcname);
if (!funcname) goto bad;
#endif
}
else {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromString(funcname);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromString(funcname);
#endif
}
#if PY_MAJOR_VERSION < 3
if (!py_funcname) goto bad;
py_code = __Pyx_PyCode_New(
0,
0,
@@ -6080,16 +6012,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
__pyx_empty_bytes /*PyObject *lnotab*/
);
Py_DECREF(py_srcfile);
#else
py_code = PyCode_NewEmpty(filename, funcname, py_line);
#endif
Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline
Py_DECREF(py_funcname);
return py_code;
bad:
Py_XDECREF(py_funcname);
#if PY_MAJOR_VERSION < 3
Py_XDECREF(py_srcfile);
#endif
Py_XDECREF(py_funcname);
return NULL;
}
static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -6937,23 +6864,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
Py_DECREF(x);
return ival;
}
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
#if PY_MAJOR_VERSION < 3
} else if (likely(PyInt_CheckExact(o))) {
return PyInt_AS_LONG(o);
#endif
} else {
Py_ssize_t ival;
PyObject *x;
x = PyNumber_Index(o);
if (!x) return -1;
ival = PyInt_AsLong(x);
Py_DECREF(x);
return ival;
}
}
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
}
common/kalman/simple_kalman_impl.so
BIN
View File
Binary file not shown.
common/params_pyx.so
BIN
View File
Binary file not shown.
common/transformations/transformations.cpp
+23 -113
View File
@@ -1,4 +1,4 @@
/* Generated by Cython 0.29.26 */
/* Generated by Cython 0.29.24 */
/* BEGIN: Cython Metadata
{
@@ -32,8 +32,8 @@ END: Cython Metadata */
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_26"
#define CYTHON_HEX_VERSION 0x001D1AF0
#define CYTHON_ABI "0_29_24"
#define CYTHON_HEX_VERSION 0x001D18F0
#define CYTHON_FUTURE_DIVISION 1
#include <stddef.h>
#ifndef offsetof
@@ -180,7 +180,7 @@ END: Cython Metadata */
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -199,7 +199,7 @@ END: Cython Metadata */
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -218,9 +218,7 @@ END: Cython Metadata */
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#if PY_MAJOR_VERSION < 3
#include "longintrepr.h"
#endif
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
@@ -351,68 +349,9 @@ class __Pyx_FakeReference {
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#define __Pyx_DefaultClassType PyType_Type
#if PY_VERSION_HEX >= 0x030B00A1
static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
PyObject *code, PyObject *c, PyObject* n, PyObject *v,
PyObject *fv, PyObject *cell, PyObject* fn,
PyObject *name, int fline, PyObject *lnos) {
PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
const char *fn_cstr=NULL;
const char *name_cstr=NULL;
PyCodeObject* co=NULL;
PyObject *type, *value, *traceback;
PyErr_Fetch(&type, &value, &traceback);
if (!(kwds=PyDict_New())) goto end;
if (!(argcount=PyLong_FromLong(a))) goto end;
if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
if (!(nlocals=PyLong_FromLong(l))) goto end;
if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
if (!(stacksize=PyLong_FromLong(s))) goto end;
if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
if (!(flags=PyLong_FromLong(f))) goto end;
if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
Py_XDECREF((PyObject*)co);
co = (PyCodeObject*)call_result;
call_result = NULL;
if (0) {
cleanup_code_too:
Py_XDECREF((PyObject*)co);
co = NULL;
}
end:
Py_XDECREF(kwds);
Py_XDECREF(argcount);
Py_XDECREF(posonlyargcount);
Py_XDECREF(kwonlyargcount);
Py_XDECREF(nlocals);
Py_XDECREF(stacksize);
Py_XDECREF(replace);
Py_XDECREF(call_result);
Py_XDECREF(empty);
if (type) {
PyErr_Restore(type, value, traceback);
}
return co;
}
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -650,10 +589,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsHash_t
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -823,7 +762,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
@@ -1407,7 +1345,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
#ifndef Py_MEMBER_SIZE
#define Py_MEMBER_SIZE(type, member) sizeof(((type *)0)->member)
#endif
#if CYTHON_FAST_PYCALL
static size_t __pyx_pyframe_localsplus_offset = 0;
#include "frameobject.h"
#define __Pxy_PyFrame_Initialize_Offsets()\
@@ -1415,7 +1352,6 @@ static PyObject *__Pyx_PyFunction_FastCallDict(PyObject *func, PyObject **args,
(void)(__pyx_pyframe_localsplus_offset = ((size_t)PyFrame_Type.tp_basicsize) - Py_MEMBER_SIZE(PyFrameObject, f_localsplus)))
#define __Pyx_PyFrame_GetLocalsplus(frame)\
(assert(__pyx_pyframe_localsplus_offset), (PyObject **)(((char *)(frame)) + __pyx_pyframe_localsplus_offset))
#endif // CYTHON_FAST_PYCALL
#endif
/* PyObjectCall.proto */
@@ -6342,9 +6278,6 @@ static PyTypeObject __pyx_type_6common_15transformations_15transformations_Local
#if PY_VERSION_HEX >= 0x030800b4 && PY_VERSION_HEX < 0x03090000
0, /*tp_print*/
#endif
#if CYTHON_COMPILING_IN_PYPY && PYPY_VERSION_NUM+0 >= 0x06000000
0, /*tp_pypy_flags*/
#endif
};
static PyMethodDef __pyx_methods[] = {
@@ -9109,7 +9042,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
}
if (!use_cline) {
c_line = 0;
(void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
}
else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
c_line = 0;
@@ -9206,31 +9139,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
const char *funcname, int c_line,
int py_line, const char *filename) {
PyCodeObject *py_code = NULL;
PyObject *py_funcname = NULL;
PyCodeObject *py_code = 0;
PyObject *py_srcfile = 0;
PyObject *py_funcname = 0;
#if PY_MAJOR_VERSION < 3
PyObject *py_srcfile = NULL;
py_srcfile = PyString_FromString(filename);
if (!py_srcfile) goto bad;
#else
py_srcfile = PyUnicode_FromString(filename);
#endif
if (!py_srcfile) goto bad;
if (c_line) {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
funcname = PyUnicode_AsUTF8(py_funcname);
if (!funcname) goto bad;
#endif
}
else {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromString(funcname);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromString(funcname);
#endif
}
#if PY_MAJOR_VERSION < 3
if (!py_funcname) goto bad;
py_code = __Pyx_PyCode_New(
0,
0,
@@ -9249,16 +9181,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
__pyx_empty_bytes /*PyObject *lnotab*/
);
Py_DECREF(py_srcfile);
#else
py_code = PyCode_NewEmpty(filename, funcname, py_line);
#endif
Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline
Py_DECREF(py_funcname);
return py_code;
bad:
Py_XDECREF(py_funcname);
#if PY_MAJOR_VERSION < 3
Py_XDECREF(py_srcfile);
#endif
Py_XDECREF(py_funcname);
return NULL;
}
static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -10434,23 +10361,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
Py_DECREF(x);
return ival;
}
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
#if PY_MAJOR_VERSION < 3
} else if (likely(PyInt_CheckExact(o))) {
return PyInt_AS_LONG(o);
#endif
} else {
Py_ssize_t ival;
PyObject *x;
x = PyNumber_Index(o);
if (!x) return -1;
ival = PyInt_AsLong(x);
Py_DECREF(x);
return ival;
}
}
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
}
common/transformations/transformations.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2022.12.30"
#define COMMA_VERSION "2023.02.07"
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.testing.signed
BIN
View File
Binary file not shown.
panda/python/__init__.py
+4
View File
@@ -206,11 +206,13 @@ class Panda:
# first byte is for EPS scaling factor
FLAG_TOYOTA_ALT_BRAKE = (1 << 8)
FLAG_TOYOTA_STOCK_LONGITUDINAL = (2 << 8)
FLAG_TOYOTA_ALKA = (4 << 8)
FLAG_HONDA_ALT_BRAKE = 1
FLAG_HONDA_BOSCH_LONG = 2
FLAG_HONDA_NIDEC_ALT = 4
FLAG_HONDA_RADARLESS = 8
FLAG_HONDA_ALKA = 16
FLAG_HYUNDAI_EV_GAS = 1
FLAG_HYUNDAI_HYBRID_GAS = 2
@@ -219,11 +221,13 @@ class Panda:
FLAG_HYUNDAI_CANFD_HDA2 = 16
FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
FLAG_HYUNDAI_ALT_LIMITS = 64
FLAG_HYUNDAI_ALKA = 128
FLAG_TESLA_POWERTRAIN = 1
FLAG_TESLA_LONG_CONTROL = 2
FLAG_VOLKSWAGEN_LONG_CONTROL = 1
FLAG_VOLKSWAGEN_ALKA = 2
FLAG_CHRYSLER_RAM_DT = 1
FLAG_CHRYSLER_RAM_HD = 2
panda/python/config.py
-23
View File
@@ -9,29 +9,6 @@ APP_ADDRESS_FX = 0x8004000
SECTOR_SIZES_FX = [0x4000 for _ in range(4)] + [0x10000] + [0x20000 for _ in range(11)]
DEVICE_SERIAL_NUMBER_ADDR_FX = 0x1FFF79C0
DEFAULT_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
DEFAULT_SSPOOF_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.sspoof.signed")
TESTING_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.testing.signed")
TESTING_SSPOOF_FW_FN = os.path.join(BASEDIR, "board", "obj", "panda.bin.testing.sspoof.signed")
atl_enabled = False
if os.path.exists('/data/params/d/dp_atl'):
with open('/data/params/d/dp_atl') as f:
if (int(f.read().strip())) != 0:
atl_enabled = True
sspoof_enabled = False
# if os.path.exists('/data/params/d/dp_sspoof'):
# with open('/data/params/d/dp_sspoof') as f:
# if (int(f.read().strip())) != 0:
# sspoof_enabled = True
if atl_enabled and sspoof_enabled and os.path.exists(TESTING_SSPOOF_FW_FN):
DEFAULT_FW_FN = TESTING_SSPOOF_FW_FN
elif atl_enabled and not sspoof_enabled and os.path.exists(TESTING_FW_FN):
DEFAULT_FW_FN = TESTING_FW_FN
elif not atl_enabled and sspoof_enabled and os.path.exists(DEFAULT_SSPOOF_FW_FN):
DEFAULT_FW_FN = DEFAULT_SSPOOF_FW_FN
DEFAULT_BOOTSTUB_FN = os.path.join(BASEDIR, "board", "obj", "bootstub.panda.bin")
BLOCK_SIZE_H7 = 0x400
rednose/helpers/ekf_sym_pyx.so
BIN
View File
Binary file not shown.
scripts/panda_recovery.sh
+1 -30
View File
@@ -3,42 +3,13 @@
DFU_UTIL="dfu-util"
DEFAULT_FW_FN="/data/openpilot/panda/board/obj/panda.bin.signed"
DEFAULT_SSPOOF_FW_FN="/data/openpilot/panda/board/obj/panda.bin.sspoof.signed"
TESTING_FW_FN="/data/openpilot/panda/board/obj/panda.bin.testing.signed"
TESTING_SSPOOF_FW_FN="/data/openpilot/panda/board/obj/panda.bin.testing.sspoof.signed"
ATL_ENABLED=0
ATL=`cat /data/params/d/dp_atl`
if [ -f /data/params/d/dp_atl ] && [ $ATL != "0" ]; then
ATL_ENABLED=1
fi
SSPOOF_ENABLED=0
#SSPOOF=`cat /data/params/d/dp_sspoof`
#if [ -f /data/params/d/dp_sspoof ] && [ $SSPOOF != "0" ]; then
# SSPOOF_ENABLED=1
#fi
FW=$DEFAULT_FW_FN
if [ "$ATL_ENABLED" = "1" ] && [ "$SSPOOF_ENABLED" = "1" ] && [ -f $TESTING_SSPOOF_FW_FN ]; then
echo "Use testing + sspoof firmware..."
FW=$TESTING_SSPOOF_FW_FN
elif [ "$ATL_ENABLED" = "1" ] && [ "$SSPOOF_ENABLED" = "0" ] && [ -f $TESTING_FW_FN ]; then
echo "Use testing firmware..."
FW=$TESTING_FW_FN
elif [ "$ATL_ENABLED" = "0" ] && [ "$SSPOOF_ENABLED" = "1" ] && [ -f $DEFAULT_SSPOOF_FW_FN ]; then
echo "Use sspoof firmware..."
FW=$DEFAULT_SSPOOF_FW_FN
else
echo "Use stock firmware..."
fi
/data/openpilot/selfdrive/ui/qt/spinner &
pkill -f boardd
python -c "from panda import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true
sleep 1
echo "\n\n\nUpdating panda.bin..."
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D $FW
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D $DEFAULT_FW_FN
echo "\n\n\nUpdating bootstub.panda.bin..."
$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D /data/openpilot/panda/board/obj/bootstub.panda.bin
sleep 1
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd_api_impl.cpp
+23 -108
View File
@@ -1,4 +1,4 @@
/* Generated by Cython 0.29.26 */
/* Generated by Cython 0.29.24 */
/* BEGIN: Cython Metadata
{
@@ -23,8 +23,8 @@ END: Cython Metadata */
#elif PY_VERSION_HEX < 0x02060000 || (0x03000000 <= PY_VERSION_HEX && PY_VERSION_HEX < 0x03030000)
#error Cython requires Python 2.6+ or Python 3.3+.
#else
#define CYTHON_ABI "0_29_26"
#define CYTHON_HEX_VERSION 0x001D1AF0
#define CYTHON_ABI "0_29_24"
#define CYTHON_HEX_VERSION 0x001D18F0
#define CYTHON_FUTURE_DIVISION 1
#include <stddef.h>
#ifndef offsetof
@@ -171,7 +171,7 @@ END: Cython Metadata */
#ifndef CYTHON_USE_UNICODE_INTERNALS
#define CYTHON_USE_UNICODE_INTERNALS 1
#endif
#if PY_VERSION_HEX < 0x030300F0 || PY_VERSION_HEX >= 0x030B00A2
#if PY_VERSION_HEX < 0x030300F0
#undef CYTHON_USE_UNICODE_WRITER
#define CYTHON_USE_UNICODE_WRITER 0
#elif !defined(CYTHON_USE_UNICODE_WRITER)
@@ -190,7 +190,7 @@ END: Cython Metadata */
#define CYTHON_FAST_THREAD_STATE 1
#endif
#ifndef CYTHON_FAST_PYCALL
#define CYTHON_FAST_PYCALL (PY_VERSION_HEX < 0x030B00A1)
#define CYTHON_FAST_PYCALL 1
#endif
#ifndef CYTHON_PEP489_MULTI_PHASE_INIT
#define CYTHON_PEP489_MULTI_PHASE_INIT (PY_VERSION_HEX >= 0x03050000)
@@ -209,9 +209,7 @@ END: Cython Metadata */
#define CYTHON_FAST_PYCCALL (CYTHON_FAST_PYCALL && PY_VERSION_HEX >= 0x030600B1)
#endif
#if CYTHON_USE_PYLONG_INTERNALS
#if PY_MAJOR_VERSION < 3
#include "longintrepr.h"
#endif
#include "longintrepr.h"
#undef SHIFT
#undef BASE
#undef MASK
@@ -342,68 +340,9 @@ class __Pyx_FakeReference {
#define __Pyx_DefaultClassType PyClass_Type
#else
#define __Pyx_BUILTIN_MODULE_NAME "builtins"
#define __Pyx_DefaultClassType PyType_Type
#if PY_VERSION_HEX >= 0x030B00A1
static CYTHON_INLINE PyCodeObject* __Pyx_PyCode_New(int a, int k, int l, int s, int f,
PyObject *code, PyObject *c, PyObject* n, PyObject *v,
PyObject *fv, PyObject *cell, PyObject* fn,
PyObject *name, int fline, PyObject *lnos) {
PyObject *kwds=NULL, *argcount=NULL, *posonlyargcount=NULL, *kwonlyargcount=NULL;
PyObject *nlocals=NULL, *stacksize=NULL, *flags=NULL, *replace=NULL, *call_result=NULL, *empty=NULL;
const char *fn_cstr=NULL;
const char *name_cstr=NULL;
PyCodeObject* co=NULL;
PyObject *type, *value, *traceback;
PyErr_Fetch(&type, &value, &traceback);
if (!(kwds=PyDict_New())) goto end;
if (!(argcount=PyLong_FromLong(a))) goto end;
if (PyDict_SetItemString(kwds, "co_argcount", argcount) != 0) goto end;
if (!(posonlyargcount=PyLong_FromLong(0))) goto end;
if (PyDict_SetItemString(kwds, "co_posonlyargcount", posonlyargcount) != 0) goto end;
if (!(kwonlyargcount=PyLong_FromLong(k))) goto end;
if (PyDict_SetItemString(kwds, "co_kwonlyargcount", kwonlyargcount) != 0) goto end;
if (!(nlocals=PyLong_FromLong(l))) goto end;
if (PyDict_SetItemString(kwds, "co_nlocals", nlocals) != 0) goto end;
if (!(stacksize=PyLong_FromLong(s))) goto end;
if (PyDict_SetItemString(kwds, "co_stacksize", stacksize) != 0) goto end;
if (!(flags=PyLong_FromLong(f))) goto end;
if (PyDict_SetItemString(kwds, "co_flags", flags) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_code", code) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_consts", c) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_names", n) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_varnames", v) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_freevars", fv) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_cellvars", cell) != 0) goto end;
if (PyDict_SetItemString(kwds, "co_linetable", lnos) != 0) goto end;
if (!(fn_cstr=PyUnicode_AsUTF8AndSize(fn, NULL))) goto end;
if (!(name_cstr=PyUnicode_AsUTF8AndSize(name, NULL))) goto end;
if (!(co = PyCode_NewEmpty(fn_cstr, name_cstr, fline))) goto end;
if (!(replace = PyObject_GetAttrString((PyObject*)co, "replace"))) goto cleanup_code_too;
if (!(empty = PyTuple_New(0))) goto cleanup_code_too; // unfortunately __pyx_empty_tuple isn't available here
if (!(call_result = PyObject_Call(replace, empty, kwds))) goto cleanup_code_too;
Py_XDECREF((PyObject*)co);
co = (PyCodeObject*)call_result;
call_result = NULL;
if (0) {
cleanup_code_too:
Py_XDECREF((PyObject*)co);
co = NULL;
}
end:
Py_XDECREF(kwds);
Py_XDECREF(argcount);
Py_XDECREF(posonlyargcount);
Py_XDECREF(kwonlyargcount);
Py_XDECREF(nlocals);
Py_XDECREF(stacksize);
Py_XDECREF(replace);
Py_XDECREF(call_result);
Py_XDECREF(empty);
if (type) {
PyErr_Restore(type, value, traceback);
}
return co;
}
#if PY_VERSION_HEX >= 0x030800A4 && PY_VERSION_HEX < 0x030800B2
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, 0, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
#else
#define __Pyx_PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)\
PyCode_New(a, k, l, s, f, code, c, n, v, fv, cell, fn, name, fline, lnos)
@@ -641,10 +580,10 @@ static CYTHON_INLINE void * PyThread_tss_get(Py_tss_t *key) {
#if PY_VERSION_HEX < 0x030200A4
typedef long Py_hash_t;
#define __Pyx_PyInt_FromHash_t PyInt_FromLong
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsHash_t
#define __Pyx_PyInt_AsHash_t PyInt_AsLong
#else
#define __Pyx_PyInt_FromHash_t PyInt_FromSsize_t
#define __Pyx_PyInt_AsHash_t __Pyx_PyIndex_AsSsize_t
#define __Pyx_PyInt_AsHash_t PyInt_AsSsize_t
#endif
#if PY_MAJOR_VERSION >= 3
#define __Pyx_PyMethod_New(func, self, klass) ((self) ? ((void)(klass), PyMethod_New(func, self)) : __Pyx_NewRef(func))
@@ -807,7 +746,6 @@ static CYTHON_INLINE PyObject* __Pyx_PyNumber_IntOrLong(PyObject* x);
(likely(PyTuple_CheckExact(obj)) ? __Pyx_NewRef(obj) : PySequence_Tuple(obj))
static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject*);
static CYTHON_INLINE PyObject * __Pyx_PyInt_FromSize_t(size_t);
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*);
#if CYTHON_ASSUME_SAFE_MACROS
#define __pyx_PyFloat_AsDouble(x) (PyFloat_CheckExact(x) ? PyFloat_AS_DOUBLE(x) : PyFloat_AsDouble(x))
#else
@@ -2630,7 +2568,7 @@ static int __Pyx_CLineForTraceback(CYTHON_NCP_UNUSED PyThreadState *tstate, int
}
if (!use_cline) {
c_line = 0;
(void) PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
PyObject_SetAttr(__pyx_cython_runtime, __pyx_n_s_cline_in_traceback, Py_False);
}
else if (use_cline == Py_False || (use_cline != Py_True && PyObject_Not(use_cline) != 0)) {
c_line = 0;
@@ -2727,31 +2665,30 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object) {
static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
const char *funcname, int c_line,
int py_line, const char *filename) {
PyCodeObject *py_code = NULL;
PyObject *py_funcname = NULL;
PyCodeObject *py_code = 0;
PyObject *py_srcfile = 0;
PyObject *py_funcname = 0;
#if PY_MAJOR_VERSION < 3
PyObject *py_srcfile = NULL;
py_srcfile = PyString_FromString(filename);
if (!py_srcfile) goto bad;
#else
py_srcfile = PyUnicode_FromString(filename);
#endif
if (!py_srcfile) goto bad;
if (c_line) {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromFormat( "%s (%s:%d)", funcname, __pyx_cfilenm, c_line);
if (!py_funcname) goto bad;
funcname = PyUnicode_AsUTF8(py_funcname);
if (!funcname) goto bad;
#endif
}
else {
#if PY_MAJOR_VERSION < 3
py_funcname = PyString_FromString(funcname);
if (!py_funcname) goto bad;
#else
py_funcname = PyUnicode_FromString(funcname);
#endif
}
#if PY_MAJOR_VERSION < 3
if (!py_funcname) goto bad;
py_code = __Pyx_PyCode_New(
0,
0,
@@ -2770,16 +2707,11 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback(
__pyx_empty_bytes /*PyObject *lnotab*/
);
Py_DECREF(py_srcfile);
#else
py_code = PyCode_NewEmpty(filename, funcname, py_line);
#endif
Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline
Py_DECREF(py_funcname);
return py_code;
bad:
Py_XDECREF(py_funcname);
#if PY_MAJOR_VERSION < 3
Py_XDECREF(py_srcfile);
#endif
Py_XDECREF(py_funcname);
return NULL;
}
static void __Pyx_AddTraceback(const char *funcname, int c_line,
@@ -3627,23 +3559,6 @@ static CYTHON_INLINE Py_ssize_t __Pyx_PyIndex_AsSsize_t(PyObject* b) {
Py_DECREF(x);
return ival;
}
static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject* o) {
if (sizeof(Py_hash_t) == sizeof(Py_ssize_t)) {
return (Py_hash_t) __Pyx_PyIndex_AsSsize_t(o);
#if PY_MAJOR_VERSION < 3
} else if (likely(PyInt_CheckExact(o))) {
return PyInt_AS_LONG(o);
#endif
} else {
Py_ssize_t ival;
PyObject *x;
x = PyNumber_Index(o);
if (!x) return -1;
ival = PyInt_AsLong(x);
Py_DECREF(x);
return ival;
}
}
static CYTHON_INLINE PyObject * __Pyx_PyBool_FromLong(long b) {
return b ? __Pyx_NewRef(Py_True) : __Pyx_NewRef(Py_False);
}
selfdrive/boardd/boardd_api_impl.so
BIN
View File
Binary file not shown.
selfdrive/car/chrysler/interface.py
+1 -3
View File
@@ -74,7 +74,7 @@ class CarInterface(CarInterfaceBase):
raise ValueError(f"Unsupported car: {candidate}")
CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning)
if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
# TODO: allow these cars to steer down to 13 m/s if already engaged.
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
@@ -86,11 +86,9 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])
events = self.dp_atl_warning(ret, events)
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 0.5):
selfdrive/car/ford/interface.py
-2
View File
@@ -61,10 +61,8 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
events = self.dp_atl_warning(ret, events)
ret.events = events.to_msg()
return ret
selfdrive/car/gm/interface.py
-2
View File
@@ -206,7 +206,6 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
buttonEvents = [create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS)]
@@ -220,7 +219,6 @@ class CarInterface(CarInterfaceBase):
events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low,
GearShifter.eco, GearShifter.manumatic],
pcm_enable=self.CP.pcmCruise, enable_buttons=(ButtonType.decelCruise,))
events = self.dp_atl_warning(ret, events)
if not self.CP.pcmCruise:
if any(b.type == ButtonType.accelCruise and b.pressed for b in ret.buttonEvents):
events.add(EventName.buttonEnable)
selfdrive/car/honda/interface.py
+8 -6
View File
@@ -53,10 +53,14 @@ class CarInterface(CarInterfaceBase):
# dp - attempt to disable op long
params = Params()
if int(params.get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
if candidate in HONDA_BOSCH:
ret.pcmCruise = not ret.openpilotLongitudinalControl
dp_atl = int(params.get("dp_atl").decode('utf-8'))
if dp_atl > 0:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALKA
if dp_atl == 1:
ret.openpilotLongitudinalControl = False
# update pcmCruise again
if candidate in HONDA_BOSCH:
ret.pcmCruise = not ret.openpilotLongitudinalControl
if candidate == CAR.CRV_5G:
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
@@ -335,7 +339,6 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
#dp
ret.engineRPM = self.CS.engineRPM
@@ -352,7 +355,6 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret, pcm_enable=False)
events = self.dp_atl_warning(ret, events)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
selfdrive/car/hyundai/carcontroller.py
+3 -3
View File
@@ -113,7 +113,7 @@ class CarController:
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.enabled, lat_active, apply_steer))
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, CC.latActive, lat_active, apply_steer))
# disable LFA on HDA2
if self.frame % 5 == 0 and hda2:
@@ -121,7 +121,7 @@ class CarController:
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.latActive))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -158,7 +158,7 @@ class CarController:
self.last_button_frame = self.frame
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, lat_active,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.latActive,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
selfdrive/car/hyundai/interface.py
+9 -6
View File
@@ -227,7 +227,15 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kpV = [0.5]
ret.longitudinalTuning.kiV = [0.0]
ret.experimentalLongitudinalAvailable = candidate not in (LEGACY_SAFETY_MODE_CAR | CAMERA_SCC_CAR)
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable
params = Params()
dp_atl = int(params.get("dp_atl").decode('utf-8'))
if dp_atl > 0:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_ALKA
if dp_atl == 1:
ret.openpilotLongitudinalControl = False
ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable and dp_atl != 1
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stoppingControl = True
@@ -285,9 +293,6 @@ class CarInterface(CarInterfaceBase):
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
params = Params()
if int(params.get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
params.put("dp_lateral_steer_rate_cost", "0.5")
return ret
@@ -305,7 +310,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
if self.CS.CP.openpilotLongitudinalControl and self.CS.cruise_buttons[-1] != self.CS.prev_cruise_buttons:
buttonEvents = [create_button_event(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)]
@@ -320,7 +324,6 @@ class CarInterface(CarInterfaceBase):
# Main button also can trigger an engagement on these cars
allow_enable = any(btn in ENABLE_BUTTONS for btn in self.CS.cruise_buttons) or any(self.CS.main_buttons)
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise, allow_enable=allow_enable)
events = self.dp_atl_warning(ret, events)
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
selfdrive/car/interfaces.py
+6 -29
View File
@@ -267,12 +267,12 @@ class CarInterfaceBase(ABC):
events.add(EventName.doorOpen)
if cs_out.seatbeltUnlatched:
events.add(EventName.seatbeltNotLatched)
if self.dragonconf.dpAtl != 1 and cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
if cs_out.gearShifter != GearShifter.drive and (extra_gears is None or
cs_out.gearShifter not in extra_gears):
events.add(EventName.wrongGear)
if cs_out.gearShifter == GearShifter.reverse:
events.add(EventName.reverseGear)
if self.dragonconf.dpAtl == 0 and not cs_out.cruiseState.available:
if not cs_out.cruiseState.available:
events.add(EventName.wrongCarMode)
if cs_out.espDisabled:
events.add(EventName.espDisabled)
@@ -282,13 +282,13 @@ class CarInterfaceBase(ABC):
events.add(EventName.stockAeb)
if self.dragonconf.dpSpeedCheck and cs_out.vEgo > MAX_CTRL_SPEED:
events.add(EventName.speedTooHigh)
if self.dragonconf.dpAtl != 1 and cs_out.cruiseState.nonAdaptive:
if cs_out.cruiseState.nonAdaptive:
events.add(EventName.wrongCruiseMode)
if self.dragonconf.dpAtl != 1 and cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
if cs_out.brakeHoldActive and self.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if self.dragonconf.dpAtl != 1 and cs_out.parkingBrake:
if cs_out.parkingBrake:
events.add(EventName.parkBrake)
if self.dragonconf.dpAtl != 1 and cs_out.accFaulted:
if cs_out.accFaulted:
events.add(EventName.accFaulted)
if cs_out.steeringPressed:
events.add(EventName.steerOverride)
@@ -326,29 +326,6 @@ class CarInterfaceBase(ABC):
return events
def dp_atl_warning(self, ret, events):
if self.dragonconf.dpAtl > 0:
if self.dp_last_cruise_actual_enabled and not ret.cruiseActualEnabled:
events.add(EventName.communityFeatureDisallowedDEPRECATED)
elif ret.cruiseState.enabled != ret.cruiseActualEnabled:
events.add(EventName.gasPressedOverride)
self.dp_last_cruise_actual_enabled = ret.cruiseActualEnabled
return events
def dp_atl_mode(self, ret):
enable = ret.cruiseState.enabled
available = ret.cruiseState.available
if self.dragonconf.dpAtl > 0 and available:
enable = True
if ret.gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park]:
enable = False
available = False
if ret.seatbeltUnlatched or ret.doorOpen:
enable = False
available = False
return enable, available
class RadarInterfaceBase(ABC):
def __init__(self, CP):
self.rcp = None
selfdrive/car/mazda/interface.py
-2
View File
@@ -59,11 +59,9 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
# events
events = self.create_common_events(ret)
events = self.dp_atl_warning(ret, events)
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)
selfdrive/car/nissan/interface.py
-2
View File
@@ -43,7 +43,6 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
buttonEvents = []
be = car.CarState.ButtonEvent.new_message()
@@ -51,7 +50,6 @@ class CarInterface(CarInterfaceBase):
buttonEvents.append(be)
events = self.create_common_events(ret)
events = self.dp_atl_warning(ret, events)
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
selfdrive/car/subaru/interface.py
-2
View File
@@ -110,12 +110,10 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
ret.events = self.create_common_events(ret).to_msg()
events = self.create_common_events(ret)
events = self.dp_atl_warning(ret, events)
ret.events = events.to_msg()
selfdrive/car/toyota/carcontroller.py
+1 -3
View File
@@ -43,7 +43,6 @@ class CarController:
# dp
self.dp_toyota_sng = False
self.dp_atl = 0
self.dp_toyota_auto_lock = False
self.dp_toyota_auto_unlock = False
@@ -53,7 +52,6 @@ class CarController:
def update(self, CC, CS, dragonconf):
if dragonconf is not None:
self.dp_toyota_sng = dragonconf.dpToyotaSng
self.dp_atl = dragonconf.dpAtl
self.dp_toyota_auto_lock = dragonconf.dpToyotaAutoLock
self.dp_toyota_auto_unlock = dragonconf.dpToyotaAutoUnlock
actuators = CC.actuators
@@ -184,7 +182,7 @@ class CarController:
if self.frame % 100 == 0 or send_ui:
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud))
if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
can_sends.append(create_fcw_command(self.packer, fcw_alert))
selfdrive/car/toyota/interface.py
+8 -8
View File
@@ -222,11 +222,15 @@ class CarInterface(CarInterfaceBase):
ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR)
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
if int(params.get("dp_atl").decode('utf-8')) == 1:
ret.openpilotLongitudinalControl = False
dp_atl = int(Params().get("dp_atl").decode('utf-8'))
if dp_atl > 0:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALKA
if smartDsu and int(params.get("dp_atl").decode('utf-8')) == 2:
ret.openpilotLongitudinalControl = True
if dp_atl == 1:
ret.openpilotLongitudinalControl = False
if smartDsu and dp_atl == 2:
ret.openpilotLongitudinalControl = True
if candidate == CAR.CHR_TSS2:
ret.enableBsm = True
@@ -281,9 +285,6 @@ class CarInterface(CarInterfaceBase):
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# dp
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
# low speed re-write
if self.dragonconf.dpToyotaCruiseOverride:
if self.dragonconf.dpToyotaCruiseOverrideSpeed != self.dp_override_speed_last:
@@ -299,7 +300,6 @@ class CarInterface(CarInterfaceBase):
# events
events = self.create_common_events(ret)
events = self.dp_atl_warning(ret, events)
if self.CP.openpilotLongitudinalControl:
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
selfdrive/car/volkswagen/carcontroller.py
+13 -7
View File
@@ -5,7 +5,7 @@ from common.conversions import Conversions as CV
from common.realtime import DT_CTRL
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.volkswagen import mqbcan, pqcan
from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams, STANDING_RESUME_SPAM_CARS
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -85,7 +85,7 @@ class CarController:
hud_alert = 0
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.latActive,
CS.out.steeringPressed, hud_alert, hud_control))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
@@ -99,11 +99,17 @@ class CarController:
# **** Stock ACC Button Controls **************************************** #
gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
counter = (CS.gra_stock_values["COUNTER"] + 1) % 16
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter,
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
if self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last: # send just after stock
standing_resume_spam = CS.out.cruiseState.standstill and self.CP.carFingerprint in STANDING_RESUME_SPAM_CARS
spam_window = self.frame % 50 < 25 # 0.25 second gap between virtual button presses
press_cancel = CC.cruiseControl.cancel
press_resume = CC.cruiseControl.resume or (standing_resume_spam and spam_window)
if press_cancel or press_resume:
counter = (CS.gra_stock_values["COUNTER"] + 1) % 16
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, counter,
cancel=press_cancel, resume=press_resume))
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
selfdrive/car/volkswagen/interface.py
+8 -3
View File
@@ -4,6 +4,7 @@ from common.conversions import Conversions as CV
from selfdrive.car import STD_CARGO_KG, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
from common.params import Params
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
@@ -80,8 +81,14 @@ class CarInterface(CarInterfaceBase):
# Global longitudinal tuning defaults, can be overridden per-vehicle
dp_atl = int(Params().get("dp_atl").decode('utf-8'))
if dp_atl > 0:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_ALKA
if dp_atl == 1:
ret.openpilotLongitudinalControl = False
ret.experimentalLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or use_off_car_defaults
if experimental_long:
if experimental_long and dp_atl != 1:
# Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
@@ -218,12 +225,10 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret)
events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic],
pcm_enable=not self.CS.CP.openpilotLongitudinalControl,
enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise))
events = self.dp_atl_warning(ret, events)
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
selfdrive/car/volkswagen/values.py
+7
View File
@@ -136,8 +136,15 @@ class CAR:
SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants
# CARs based on the PQ35/PQ46/NMS platforms and using PQ-style CAN messaging (default is MQB)
PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2}
# CARs that benefit from continuous Resume spam at a stop, effectively upgrading stock FtS to SnG
# Appears to be MQB-A0s and the Transporter family
# TODO: try to determine this from ABS firmware instead
STANDING_RESUME_SPAM_CARS = {CAR.POLO_MK6, CAR.TCROSS_MK1, CAR.TROC_MK1, CAR.SKODA_KAMIQ_MK1,
CAR.SKODA_SCALA_MK1, CAR.TRANSPORTER_T61}
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None))
for car_type in PQ_CARS:
selfdrive/controls/controlsd.py
+24 -16
View File
@@ -121,8 +121,6 @@ class Controls:
# set alternative experiences from parameters
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
if self.sm['dragonConf'].dpAtl > 0:
self.disengage_on_accelerator = False
self.CP.alternativeExperience = 0
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
@@ -258,13 +256,13 @@ class Controls:
# Block resume if cruise never previously enabled
resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
if self.sm['dragonConf'].dpAtl == 0 and not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
self.events.add(EventName.resumeBlocked)
# Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
if self.sm['dragonConf'].dpAtl == 0 and ((CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
(CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill))):
(CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
self.events.add(EventName.pedalPressed)
if CS.brakePressed and CS.standstill:
@@ -402,7 +400,7 @@ class Controls:
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if self.sm['dragonConf'].dpAtl == 0 and not REPLAY:
if not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
@@ -549,8 +547,6 @@ class Controls:
# DISABLED
elif self.state == State.disabled:
if CS.cruiseState.available and not CS.cruiseActualEnabled and self.sm['dragonConf'].dpAtl > 0 and not self.events.any(ET.NO_ENTRY):
self.state = State.overriding
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
@@ -596,8 +592,23 @@ class Controls:
CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if self.sm['dragonConf'].dpAtl == 2 and not CS.cruiseActualEnabled:
CC.longActive = False
if self.sm['dragonConf'].dpAtl > 0:
if not CS.cruiseState.available:
pass
# CC.latActive = False
elif CS.steerFaultTemporary:
pass
# CC.latActive = False
elif CS.steerFaultPermanent:
pass
# CC.latActive = False
elif CS.standstill:
pass
# CC.latActive = False
elif CS.gearShifter == car.CarState.GearShifter.reverse:
pass
else:
CC.latActive = True
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
@@ -701,12 +712,9 @@ class Controls:
if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
if self.dp_vag_resume_fix or self.sm['dragonConf'].dpAtl == 1:
CC.cruiseControl.resume = CS.cruiseActualEnabled and CS.cruiseState.standstill
else:
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds):
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
speeds = self.sm['longitudinalPlan'].speeds
if len(speeds):
CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
hudControl = CC.hudControl
hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS)
selfdrive/controls/lib/events.py
+8 -7
View File
@@ -195,7 +195,7 @@ class NormalPermanentAlert(Alert):
class StartupAlert(Alert):
def __init__(self, alert_text_1: str, alert_text_2: str = _("Happy Holiday / New Years! Safe Travel, cya in 2023"), alert_status=AlertStatus.normal):
def __init__(self, alert_text_1: str, alert_text_2: str = _("Always keep hands on wheel and eyes on road"), alert_status=AlertStatus.normal):
super().__init__(alert_text_1, alert_text_2,
alert_status, AlertSize.mid,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, 10.),
@@ -449,7 +449,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.steerTempUnavailableSilent: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Steering Temporarily Unavailable"),
"",
AlertStatus.userPrompt, AlertSize.small,
@@ -525,7 +525,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.preLaneChangeLeft: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Steer Left to Start Lane Change Once Safe"),
"",
AlertStatus.normal, AlertSize.small,
@@ -533,7 +533,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.preLaneChangeRight: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Steer Right to Start Lane Change Once Safe"),
"",
AlertStatus.normal, AlertSize.small,
@@ -541,7 +541,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.laneChangeBlocked: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Car Detected in Blindspot"),
"",
AlertStatus.userPrompt, AlertSize.small,
@@ -549,7 +549,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.laneChange: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Changing Lanes"),
"",
AlertStatus.normal, AlertSize.small,
@@ -557,7 +557,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.steerSaturated: {
ET.WARNING: Alert(
ET.PERMANENT: Alert(
_("Take Control"),
_("Turn Exceeds Steering Limit"),
AlertStatus.userPrompt, AlertSize.mid,
@@ -680,6 +680,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
},
EventName.steerTempUnavailable: {
ET.PERMANENT: soft_disable_alert(_("Steering Temporarily Unavailable")),
ET.SOFT_DISABLE: soft_disable_alert(_("Steering Temporarily Unavailable")),
ET.NO_ENTRY: NoEntryAlert(_("Steering Temporarily Unavailable")),
},
selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json
+544
View File
@@ -0,0 +1,544 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 16,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 5,
"ny_0": 5,
"ny_e": 3,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 2.5,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375
]
}
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* 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.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 5);
// y_ref
ssSetInputPortVectorDimension(S, 4, 75);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 3);
// lbx
ssSetInputPortVectorDimension(S, 6, 30);
// ubx
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[5];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 5; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 5; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% 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.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [75]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/acados_ocp_lat.json
+450
View File
@@ -0,0 +1,450 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0
],
[
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 16,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 3,
"ny_0": 3,
"ny_e": 2,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 2.5,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375
]
}
}
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* 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.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 3);
// y_ref
ssSetInputPortVectorDimension(S, 4, 45);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 2);
// lbx
ssSetInputPortVectorDimension(S, 6, 30);
// ubx
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[4];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 3; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 2; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/dragonpilot/controls_0813/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% 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.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [45]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/acados_ocp_lat.json
+450
View File
@@ -0,0 +1,450 @@
{
"acados_include_path": "/data/openpilot/third_party/acados/include/acados/include",
"acados_lib_path": "/data/openpilot/third_party/acados/include/acados/lib",
"code_export_directory": "/data/openpilot/selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code",
"constraints": {
"C": [],
"C_e": [],
"D": [],
"constr_type": "BGH",
"constr_type_e": "BGH",
"idxbu": [],
"idxbx": [
2,
3
],
"idxbx_0": [
0,
1,
2,
3
],
"idxbx_e": [],
"idxbxe_0": [
0,
1,
2,
3
],
"idxsbu": [],
"idxsbx": [],
"idxsbx_e": [],
"idxsg": [],
"idxsg_e": [],
"idxsh": [],
"idxsh_e": [],
"idxsphi": [],
"idxsphi_e": [],
"lbu": [],
"lbx": [
-1.5707963267948966,
-0.8726646259971648
],
"lbx_0": [
0.0,
0.0,
0.0,
0.0
],
"lbx_e": [],
"lg": [],
"lg_e": [],
"lh": [],
"lh_e": [],
"lphi": [],
"lphi_e": [],
"lsbu": [],
"lsbx": [],
"lsbx_e": [],
"lsg": [],
"lsg_e": [],
"lsh": [],
"lsh_e": [],
"lsphi": [],
"lsphi_e": [],
"ubu": [],
"ubx": [
1.5707963267948966,
0.8726646259971648
],
"ubx_0": [
0.0,
0.0,
0.0,
0.0
],
"ubx_e": [],
"ug": [],
"ug_e": [],
"uh": [],
"uh_e": [],
"uphi": [],
"uphi_e": [],
"usbu": [],
"usbx": [],
"usbx_e": [],
"usg": [],
"usg_e": [],
"ush": [],
"ush_e": [],
"usphi": [],
"usphi_e": []
},
"cost": {
"Vu": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vu_0": [
[
0.0
],
[
0.0
],
[
0.0
]
],
"Vx": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_0": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vx_e": [
[
0.0,
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0,
0.0
]
],
"Vz": [],
"Vz_0": [],
"W": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_0": [
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
],
[
0.0,
0.0,
0.0
]
],
"W_e": [
[
0.0,
0.0
],
[
0.0,
0.0
]
],
"Zl": [],
"Zl_e": [],
"Zu": [],
"Zu_e": [],
"cost_ext_fun_type": "casadi",
"cost_ext_fun_type_0": "casadi",
"cost_ext_fun_type_e": "casadi",
"cost_type": "NONLINEAR_LS",
"cost_type_0": "NONLINEAR_LS",
"cost_type_e": "NONLINEAR_LS",
"yref": [
0.0,
0.0,
0.0
],
"yref_0": [
0.0,
0.0,
0.0
],
"yref_e": [
0.0,
0.0
],
"zl": [],
"zl_e": [],
"zu": [],
"zu_e": []
},
"cython_include_dirs": "/data/data/com.termux/files/usr/lib/python3.8/site-packages/numpy/core/include",
"dims": {
"N": 16,
"nbu": 0,
"nbx": 2,
"nbx_0": 4,
"nbx_e": 0,
"nbxe_0": 4,
"ng": 0,
"ng_e": 0,
"nh": 0,
"nh_e": 0,
"np": 2,
"nphi": 0,
"nphi_e": 0,
"nr": 0,
"nr_e": 0,
"ns": 0,
"ns_e": 0,
"nsbu": 0,
"nsbx": 0,
"nsbx_e": 0,
"nsg": 0,
"nsg_e": 0,
"nsh": 0,
"nsh_e": 0,
"nsphi": 0,
"nsphi_e": 0,
"nu": 1,
"nx": 4,
"ny": 3,
"ny_0": 3,
"ny_e": 2,
"nz": 0
},
"model": {
"dyn_disc_fun": null,
"dyn_disc_fun_jac": null,
"dyn_disc_fun_jac_hess": null,
"dyn_ext_fun_type": "casadi",
"dyn_source_discrete": null,
"gnsf": {
"nontrivial_f_LO": 1,
"purely_linear": 0
},
"name": "lat"
},
"parameter_values": [
0.0,
0.0
],
"problem_class": "OCP",
"simulink_opts": {
"inputs": {
"cost_W": 0,
"cost_W_0": 0,
"cost_W_e": 0,
"lbu": 1,
"lbx": 1,
"lbx_0": 1,
"lbx_e": 1,
"lg": 1,
"lh": 1,
"parameter_traj": 1,
"reset_solver": 0,
"u_init": 0,
"ubu": 1,
"ubx": 1,
"ubx_0": 1,
"ubx_e": 1,
"ug": 1,
"uh": 1,
"x_init": 0,
"y_ref": 1,
"y_ref_0": 1,
"y_ref_e": 1
},
"outputs": {
"CPU_time": 1,
"CPU_time_lin": 0,
"CPU_time_qp": 0,
"CPU_time_sim": 0,
"KKT_residual": 1,
"solver_status": 1,
"sqp_iter": 1,
"u0": 1,
"utraj": 0,
"x1": 1,
"xtraj": 0
},
"samplingtime": "t0"
},
"solver_options": {
"Tsim": 0.009765625,
"alpha_min": 0.05,
"alpha_reduction": 0.7,
"collocation_type": "GAUSS_LEGENDRE",
"eps_sufficient_descent": 0.0001,
"exact_hess_constr": 1,
"exact_hess_cost": 1,
"exact_hess_dyn": 1,
"ext_cost_num_hess": 0,
"full_step_dual": 0,
"globalization": "FIXED_STEP",
"globalization_use_SOC": 0,
"hessian_approx": "GAUSS_NEWTON",
"hpipm_mode": "BALANCE",
"initialize_t_slacks": 0,
"integrator_type": "ERK",
"levenberg_marquardt": 0.0,
"line_search_use_sufficient_descent": 0,
"model_external_shared_lib_dir": null,
"model_external_shared_lib_name": null,
"nlp_solver_max_iter": 100,
"nlp_solver_step_length": 1.0,
"nlp_solver_tol_comp": 1e-06,
"nlp_solver_tol_eq": 1e-06,
"nlp_solver_tol_ineq": 1e-06,
"nlp_solver_tol_stat": 1e-06,
"nlp_solver_type": "SQP_RTI",
"print_level": 0,
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
"qp_solver_cond_N": 1,
"qp_solver_iter_max": 1,
"qp_solver_tol_comp": null,
"qp_solver_tol_eq": null,
"qp_solver_tol_ineq": null,
"qp_solver_tol_stat": null,
"qp_solver_warm_start": 0,
"regularize_method": null,
"sim_method_jac_reuse": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"sim_method_newton_iter": 3,
"sim_method_num_stages": [
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4,
4
],
"sim_method_num_steps": [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1
],
"tf": 2.5,
"time_steps": [
0.009765625,
0.029296875,
0.048828125,
0.068359375,
0.087890625,
0.107421875,
0.126953125,
0.146484375,
0.166015625,
0.185546875,
0.205078125,
0.224609375,
0.244140625,
0.263671875,
0.283203125,
0.302734375
]
}
}
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c
+278
View File
@@ -0,0 +1,278 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_sim_solver_lat.h"
// ** solver data **
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int lat_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = LAT_NX;
const int nu = LAT_NU;
const int nz = LAT_NZ;
const int np = LAT_NP;
bool tmp_bool;
double Tsim = 0.009765625;
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = ERK;
// create correct config based on plan
sim_config * lat_sim_config = sim_config_create(plan);
capsule->acados_sim_config = lat_sim_config;
// sim dims
void *lat_sim_dims = sim_dims_create(lat_sim_config);
capsule->acados_sim_dims = lat_sim_dims;
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
// sim opts
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_opts = lat_sim_opts;
int tmp_int = 3;
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
tmp_int = 4;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
tmp_int = 1;
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
tmp_bool = 0;
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
// sim in / out
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_in = lat_sim_in;
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
capsule->acados_sim_out = lat_sim_out;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "T", &Tsim);
// model functions
lat_sim_config->model_set(lat_sim_in->model,
"expl_vde_for", capsule->sim_forw_vde_casadi);
lat_sim_config->model_set(lat_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
// sim solver
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
lat_sim_dims, lat_sim_opts);
capsule->acados_sim_solver = lat_sim_solver;
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
lat_acados_sim_update_params(capsule, p, np);
free(p);
/* initialize input */
// x
double x0[4];
for (int ii = 0; ii < 4; ii++)
x0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "x", x0);
// u
double u0[1];
for (int ii = 0; ii < 1; ii++)
u0[ii] = 0.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "u", u0);
// S_forw
double S_forw[20];
for (int ii = 0; ii < 20; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < 4; ii++)
S_forw[ii + ii * 4 ] = 1.0;
sim_in_set(lat_sim_config, lat_sim_dims,
lat_sim_in, "S_forw", S_forw);
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
return status;
}
int lat_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in lat_acados_sim_solve()! Exiting.\n");
return status;
}
int lat_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
return 0;
}
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = LAT_NP;
if (casadi_np != np) {
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
return status;
}
/* getters pointers to C objects*/
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.h
+103
View File
@@ -0,0 +1,103 @@
/*
* 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.;
*/
#ifndef ACADOS_SIM_lat_H_
#define ACADOS_SIM_lat_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define LAT_NX 4
#define LAT_NZ 0
#define LAT_NU 1
#define LAT_NP 2
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_lat_H_
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c
+264
View File
@@ -0,0 +1,264 @@
/*
* 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_lat
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "lat_model/lat_model.h"
#include "acados_solver_lat.h"
#include "simstruc.h"
#define SAMPLINGTIME 0.009765625
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);// specify the number of input ports
if ( !ssSetNumInputPorts(S, 8) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 6) )
return;
// specify dimension information for the input ports
// lbx_0
ssSetInputPortVectorDimension(S, 0, 4);
// ubx_0
ssSetInputPortVectorDimension(S, 1, 4);
// parameters
ssSetInputPortVectorDimension(S, 2, (16+1) * 2);
// y_ref_0
ssSetInputPortVectorDimension(S, 3, 3);
// y_ref
ssSetInputPortVectorDimension(S, 4, 45);
// y_ref_e
ssSetInputPortVectorDimension(S, 5, 2);
// lbx
ssSetInputPortVectorDimension(S, 6, 30);
// ubx
ssSetInputPortVectorDimension(S, 7, 30);/* specify dimension information for the OUTPUT ports */
ssSetOutputPortVectorDimension(S, 0, 1 );
ssSetOutputPortVectorDimension(S, 1, 1 );
ssSetOutputPortVectorDimension(S, 2, 1 );
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
ssSetOutputPortVectorDimension(S, 4, 1);
ssSetOutputPortVectorDimension(S, 5, 1 );
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
ssSetInputPortDirectFeedThrough(S, 0, 1);
ssSetInputPortDirectFeedThrough(S, 1, 1);
ssSetInputPortDirectFeedThrough(S, 2, 1);
ssSetInputPortDirectFeedThrough(S, 3, 1);
ssSetInputPortDirectFeedThrough(S, 4, 1);
ssSetInputPortDirectFeedThrough(S, 5, 1);
ssSetInputPortDirectFeedThrough(S, 6, 1);
ssSetInputPortDirectFeedThrough(S, 7, 1);
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
lat_solver_capsule *capsule = lat_acados_create_capsule();
lat_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
lat_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
// local buffer
real_t buffer[4];
/* go through inputs */
// lbx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
// ubx_0
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
for (int i = 0; i < 4; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
// parameters - stage-variant !!!
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
// update value of parameters
for (int ii = 0; ii <= 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[ii*2+jj]);
lat_acados_update_params(capsule, ii, buffer, 2);
}
// y_ref_0
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
for (int i = 0; i < 3; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
// y_ref - for stages 1 to N-1
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 3; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*3+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
// y_ref_e
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
for (int i = 0; i < 2; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 16, "yref", (void *) buffer);
// lbx
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
// ubx
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
for (int ii = 1; ii < 16; ii++)
{
for (int jj = 0; jj < 2; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = lat_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
int tmp_int;
out_u0 = ssGetOutputPortRealSignal(S, 0);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
out_status = ssGetOutputPortRealSignal(S, 1);
*out_status = (real_t) acados_status;
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
out_x1 = ssGetOutputPortRealSignal(S, 3);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
}
static void mdlTerminate(SimStruct *S)
{
lat_solver_capsule *capsule = ssGetUserData(S);
lat_acados_free(capsule);
lat_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c
+128
View File
@@ -0,0 +1,128 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_lat.h"
#define NX LAT_NX
#define NZ LAT_NZ
#define NU LAT_NU
#define NP LAT_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
status = lat_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
x_current[0] = 0.0;
x_current[1] = 0.0;
x_current[2] = 0.0;
x_current[3] = 0.0;
x_current[0] = 0;
x_current[1] = 0;
x_current[2] = 0;
x_current[3] = 0;
// initial value for control input
double u0[NU];
u0[0] = 0.0;
// set parameters
double p[NP];
p[0] = 0;
p[1] = 0;
lat_acados_sim_update_params(capsule, p, NP);
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = lat_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = lat_acados_sim_free(capsule);
if (status) {
printf("lat_acados_sim_free() returned status %d. \n", status);
}
lat_acados_sim_solver_free_capsule(capsule);
return status;
}
selfdrive/dragonpilot/controls_0816/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m
+125
View File
@@ -0,0 +1,125 @@
%
% 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.;
%
SOURCES = { ...
'lat_model/lat_expl_ode_fun.c', ...
'lat_model/lat_expl_vde_forw.c',...
'lat_cost/lat_cost_y_0_fun.c',...
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_0_hess.c',...
'lat_cost/lat_cost_y_fun.c',...
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_hess.c',...
'lat_cost/lat_cost_y_e_fun.c',...
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
'lat_cost/lat_cost_y_e_hess.c',...
'acados_solver_sfunction_lat.c', ...
'acados_solver_lat.c'
};
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
['-I', fullfile(INC_PATH, 'acados')], ...
['-I', fullfile(INC_PATH)]};
CFLAGS = 'CFLAGS=$CFLAGS';
LDFLAGS = 'LDFLAGS=$LDFLAGS';
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
% acados linking libraries and flags
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_lat' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
' size [4]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
' size [34]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [3]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
' size [45]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
i_in = i_in + 1;
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
fprintf(output_note)
selfdrive/locationd/locationd
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/car.cpp
+342 -342
View File
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100) {
out_1840749225442711100[0] = delta_x[0] + nom_x[0];
out_1840749225442711100[1] = delta_x[1] + nom_x[1];
out_1840749225442711100[2] = delta_x[2] + nom_x[2];
out_1840749225442711100[3] = delta_x[3] + nom_x[3];
out_1840749225442711100[4] = delta_x[4] + nom_x[4];
out_1840749225442711100[5] = delta_x[5] + nom_x[5];
out_1840749225442711100[6] = delta_x[6] + nom_x[6];
out_1840749225442711100[7] = delta_x[7] + nom_x[7];
out_1840749225442711100[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446) {
out_5232708683747839446[0] = delta_x[0] + nom_x[0];
out_5232708683747839446[1] = delta_x[1] + nom_x[1];
out_5232708683747839446[2] = delta_x[2] + nom_x[2];
out_5232708683747839446[3] = delta_x[3] + nom_x[3];
out_5232708683747839446[4] = delta_x[4] + nom_x[4];
out_5232708683747839446[5] = delta_x[5] + nom_x[5];
out_5232708683747839446[6] = delta_x[6] + nom_x[6];
out_5232708683747839446[7] = delta_x[7] + nom_x[7];
out_5232708683747839446[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
out_7887258947850824197[0] = -nom_x[0] + true_x[0];
out_7887258947850824197[1] = -nom_x[1] + true_x[1];
out_7887258947850824197[2] = -nom_x[2] + true_x[2];
out_7887258947850824197[3] = -nom_x[3] + true_x[3];
out_7887258947850824197[4] = -nom_x[4] + true_x[4];
out_7887258947850824197[5] = -nom_x[5] + true_x[5];
out_7887258947850824197[6] = -nom_x[6] + true_x[6];
out_7887258947850824197[7] = -nom_x[7] + true_x[7];
out_7887258947850824197[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138) {
out_6273110318621425138[0] = -nom_x[0] + true_x[0];
out_6273110318621425138[1] = -nom_x[1] + true_x[1];
out_6273110318621425138[2] = -nom_x[2] + true_x[2];
out_6273110318621425138[3] = -nom_x[3] + true_x[3];
out_6273110318621425138[4] = -nom_x[4] + true_x[4];
out_6273110318621425138[5] = -nom_x[5] + true_x[5];
out_6273110318621425138[6] = -nom_x[6] + true_x[6];
out_6273110318621425138[7] = -nom_x[7] + true_x[7];
out_6273110318621425138[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_5849575757077882696) {
out_5849575757077882696[0] = 1.0;
out_5849575757077882696[1] = 0;
out_5849575757077882696[2] = 0;
out_5849575757077882696[3] = 0;
out_5849575757077882696[4] = 0;
out_5849575757077882696[5] = 0;
out_5849575757077882696[6] = 0;
out_5849575757077882696[7] = 0;
out_5849575757077882696[8] = 0;
out_5849575757077882696[9] = 0;
out_5849575757077882696[10] = 1.0;
out_5849575757077882696[11] = 0;
out_5849575757077882696[12] = 0;
out_5849575757077882696[13] = 0;
out_5849575757077882696[14] = 0;
out_5849575757077882696[15] = 0;
out_5849575757077882696[16] = 0;
out_5849575757077882696[17] = 0;
out_5849575757077882696[18] = 0;
out_5849575757077882696[19] = 0;
out_5849575757077882696[20] = 1.0;
out_5849575757077882696[21] = 0;
out_5849575757077882696[22] = 0;
out_5849575757077882696[23] = 0;
out_5849575757077882696[24] = 0;
out_5849575757077882696[25] = 0;
out_5849575757077882696[26] = 0;
out_5849575757077882696[27] = 0;
out_5849575757077882696[28] = 0;
out_5849575757077882696[29] = 0;
out_5849575757077882696[30] = 1.0;
out_5849575757077882696[31] = 0;
out_5849575757077882696[32] = 0;
out_5849575757077882696[33] = 0;
out_5849575757077882696[34] = 0;
out_5849575757077882696[35] = 0;
out_5849575757077882696[36] = 0;
out_5849575757077882696[37] = 0;
out_5849575757077882696[38] = 0;
out_5849575757077882696[39] = 0;
out_5849575757077882696[40] = 1.0;
out_5849575757077882696[41] = 0;
out_5849575757077882696[42] = 0;
out_5849575757077882696[43] = 0;
out_5849575757077882696[44] = 0;
out_5849575757077882696[45] = 0;
out_5849575757077882696[46] = 0;
out_5849575757077882696[47] = 0;
out_5849575757077882696[48] = 0;
out_5849575757077882696[49] = 0;
out_5849575757077882696[50] = 1.0;
out_5849575757077882696[51] = 0;
out_5849575757077882696[52] = 0;
out_5849575757077882696[53] = 0;
out_5849575757077882696[54] = 0;
out_5849575757077882696[55] = 0;
out_5849575757077882696[56] = 0;
out_5849575757077882696[57] = 0;
out_5849575757077882696[58] = 0;
out_5849575757077882696[59] = 0;
out_5849575757077882696[60] = 1.0;
out_5849575757077882696[61] = 0;
out_5849575757077882696[62] = 0;
out_5849575757077882696[63] = 0;
out_5849575757077882696[64] = 0;
out_5849575757077882696[65] = 0;
out_5849575757077882696[66] = 0;
out_5849575757077882696[67] = 0;
out_5849575757077882696[68] = 0;
out_5849575757077882696[69] = 0;
out_5849575757077882696[70] = 1.0;
out_5849575757077882696[71] = 0;
out_5849575757077882696[72] = 0;
out_5849575757077882696[73] = 0;
out_5849575757077882696[74] = 0;
out_5849575757077882696[75] = 0;
out_5849575757077882696[76] = 0;
out_5849575757077882696[77] = 0;
out_5849575757077882696[78] = 0;
out_5849575757077882696[79] = 0;
out_5849575757077882696[80] = 1.0;
void H_mod_fun(double *state, double *out_376479380516898605) {
out_376479380516898605[0] = 1.0;
out_376479380516898605[1] = 0;
out_376479380516898605[2] = 0;
out_376479380516898605[3] = 0;
out_376479380516898605[4] = 0;
out_376479380516898605[5] = 0;
out_376479380516898605[6] = 0;
out_376479380516898605[7] = 0;
out_376479380516898605[8] = 0;
out_376479380516898605[9] = 0;
out_376479380516898605[10] = 1.0;
out_376479380516898605[11] = 0;
out_376479380516898605[12] = 0;
out_376479380516898605[13] = 0;
out_376479380516898605[14] = 0;
out_376479380516898605[15] = 0;
out_376479380516898605[16] = 0;
out_376479380516898605[17] = 0;
out_376479380516898605[18] = 0;
out_376479380516898605[19] = 0;
out_376479380516898605[20] = 1.0;
out_376479380516898605[21] = 0;
out_376479380516898605[22] = 0;
out_376479380516898605[23] = 0;
out_376479380516898605[24] = 0;
out_376479380516898605[25] = 0;
out_376479380516898605[26] = 0;
out_376479380516898605[27] = 0;
out_376479380516898605[28] = 0;
out_376479380516898605[29] = 0;
out_376479380516898605[30] = 1.0;
out_376479380516898605[31] = 0;
out_376479380516898605[32] = 0;
out_376479380516898605[33] = 0;
out_376479380516898605[34] = 0;
out_376479380516898605[35] = 0;
out_376479380516898605[36] = 0;
out_376479380516898605[37] = 0;
out_376479380516898605[38] = 0;
out_376479380516898605[39] = 0;
out_376479380516898605[40] = 1.0;
out_376479380516898605[41] = 0;
out_376479380516898605[42] = 0;
out_376479380516898605[43] = 0;
out_376479380516898605[44] = 0;
out_376479380516898605[45] = 0;
out_376479380516898605[46] = 0;
out_376479380516898605[47] = 0;
out_376479380516898605[48] = 0;
out_376479380516898605[49] = 0;
out_376479380516898605[50] = 1.0;
out_376479380516898605[51] = 0;
out_376479380516898605[52] = 0;
out_376479380516898605[53] = 0;
out_376479380516898605[54] = 0;
out_376479380516898605[55] = 0;
out_376479380516898605[56] = 0;
out_376479380516898605[57] = 0;
out_376479380516898605[58] = 0;
out_376479380516898605[59] = 0;
out_376479380516898605[60] = 1.0;
out_376479380516898605[61] = 0;
out_376479380516898605[62] = 0;
out_376479380516898605[63] = 0;
out_376479380516898605[64] = 0;
out_376479380516898605[65] = 0;
out_376479380516898605[66] = 0;
out_376479380516898605[67] = 0;
out_376479380516898605[68] = 0;
out_376479380516898605[69] = 0;
out_376479380516898605[70] = 1.0;
out_376479380516898605[71] = 0;
out_376479380516898605[72] = 0;
out_376479380516898605[73] = 0;
out_376479380516898605[74] = 0;
out_376479380516898605[75] = 0;
out_376479380516898605[76] = 0;
out_376479380516898605[77] = 0;
out_376479380516898605[78] = 0;
out_376479380516898605[79] = 0;
out_376479380516898605[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_1306455665935687231) {
out_1306455665935687231[0] = state[0];
out_1306455665935687231[1] = state[1];
out_1306455665935687231[2] = state[2];
out_1306455665935687231[3] = state[3];
out_1306455665935687231[4] = state[4];
out_1306455665935687231[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_1306455665935687231[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_1306455665935687231[7] = state[7];
out_1306455665935687231[8] = state[8];
void f_fun(double *state, double dt, double *out_7586787697879010325) {
out_7586787697879010325[0] = state[0];
out_7586787697879010325[1] = state[1];
out_7586787697879010325[2] = state[2];
out_7586787697879010325[3] = state[3];
out_7586787697879010325[4] = state[4];
out_7586787697879010325[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
out_7586787697879010325[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
out_7586787697879010325[7] = state[7];
out_7586787697879010325[8] = state[8];
}
void F_fun(double *state, double dt, double *out_1015750461961158607) {
out_1015750461961158607[0] = 1;
out_1015750461961158607[1] = 0;
out_1015750461961158607[2] = 0;
out_1015750461961158607[3] = 0;
out_1015750461961158607[4] = 0;
out_1015750461961158607[5] = 0;
out_1015750461961158607[6] = 0;
out_1015750461961158607[7] = 0;
out_1015750461961158607[8] = 0;
out_1015750461961158607[9] = 0;
out_1015750461961158607[10] = 1;
out_1015750461961158607[11] = 0;
out_1015750461961158607[12] = 0;
out_1015750461961158607[13] = 0;
out_1015750461961158607[14] = 0;
out_1015750461961158607[15] = 0;
out_1015750461961158607[16] = 0;
out_1015750461961158607[17] = 0;
out_1015750461961158607[18] = 0;
out_1015750461961158607[19] = 0;
out_1015750461961158607[20] = 1;
out_1015750461961158607[21] = 0;
out_1015750461961158607[22] = 0;
out_1015750461961158607[23] = 0;
out_1015750461961158607[24] = 0;
out_1015750461961158607[25] = 0;
out_1015750461961158607[26] = 0;
out_1015750461961158607[27] = 0;
out_1015750461961158607[28] = 0;
out_1015750461961158607[29] = 0;
out_1015750461961158607[30] = 1;
out_1015750461961158607[31] = 0;
out_1015750461961158607[32] = 0;
out_1015750461961158607[33] = 0;
out_1015750461961158607[34] = 0;
out_1015750461961158607[35] = 0;
out_1015750461961158607[36] = 0;
out_1015750461961158607[37] = 0;
out_1015750461961158607[38] = 0;
out_1015750461961158607[39] = 0;
out_1015750461961158607[40] = 1;
out_1015750461961158607[41] = 0;
out_1015750461961158607[42] = 0;
out_1015750461961158607[43] = 0;
out_1015750461961158607[44] = 0;
out_1015750461961158607[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_1015750461961158607[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_1015750461961158607[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_1015750461961158607[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_1015750461961158607[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_1015750461961158607[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_1015750461961158607[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_1015750461961158607[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_1015750461961158607[53] = -9.8000000000000007*dt;
out_1015750461961158607[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_1015750461961158607[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_1015750461961158607[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1015750461961158607[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1015750461961158607[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_1015750461961158607[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_1015750461961158607[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_1015750461961158607[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1015750461961158607[62] = 0;
out_1015750461961158607[63] = 0;
out_1015750461961158607[64] = 0;
out_1015750461961158607[65] = 0;
out_1015750461961158607[66] = 0;
out_1015750461961158607[67] = 0;
out_1015750461961158607[68] = 0;
out_1015750461961158607[69] = 0;
out_1015750461961158607[70] = 1;
out_1015750461961158607[71] = 0;
out_1015750461961158607[72] = 0;
out_1015750461961158607[73] = 0;
out_1015750461961158607[74] = 0;
out_1015750461961158607[75] = 0;
out_1015750461961158607[76] = 0;
out_1015750461961158607[77] = 0;
out_1015750461961158607[78] = 0;
out_1015750461961158607[79] = 0;
out_1015750461961158607[80] = 1;
void F_fun(double *state, double dt, double *out_472790716300753243) {
out_472790716300753243[0] = 1;
out_472790716300753243[1] = 0;
out_472790716300753243[2] = 0;
out_472790716300753243[3] = 0;
out_472790716300753243[4] = 0;
out_472790716300753243[5] = 0;
out_472790716300753243[6] = 0;
out_472790716300753243[7] = 0;
out_472790716300753243[8] = 0;
out_472790716300753243[9] = 0;
out_472790716300753243[10] = 1;
out_472790716300753243[11] = 0;
out_472790716300753243[12] = 0;
out_472790716300753243[13] = 0;
out_472790716300753243[14] = 0;
out_472790716300753243[15] = 0;
out_472790716300753243[16] = 0;
out_472790716300753243[17] = 0;
out_472790716300753243[18] = 0;
out_472790716300753243[19] = 0;
out_472790716300753243[20] = 1;
out_472790716300753243[21] = 0;
out_472790716300753243[22] = 0;
out_472790716300753243[23] = 0;
out_472790716300753243[24] = 0;
out_472790716300753243[25] = 0;
out_472790716300753243[26] = 0;
out_472790716300753243[27] = 0;
out_472790716300753243[28] = 0;
out_472790716300753243[29] = 0;
out_472790716300753243[30] = 1;
out_472790716300753243[31] = 0;
out_472790716300753243[32] = 0;
out_472790716300753243[33] = 0;
out_472790716300753243[34] = 0;
out_472790716300753243[35] = 0;
out_472790716300753243[36] = 0;
out_472790716300753243[37] = 0;
out_472790716300753243[38] = 0;
out_472790716300753243[39] = 0;
out_472790716300753243[40] = 1;
out_472790716300753243[41] = 0;
out_472790716300753243[42] = 0;
out_472790716300753243[43] = 0;
out_472790716300753243[44] = 0;
out_472790716300753243[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
out_472790716300753243[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_472790716300753243[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_472790716300753243[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_472790716300753243[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
out_472790716300753243[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_472790716300753243[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_472790716300753243[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_472790716300753243[53] = -9.8000000000000007*dt;
out_472790716300753243[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
out_472790716300753243[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_472790716300753243[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_472790716300753243[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_472790716300753243[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
out_472790716300753243[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_472790716300753243[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
out_472790716300753243[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_472790716300753243[62] = 0;
out_472790716300753243[63] = 0;
out_472790716300753243[64] = 0;
out_472790716300753243[65] = 0;
out_472790716300753243[66] = 0;
out_472790716300753243[67] = 0;
out_472790716300753243[68] = 0;
out_472790716300753243[69] = 0;
out_472790716300753243[70] = 1;
out_472790716300753243[71] = 0;
out_472790716300753243[72] = 0;
out_472790716300753243[73] = 0;
out_472790716300753243[74] = 0;
out_472790716300753243[75] = 0;
out_472790716300753243[76] = 0;
out_472790716300753243[77] = 0;
out_472790716300753243[78] = 0;
out_472790716300753243[79] = 0;
out_472790716300753243[80] = 1;
}
void h_25(double *state, double *unused, double *out_579481060641732431) {
out_579481060641732431[0] = state[6];
void h_25(double *state, double *unused, double *out_6846635756302794664) {
out_6846635756302794664[0] = state[6];
}
void H_25(double *state, double *unused, double *out_8838814511200069080) {
out_8838814511200069080[0] = 0;
out_8838814511200069080[1] = 0;
out_8838814511200069080[2] = 0;
out_8838814511200069080[3] = 0;
out_8838814511200069080[4] = 0;
out_8838814511200069080[5] = 0;
out_8838814511200069080[6] = 1;
out_8838814511200069080[7] = 0;
out_8838814511200069080[8] = 0;
void H_25(double *state, double *unused, double *out_1681825998279428652) {
out_1681825998279428652[0] = 0;
out_1681825998279428652[1] = 0;
out_1681825998279428652[2] = 0;
out_1681825998279428652[3] = 0;
out_1681825998279428652[4] = 0;
out_1681825998279428652[5] = 0;
out_1681825998279428652[6] = 1;
out_1681825998279428652[7] = 0;
out_1681825998279428652[8] = 0;
}
void h_24(double *state, double *unused, double *out_8628826922044058076) {
out_8628826922044058076[0] = state[4];
out_8628826922044058076[1] = state[5];
void h_24(double *state, double *unused, double *out_7639061379755997248) {
out_7639061379755997248[0] = state[4];
out_7639061379755997248[1] = state[5];
}
void H_24(double *state, double *unused, double *out_7435279963503982970) {
out_7435279963503982970[0] = 0;
out_7435279963503982970[1] = 0;
out_7435279963503982970[2] = 0;
out_7435279963503982970[3] = 0;
out_7435279963503982970[4] = 1;
out_7435279963503982970[5] = 0;
out_7435279963503982970[6] = 0;
out_7435279963503982970[7] = 0;
out_7435279963503982970[8] = 0;
out_7435279963503982970[9] = 0;
out_7435279963503982970[10] = 0;
out_7435279963503982970[11] = 0;
out_7435279963503982970[12] = 0;
out_7435279963503982970[13] = 0;
out_7435279963503982970[14] = 1;
out_7435279963503982970[15] = 0;
out_7435279963503982970[16] = 0;
out_7435279963503982970[17] = 0;
void H_24(double *state, double *unused, double *out_4893745808312089449) {
out_4893745808312089449[0] = 0;
out_4893745808312089449[1] = 0;
out_4893745808312089449[2] = 0;
out_4893745808312089449[3] = 0;
out_4893745808312089449[4] = 1;
out_4893745808312089449[5] = 0;
out_4893745808312089449[6] = 0;
out_4893745808312089449[7] = 0;
out_4893745808312089449[8] = 0;
out_4893745808312089449[9] = 0;
out_4893745808312089449[10] = 0;
out_4893745808312089449[11] = 0;
out_4893745808312089449[12] = 0;
out_4893745808312089449[13] = 0;
out_4893745808312089449[14] = 1;
out_4893745808312089449[15] = 0;
out_4893745808312089449[16] = 0;
out_4893745808312089449[17] = 0;
}
void h_30(double *state, double *unused, double *out_8060396416076809453) {
out_8060396416076809453[0] = state[4];
void h_30(double *state, double *unused, double *out_2046273354615198377) {
out_2046273354615198377[0] = state[4];
}
void H_30(double *state, double *unused, double *out_6320481552692820453) {
out_6320481552692820453[0] = 0;
out_6320481552692820453[1] = 0;
out_6320481552692820453[2] = 0;
out_6320481552692820453[3] = 0;
out_6320481552692820453[4] = 1;
out_6320481552692820453[5] = 0;
out_6320481552692820453[6] = 0;
out_6320481552692820453[7] = 0;
out_6320481552692820453[8] = 0;
void H_30(double *state, double *unused, double *out_5234864343212188103) {
out_5234864343212188103[0] = 0;
out_5234864343212188103[1] = 0;
out_5234864343212188103[2] = 0;
out_5234864343212188103[3] = 0;
out_5234864343212188103[4] = 1;
out_5234864343212188103[5] = 0;
out_5234864343212188103[6] = 0;
out_5234864343212188103[7] = 0;
out_5234864343212188103[8] = 0;
}
void h_26(double *state, double *unused, double *out_8663712300753286970) {
out_8663712300753286970[0] = state[7];
void h_26(double *state, double *unused, double *out_4200177702844975091) {
out_4200177702844975091[0] = state[7];
}
void H_26(double *state, double *unused, double *out_5866426243635426312) {
out_5866426243635426312[0] = 0;
out_5866426243635426312[1] = 0;
out_5866426243635426312[2] = 0;
out_5866426243635426312[3] = 0;
out_5866426243635426312[4] = 0;
out_5866426243635426312[5] = 0;
out_5866426243635426312[6] = 0;
out_5866426243635426312[7] = 1;
out_5866426243635426312[8] = 0;
void H_26(double *state, double *unused, double *out_5423329317153484876) {
out_5423329317153484876[0] = 0;
out_5423329317153484876[1] = 0;
out_5423329317153484876[2] = 0;
out_5423329317153484876[3] = 0;
out_5423329317153484876[4] = 0;
out_5423329317153484876[5] = 0;
out_5423329317153484876[6] = 0;
out_5423329317153484876[7] = 1;
out_5423329317153484876[8] = 0;
}
void h_27(double *state, double *unused, double *out_2233071042628120952) {
out_2233071042628120952[0] = state[3];
void h_27(double *state, double *unused, double *out_6628820938264559055) {
out_6628820938264559055[0] = state[3];
}
void H_27(double *state, double *unused, double *out_4096887481508877236) {
out_4096887481508877236[0] = 0;
out_4096887481508877236[1] = 0;
out_4096887481508877236[2] = 0;
out_4096887481508877236[3] = 1;
out_4096887481508877236[4] = 0;
out_4096887481508877236[5] = 0;
out_4096887481508877236[6] = 0;
out_4096887481508877236[7] = 0;
out_4096887481508877236[8] = 0;
void H_27(double *state, double *unused, double *out_3985928257223093633) {
out_3985928257223093633[0] = 0;
out_3985928257223093633[1] = 0;
out_3985928257223093633[2] = 0;
out_3985928257223093633[3] = 1;
out_3985928257223093633[4] = 0;
out_3985928257223093633[5] = 0;
out_3985928257223093633[6] = 0;
out_3985928257223093633[7] = 0;
out_3985928257223093633[8] = 0;
}
void h_29(double *state, double *unused, double *out_8528880318658063037) {
out_8528880318658063037[0] = state[1];
void h_29(double *state, double *unused, double *out_1102763501713535942) {
out_1102763501713535942[0] = state[1];
}
void H_29(double *state, double *unused, double *out_5810250208378428269) {
out_5810250208378428269[0] = 0;
out_5810250208378428269[1] = 1;
out_5810250208378428269[2] = 0;
out_5810250208378428269[3] = 0;
out_5810250208378428269[4] = 0;
out_5810250208378428269[5] = 0;
out_5810250208378428269[6] = 0;
out_5810250208378428269[7] = 0;
out_5810250208378428269[8] = 0;
void H_29(double *state, double *unused, double *out_5699290984092644666) {
out_5699290984092644666[0] = 0;
out_5699290984092644666[1] = 1;
out_5699290984092644666[2] = 0;
out_5699290984092644666[3] = 0;
out_5699290984092644666[4] = 0;
out_5699290984092644666[5] = 0;
out_5699290984092644666[6] = 0;
out_5699290984092644666[7] = 0;
out_5699290984092644666[8] = 0;
}
void h_28(double *state, double *unused, double *out_4837476363436818603) {
out_4837476363436818603[0] = state[0];
void h_28(double *state, double *unused, double *out_6175193264364035726) {
out_6175193264364035726[0] = state[0];
}
void H_28(double *state, double *unused, double *out_7554094848261592773) {
out_7554094848261592773[0] = 1;
out_7554094848261592773[1] = 0;
out_7554094848261592773[2] = 0;
out_7554094848261592773[3] = 0;
out_7554094848261592773[4] = 0;
out_7554094848261592773[5] = 0;
out_7554094848261592773[6] = 0;
out_7554094848261592773[7] = 0;
out_7554094848261592773[8] = 0;
void H_28(double *state, double *unused, double *out_3735660712527318415) {
out_3735660712527318415[0] = 1;
out_3735660712527318415[1] = 0;
out_3735660712527318415[2] = 0;
out_3735660712527318415[3] = 0;
out_3735660712527318415[4] = 0;
out_3735660712527318415[5] = 0;
out_3735660712527318415[6] = 0;
out_3735660712527318415[7] = 0;
out_3735660712527318415[8] = 0;
}
void h_31(double *state, double *unused, double *out_7350316286992083367) {
out_7350316286992083367[0] = state[8];
void h_31(double *state, double *unused, double *out_3077779063232596577) {
out_3077779063232596577[0] = state[8];
}
void H_31(double *state, double *unused, double *out_5240218141402074836) {
out_5240218141402074836[0] = 0;
out_5240218141402074836[1] = 0;
out_5240218141402074836[2] = 0;
out_5240218141402074836[3] = 0;
out_5240218141402074836[4] = 0;
out_5240218141402074836[5] = 0;
out_5240218141402074836[6] = 0;
out_5240218141402074836[7] = 0;
out_5240218141402074836[8] = 1;
void H_31(double *state, double *unused, double *out_1651180036402468224) {
out_1651180036402468224[0] = 0;
out_1651180036402468224[1] = 0;
out_1651180036402468224[2] = 0;
out_1651180036402468224[3] = 0;
out_1651180036402468224[4] = 0;
out_1651180036402468224[5] = 0;
out_1651180036402468224[6] = 0;
out_1651180036402468224[7] = 0;
out_1651180036402468224[8] = 1;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100) {
err_fun(nom_x, delta_x, out_1840749225442711100);
void car_err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446) {
err_fun(nom_x, delta_x, out_5232708683747839446);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
inv_err_fun(nom_x, true_x, out_7887258947850824197);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138) {
inv_err_fun(nom_x, true_x, out_6273110318621425138);
}
void car_H_mod_fun(double *state, double *out_5849575757077882696) {
H_mod_fun(state, out_5849575757077882696);
void car_H_mod_fun(double *state, double *out_376479380516898605) {
H_mod_fun(state, out_376479380516898605);
}
void car_f_fun(double *state, double dt, double *out_1306455665935687231) {
f_fun(state, dt, out_1306455665935687231);
void car_f_fun(double *state, double dt, double *out_7586787697879010325) {
f_fun(state, dt, out_7586787697879010325);
}
void car_F_fun(double *state, double dt, double *out_1015750461961158607) {
F_fun(state, dt, out_1015750461961158607);
void car_F_fun(double *state, double dt, double *out_472790716300753243) {
F_fun(state, dt, out_472790716300753243);
}
void car_h_25(double *state, double *unused, double *out_579481060641732431) {
h_25(state, unused, out_579481060641732431);
void car_h_25(double *state, double *unused, double *out_6846635756302794664) {
h_25(state, unused, out_6846635756302794664);
}
void car_H_25(double *state, double *unused, double *out_8838814511200069080) {
H_25(state, unused, out_8838814511200069080);
void car_H_25(double *state, double *unused, double *out_1681825998279428652) {
H_25(state, unused, out_1681825998279428652);
}
void car_h_24(double *state, double *unused, double *out_8628826922044058076) {
h_24(state, unused, out_8628826922044058076);
void car_h_24(double *state, double *unused, double *out_7639061379755997248) {
h_24(state, unused, out_7639061379755997248);
}
void car_H_24(double *state, double *unused, double *out_7435279963503982970) {
H_24(state, unused, out_7435279963503982970);
void car_H_24(double *state, double *unused, double *out_4893745808312089449) {
H_24(state, unused, out_4893745808312089449);
}
void car_h_30(double *state, double *unused, double *out_8060396416076809453) {
h_30(state, unused, out_8060396416076809453);
void car_h_30(double *state, double *unused, double *out_2046273354615198377) {
h_30(state, unused, out_2046273354615198377);
}
void car_H_30(double *state, double *unused, double *out_6320481552692820453) {
H_30(state, unused, out_6320481552692820453);
void car_H_30(double *state, double *unused, double *out_5234864343212188103) {
H_30(state, unused, out_5234864343212188103);
}
void car_h_26(double *state, double *unused, double *out_8663712300753286970) {
h_26(state, unused, out_8663712300753286970);
void car_h_26(double *state, double *unused, double *out_4200177702844975091) {
h_26(state, unused, out_4200177702844975091);
}
void car_H_26(double *state, double *unused, double *out_5866426243635426312) {
H_26(state, unused, out_5866426243635426312);
void car_H_26(double *state, double *unused, double *out_5423329317153484876) {
H_26(state, unused, out_5423329317153484876);
}
void car_h_27(double *state, double *unused, double *out_2233071042628120952) {
h_27(state, unused, out_2233071042628120952);
void car_h_27(double *state, double *unused, double *out_6628820938264559055) {
h_27(state, unused, out_6628820938264559055);
}
void car_H_27(double *state, double *unused, double *out_4096887481508877236) {
H_27(state, unused, out_4096887481508877236);
void car_H_27(double *state, double *unused, double *out_3985928257223093633) {
H_27(state, unused, out_3985928257223093633);
}
void car_h_29(double *state, double *unused, double *out_8528880318658063037) {
h_29(state, unused, out_8528880318658063037);
void car_h_29(double *state, double *unused, double *out_1102763501713535942) {
h_29(state, unused, out_1102763501713535942);
}
void car_H_29(double *state, double *unused, double *out_5810250208378428269) {
H_29(state, unused, out_5810250208378428269);
void car_H_29(double *state, double *unused, double *out_5699290984092644666) {
H_29(state, unused, out_5699290984092644666);
}
void car_h_28(double *state, double *unused, double *out_4837476363436818603) {
h_28(state, unused, out_4837476363436818603);
void car_h_28(double *state, double *unused, double *out_6175193264364035726) {
h_28(state, unused, out_6175193264364035726);
}
void car_H_28(double *state, double *unused, double *out_7554094848261592773) {
H_28(state, unused, out_7554094848261592773);
void car_H_28(double *state, double *unused, double *out_3735660712527318415) {
H_28(state, unused, out_3735660712527318415);
}
void car_h_31(double *state, double *unused, double *out_7350316286992083367) {
h_31(state, unused, out_7350316286992083367);
void car_h_31(double *state, double *unused, double *out_3077779063232596577) {
h_31(state, unused, out_3077779063232596577);
}
void car_H_31(double *state, double *unused, double *out_5240218141402074836) {
H_31(state, unused, out_5240218141402074836);
void car_H_31(double *state, double *unused, double *out_1651180036402468224) {
H_31(state, unused, out_1651180036402468224);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197);
void car_H_mod_fun(double *state, double *out_5849575757077882696);
void car_f_fun(double *state, double dt, double *out_1306455665935687231);
void car_F_fun(double *state, double dt, double *out_1015750461961158607);
void car_h_25(double *state, double *unused, double *out_579481060641732431);
void car_H_25(double *state, double *unused, double *out_8838814511200069080);
void car_h_24(double *state, double *unused, double *out_8628826922044058076);
void car_H_24(double *state, double *unused, double *out_7435279963503982970);
void car_h_30(double *state, double *unused, double *out_8060396416076809453);
void car_H_30(double *state, double *unused, double *out_6320481552692820453);
void car_h_26(double *state, double *unused, double *out_8663712300753286970);
void car_H_26(double *state, double *unused, double *out_5866426243635426312);
void car_h_27(double *state, double *unused, double *out_2233071042628120952);
void car_H_27(double *state, double *unused, double *out_4096887481508877236);
void car_h_29(double *state, double *unused, double *out_8528880318658063037);
void car_H_29(double *state, double *unused, double *out_5810250208378428269);
void car_h_28(double *state, double *unused, double *out_4837476363436818603);
void car_H_28(double *state, double *unused, double *out_7554094848261592773);
void car_h_31(double *state, double *unused, double *out_7350316286992083367);
void car_H_31(double *state, double *unused, double *out_5240218141402074836);
void car_err_fun(double *nom_x, double *delta_x, double *out_5232708683747839446);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_6273110318621425138);
void car_H_mod_fun(double *state, double *out_376479380516898605);
void car_f_fun(double *state, double dt, double *out_7586787697879010325);
void car_F_fun(double *state, double dt, double *out_472790716300753243);
void car_h_25(double *state, double *unused, double *out_6846635756302794664);
void car_H_25(double *state, double *unused, double *out_1681825998279428652);
void car_h_24(double *state, double *unused, double *out_7639061379755997248);
void car_H_24(double *state, double *unused, double *out_4893745808312089449);
void car_h_30(double *state, double *unused, double *out_2046273354615198377);
void car_H_30(double *state, double *unused, double *out_5234864343212188103);
void car_h_26(double *state, double *unused, double *out_4200177702844975091);
void car_H_26(double *state, double *unused, double *out_5423329317153484876);
void car_h_27(double *state, double *unused, double *out_6628820938264559055);
void car_H_27(double *state, double *unused, double *out_3985928257223093633);
void car_h_29(double *state, double *unused, double *out_1102763501713535942);
void car_H_29(double *state, double *unused, double *out_5699290984092644666);
void car_h_28(double *state, double *unused, double *out_6175193264364035726);
void car_H_28(double *state, double *unused, double *out_3735660712527318415);
void car_h_31(double *state, double *unused, double *out_3077779063232596577);
void car_H_31(double *state, double *unused, double *out_1651180036402468224);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
selfdrive/locationd/models/generated/gnss.cpp
+362 -362
View File
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
out_2351947283659598660[0] = delta_x[0] + nom_x[0];
out_2351947283659598660[1] = delta_x[1] + nom_x[1];
out_2351947283659598660[2] = delta_x[2] + nom_x[2];
out_2351947283659598660[3] = delta_x[3] + nom_x[3];
out_2351947283659598660[4] = delta_x[4] + nom_x[4];
out_2351947283659598660[5] = delta_x[5] + nom_x[5];
out_2351947283659598660[6] = delta_x[6] + nom_x[6];
out_2351947283659598660[7] = delta_x[7] + nom_x[7];
out_2351947283659598660[8] = delta_x[8] + nom_x[8];
out_2351947283659598660[9] = delta_x[9] + nom_x[9];
out_2351947283659598660[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745) {
out_4281415265925619745[0] = delta_x[0] + nom_x[0];
out_4281415265925619745[1] = delta_x[1] + nom_x[1];
out_4281415265925619745[2] = delta_x[2] + nom_x[2];
out_4281415265925619745[3] = delta_x[3] + nom_x[3];
out_4281415265925619745[4] = delta_x[4] + nom_x[4];
out_4281415265925619745[5] = delta_x[5] + nom_x[5];
out_4281415265925619745[6] = delta_x[6] + nom_x[6];
out_4281415265925619745[7] = delta_x[7] + nom_x[7];
out_4281415265925619745[8] = delta_x[8] + nom_x[8];
out_4281415265925619745[9] = delta_x[9] + nom_x[9];
out_4281415265925619745[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
out_8539310053726918653[0] = -nom_x[0] + true_x[0];
out_8539310053726918653[1] = -nom_x[1] + true_x[1];
out_8539310053726918653[2] = -nom_x[2] + true_x[2];
out_8539310053726918653[3] = -nom_x[3] + true_x[3];
out_8539310053726918653[4] = -nom_x[4] + true_x[4];
out_8539310053726918653[5] = -nom_x[5] + true_x[5];
out_8539310053726918653[6] = -nom_x[6] + true_x[6];
out_8539310053726918653[7] = -nom_x[7] + true_x[7];
out_8539310053726918653[8] = -nom_x[8] + true_x[8];
out_8539310053726918653[9] = -nom_x[9] + true_x[9];
out_8539310053726918653[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220) {
out_8058048663833159220[0] = -nom_x[0] + true_x[0];
out_8058048663833159220[1] = -nom_x[1] + true_x[1];
out_8058048663833159220[2] = -nom_x[2] + true_x[2];
out_8058048663833159220[3] = -nom_x[3] + true_x[3];
out_8058048663833159220[4] = -nom_x[4] + true_x[4];
out_8058048663833159220[5] = -nom_x[5] + true_x[5];
out_8058048663833159220[6] = -nom_x[6] + true_x[6];
out_8058048663833159220[7] = -nom_x[7] + true_x[7];
out_8058048663833159220[8] = -nom_x[8] + true_x[8];
out_8058048663833159220[9] = -nom_x[9] + true_x[9];
out_8058048663833159220[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_2843304307356815987) {
out_2843304307356815987[0] = 1.0;
out_2843304307356815987[1] = 0;
out_2843304307356815987[2] = 0;
out_2843304307356815987[3] = 0;
out_2843304307356815987[4] = 0;
out_2843304307356815987[5] = 0;
out_2843304307356815987[6] = 0;
out_2843304307356815987[7] = 0;
out_2843304307356815987[8] = 0;
out_2843304307356815987[9] = 0;
out_2843304307356815987[10] = 0;
out_2843304307356815987[11] = 0;
out_2843304307356815987[12] = 1.0;
out_2843304307356815987[13] = 0;
out_2843304307356815987[14] = 0;
out_2843304307356815987[15] = 0;
out_2843304307356815987[16] = 0;
out_2843304307356815987[17] = 0;
out_2843304307356815987[18] = 0;
out_2843304307356815987[19] = 0;
out_2843304307356815987[20] = 0;
out_2843304307356815987[21] = 0;
out_2843304307356815987[22] = 0;
out_2843304307356815987[23] = 0;
out_2843304307356815987[24] = 1.0;
out_2843304307356815987[25] = 0;
out_2843304307356815987[26] = 0;
out_2843304307356815987[27] = 0;
out_2843304307356815987[28] = 0;
out_2843304307356815987[29] = 0;
out_2843304307356815987[30] = 0;
out_2843304307356815987[31] = 0;
out_2843304307356815987[32] = 0;
out_2843304307356815987[33] = 0;
out_2843304307356815987[34] = 0;
out_2843304307356815987[35] = 0;
out_2843304307356815987[36] = 1.0;
out_2843304307356815987[37] = 0;
out_2843304307356815987[38] = 0;
out_2843304307356815987[39] = 0;
out_2843304307356815987[40] = 0;
out_2843304307356815987[41] = 0;
out_2843304307356815987[42] = 0;
out_2843304307356815987[43] = 0;
out_2843304307356815987[44] = 0;
out_2843304307356815987[45] = 0;
out_2843304307356815987[46] = 0;
out_2843304307356815987[47] = 0;
out_2843304307356815987[48] = 1.0;
out_2843304307356815987[49] = 0;
out_2843304307356815987[50] = 0;
out_2843304307356815987[51] = 0;
out_2843304307356815987[52] = 0;
out_2843304307356815987[53] = 0;
out_2843304307356815987[54] = 0;
out_2843304307356815987[55] = 0;
out_2843304307356815987[56] = 0;
out_2843304307356815987[57] = 0;
out_2843304307356815987[58] = 0;
out_2843304307356815987[59] = 0;
out_2843304307356815987[60] = 1.0;
out_2843304307356815987[61] = 0;
out_2843304307356815987[62] = 0;
out_2843304307356815987[63] = 0;
out_2843304307356815987[64] = 0;
out_2843304307356815987[65] = 0;
out_2843304307356815987[66] = 0;
out_2843304307356815987[67] = 0;
out_2843304307356815987[68] = 0;
out_2843304307356815987[69] = 0;
out_2843304307356815987[70] = 0;
out_2843304307356815987[71] = 0;
out_2843304307356815987[72] = 1.0;
out_2843304307356815987[73] = 0;
out_2843304307356815987[74] = 0;
out_2843304307356815987[75] = 0;
out_2843304307356815987[76] = 0;
out_2843304307356815987[77] = 0;
out_2843304307356815987[78] = 0;
out_2843304307356815987[79] = 0;
out_2843304307356815987[80] = 0;
out_2843304307356815987[81] = 0;
out_2843304307356815987[82] = 0;
out_2843304307356815987[83] = 0;
out_2843304307356815987[84] = 1.0;
out_2843304307356815987[85] = 0;
out_2843304307356815987[86] = 0;
out_2843304307356815987[87] = 0;
out_2843304307356815987[88] = 0;
out_2843304307356815987[89] = 0;
out_2843304307356815987[90] = 0;
out_2843304307356815987[91] = 0;
out_2843304307356815987[92] = 0;
out_2843304307356815987[93] = 0;
out_2843304307356815987[94] = 0;
out_2843304307356815987[95] = 0;
out_2843304307356815987[96] = 1.0;
out_2843304307356815987[97] = 0;
out_2843304307356815987[98] = 0;
out_2843304307356815987[99] = 0;
out_2843304307356815987[100] = 0;
out_2843304307356815987[101] = 0;
out_2843304307356815987[102] = 0;
out_2843304307356815987[103] = 0;
out_2843304307356815987[104] = 0;
out_2843304307356815987[105] = 0;
out_2843304307356815987[106] = 0;
out_2843304307356815987[107] = 0;
out_2843304307356815987[108] = 1.0;
out_2843304307356815987[109] = 0;
out_2843304307356815987[110] = 0;
out_2843304307356815987[111] = 0;
out_2843304307356815987[112] = 0;
out_2843304307356815987[113] = 0;
out_2843304307356815987[114] = 0;
out_2843304307356815987[115] = 0;
out_2843304307356815987[116] = 0;
out_2843304307356815987[117] = 0;
out_2843304307356815987[118] = 0;
out_2843304307356815987[119] = 0;
out_2843304307356815987[120] = 1.0;
void H_mod_fun(double *state, double *out_3690945488544052836) {
out_3690945488544052836[0] = 1.0;
out_3690945488544052836[1] = 0;
out_3690945488544052836[2] = 0;
out_3690945488544052836[3] = 0;
out_3690945488544052836[4] = 0;
out_3690945488544052836[5] = 0;
out_3690945488544052836[6] = 0;
out_3690945488544052836[7] = 0;
out_3690945488544052836[8] = 0;
out_3690945488544052836[9] = 0;
out_3690945488544052836[10] = 0;
out_3690945488544052836[11] = 0;
out_3690945488544052836[12] = 1.0;
out_3690945488544052836[13] = 0;
out_3690945488544052836[14] = 0;
out_3690945488544052836[15] = 0;
out_3690945488544052836[16] = 0;
out_3690945488544052836[17] = 0;
out_3690945488544052836[18] = 0;
out_3690945488544052836[19] = 0;
out_3690945488544052836[20] = 0;
out_3690945488544052836[21] = 0;
out_3690945488544052836[22] = 0;
out_3690945488544052836[23] = 0;
out_3690945488544052836[24] = 1.0;
out_3690945488544052836[25] = 0;
out_3690945488544052836[26] = 0;
out_3690945488544052836[27] = 0;
out_3690945488544052836[28] = 0;
out_3690945488544052836[29] = 0;
out_3690945488544052836[30] = 0;
out_3690945488544052836[31] = 0;
out_3690945488544052836[32] = 0;
out_3690945488544052836[33] = 0;
out_3690945488544052836[34] = 0;
out_3690945488544052836[35] = 0;
out_3690945488544052836[36] = 1.0;
out_3690945488544052836[37] = 0;
out_3690945488544052836[38] = 0;
out_3690945488544052836[39] = 0;
out_3690945488544052836[40] = 0;
out_3690945488544052836[41] = 0;
out_3690945488544052836[42] = 0;
out_3690945488544052836[43] = 0;
out_3690945488544052836[44] = 0;
out_3690945488544052836[45] = 0;
out_3690945488544052836[46] = 0;
out_3690945488544052836[47] = 0;
out_3690945488544052836[48] = 1.0;
out_3690945488544052836[49] = 0;
out_3690945488544052836[50] = 0;
out_3690945488544052836[51] = 0;
out_3690945488544052836[52] = 0;
out_3690945488544052836[53] = 0;
out_3690945488544052836[54] = 0;
out_3690945488544052836[55] = 0;
out_3690945488544052836[56] = 0;
out_3690945488544052836[57] = 0;
out_3690945488544052836[58] = 0;
out_3690945488544052836[59] = 0;
out_3690945488544052836[60] = 1.0;
out_3690945488544052836[61] = 0;
out_3690945488544052836[62] = 0;
out_3690945488544052836[63] = 0;
out_3690945488544052836[64] = 0;
out_3690945488544052836[65] = 0;
out_3690945488544052836[66] = 0;
out_3690945488544052836[67] = 0;
out_3690945488544052836[68] = 0;
out_3690945488544052836[69] = 0;
out_3690945488544052836[70] = 0;
out_3690945488544052836[71] = 0;
out_3690945488544052836[72] = 1.0;
out_3690945488544052836[73] = 0;
out_3690945488544052836[74] = 0;
out_3690945488544052836[75] = 0;
out_3690945488544052836[76] = 0;
out_3690945488544052836[77] = 0;
out_3690945488544052836[78] = 0;
out_3690945488544052836[79] = 0;
out_3690945488544052836[80] = 0;
out_3690945488544052836[81] = 0;
out_3690945488544052836[82] = 0;
out_3690945488544052836[83] = 0;
out_3690945488544052836[84] = 1.0;
out_3690945488544052836[85] = 0;
out_3690945488544052836[86] = 0;
out_3690945488544052836[87] = 0;
out_3690945488544052836[88] = 0;
out_3690945488544052836[89] = 0;
out_3690945488544052836[90] = 0;
out_3690945488544052836[91] = 0;
out_3690945488544052836[92] = 0;
out_3690945488544052836[93] = 0;
out_3690945488544052836[94] = 0;
out_3690945488544052836[95] = 0;
out_3690945488544052836[96] = 1.0;
out_3690945488544052836[97] = 0;
out_3690945488544052836[98] = 0;
out_3690945488544052836[99] = 0;
out_3690945488544052836[100] = 0;
out_3690945488544052836[101] = 0;
out_3690945488544052836[102] = 0;
out_3690945488544052836[103] = 0;
out_3690945488544052836[104] = 0;
out_3690945488544052836[105] = 0;
out_3690945488544052836[106] = 0;
out_3690945488544052836[107] = 0;
out_3690945488544052836[108] = 1.0;
out_3690945488544052836[109] = 0;
out_3690945488544052836[110] = 0;
out_3690945488544052836[111] = 0;
out_3690945488544052836[112] = 0;
out_3690945488544052836[113] = 0;
out_3690945488544052836[114] = 0;
out_3690945488544052836[115] = 0;
out_3690945488544052836[116] = 0;
out_3690945488544052836[117] = 0;
out_3690945488544052836[118] = 0;
out_3690945488544052836[119] = 0;
out_3690945488544052836[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_774769938980785647) {
out_774769938980785647[0] = dt*state[3] + state[0];
out_774769938980785647[1] = dt*state[4] + state[1];
out_774769938980785647[2] = dt*state[5] + state[2];
out_774769938980785647[3] = state[3];
out_774769938980785647[4] = state[4];
out_774769938980785647[5] = state[5];
out_774769938980785647[6] = dt*state[7] + state[6];
out_774769938980785647[7] = dt*state[8] + state[7];
out_774769938980785647[8] = state[8];
out_774769938980785647[9] = state[9];
out_774769938980785647[10] = state[10];
void f_fun(double *state, double dt, double *out_2954456239899519205) {
out_2954456239899519205[0] = dt*state[3] + state[0];
out_2954456239899519205[1] = dt*state[4] + state[1];
out_2954456239899519205[2] = dt*state[5] + state[2];
out_2954456239899519205[3] = state[3];
out_2954456239899519205[4] = state[4];
out_2954456239899519205[5] = state[5];
out_2954456239899519205[6] = dt*state[7] + state[6];
out_2954456239899519205[7] = dt*state[8] + state[7];
out_2954456239899519205[8] = state[8];
out_2954456239899519205[9] = state[9];
out_2954456239899519205[10] = state[10];
}
void F_fun(double *state, double dt, double *out_4467768114936024642) {
out_4467768114936024642[0] = 1;
out_4467768114936024642[1] = 0;
out_4467768114936024642[2] = 0;
out_4467768114936024642[3] = dt;
out_4467768114936024642[4] = 0;
out_4467768114936024642[5] = 0;
out_4467768114936024642[6] = 0;
out_4467768114936024642[7] = 0;
out_4467768114936024642[8] = 0;
out_4467768114936024642[9] = 0;
out_4467768114936024642[10] = 0;
out_4467768114936024642[11] = 0;
out_4467768114936024642[12] = 1;
out_4467768114936024642[13] = 0;
out_4467768114936024642[14] = 0;
out_4467768114936024642[15] = dt;
out_4467768114936024642[16] = 0;
out_4467768114936024642[17] = 0;
out_4467768114936024642[18] = 0;
out_4467768114936024642[19] = 0;
out_4467768114936024642[20] = 0;
out_4467768114936024642[21] = 0;
out_4467768114936024642[22] = 0;
out_4467768114936024642[23] = 0;
out_4467768114936024642[24] = 1;
out_4467768114936024642[25] = 0;
out_4467768114936024642[26] = 0;
out_4467768114936024642[27] = dt;
out_4467768114936024642[28] = 0;
out_4467768114936024642[29] = 0;
out_4467768114936024642[30] = 0;
out_4467768114936024642[31] = 0;
out_4467768114936024642[32] = 0;
out_4467768114936024642[33] = 0;
out_4467768114936024642[34] = 0;
out_4467768114936024642[35] = 0;
out_4467768114936024642[36] = 1;
out_4467768114936024642[37] = 0;
out_4467768114936024642[38] = 0;
out_4467768114936024642[39] = 0;
out_4467768114936024642[40] = 0;
out_4467768114936024642[41] = 0;
out_4467768114936024642[42] = 0;
out_4467768114936024642[43] = 0;
out_4467768114936024642[44] = 0;
out_4467768114936024642[45] = 0;
out_4467768114936024642[46] = 0;
out_4467768114936024642[47] = 0;
out_4467768114936024642[48] = 1;
out_4467768114936024642[49] = 0;
out_4467768114936024642[50] = 0;
out_4467768114936024642[51] = 0;
out_4467768114936024642[52] = 0;
out_4467768114936024642[53] = 0;
out_4467768114936024642[54] = 0;
out_4467768114936024642[55] = 0;
out_4467768114936024642[56] = 0;
out_4467768114936024642[57] = 0;
out_4467768114936024642[58] = 0;
out_4467768114936024642[59] = 0;
out_4467768114936024642[60] = 1;
out_4467768114936024642[61] = 0;
out_4467768114936024642[62] = 0;
out_4467768114936024642[63] = 0;
out_4467768114936024642[64] = 0;
out_4467768114936024642[65] = 0;
out_4467768114936024642[66] = 0;
out_4467768114936024642[67] = 0;
out_4467768114936024642[68] = 0;
out_4467768114936024642[69] = 0;
out_4467768114936024642[70] = 0;
out_4467768114936024642[71] = 0;
out_4467768114936024642[72] = 1;
out_4467768114936024642[73] = dt;
out_4467768114936024642[74] = 0;
out_4467768114936024642[75] = 0;
out_4467768114936024642[76] = 0;
out_4467768114936024642[77] = 0;
out_4467768114936024642[78] = 0;
out_4467768114936024642[79] = 0;
out_4467768114936024642[80] = 0;
out_4467768114936024642[81] = 0;
out_4467768114936024642[82] = 0;
out_4467768114936024642[83] = 0;
out_4467768114936024642[84] = 1;
out_4467768114936024642[85] = dt;
out_4467768114936024642[86] = 0;
out_4467768114936024642[87] = 0;
out_4467768114936024642[88] = 0;
out_4467768114936024642[89] = 0;
out_4467768114936024642[90] = 0;
out_4467768114936024642[91] = 0;
out_4467768114936024642[92] = 0;
out_4467768114936024642[93] = 0;
out_4467768114936024642[94] = 0;
out_4467768114936024642[95] = 0;
out_4467768114936024642[96] = 1;
out_4467768114936024642[97] = 0;
out_4467768114936024642[98] = 0;
out_4467768114936024642[99] = 0;
out_4467768114936024642[100] = 0;
out_4467768114936024642[101] = 0;
out_4467768114936024642[102] = 0;
out_4467768114936024642[103] = 0;
out_4467768114936024642[104] = 0;
out_4467768114936024642[105] = 0;
out_4467768114936024642[106] = 0;
out_4467768114936024642[107] = 0;
out_4467768114936024642[108] = 1;
out_4467768114936024642[109] = 0;
out_4467768114936024642[110] = 0;
out_4467768114936024642[111] = 0;
out_4467768114936024642[112] = 0;
out_4467768114936024642[113] = 0;
out_4467768114936024642[114] = 0;
out_4467768114936024642[115] = 0;
out_4467768114936024642[116] = 0;
out_4467768114936024642[117] = 0;
out_4467768114936024642[118] = 0;
out_4467768114936024642[119] = 0;
out_4467768114936024642[120] = 1;
void F_fun(double *state, double dt, double *out_709514566261682476) {
out_709514566261682476[0] = 1;
out_709514566261682476[1] = 0;
out_709514566261682476[2] = 0;
out_709514566261682476[3] = dt;
out_709514566261682476[4] = 0;
out_709514566261682476[5] = 0;
out_709514566261682476[6] = 0;
out_709514566261682476[7] = 0;
out_709514566261682476[8] = 0;
out_709514566261682476[9] = 0;
out_709514566261682476[10] = 0;
out_709514566261682476[11] = 0;
out_709514566261682476[12] = 1;
out_709514566261682476[13] = 0;
out_709514566261682476[14] = 0;
out_709514566261682476[15] = dt;
out_709514566261682476[16] = 0;
out_709514566261682476[17] = 0;
out_709514566261682476[18] = 0;
out_709514566261682476[19] = 0;
out_709514566261682476[20] = 0;
out_709514566261682476[21] = 0;
out_709514566261682476[22] = 0;
out_709514566261682476[23] = 0;
out_709514566261682476[24] = 1;
out_709514566261682476[25] = 0;
out_709514566261682476[26] = 0;
out_709514566261682476[27] = dt;
out_709514566261682476[28] = 0;
out_709514566261682476[29] = 0;
out_709514566261682476[30] = 0;
out_709514566261682476[31] = 0;
out_709514566261682476[32] = 0;
out_709514566261682476[33] = 0;
out_709514566261682476[34] = 0;
out_709514566261682476[35] = 0;
out_709514566261682476[36] = 1;
out_709514566261682476[37] = 0;
out_709514566261682476[38] = 0;
out_709514566261682476[39] = 0;
out_709514566261682476[40] = 0;
out_709514566261682476[41] = 0;
out_709514566261682476[42] = 0;
out_709514566261682476[43] = 0;
out_709514566261682476[44] = 0;
out_709514566261682476[45] = 0;
out_709514566261682476[46] = 0;
out_709514566261682476[47] = 0;
out_709514566261682476[48] = 1;
out_709514566261682476[49] = 0;
out_709514566261682476[50] = 0;
out_709514566261682476[51] = 0;
out_709514566261682476[52] = 0;
out_709514566261682476[53] = 0;
out_709514566261682476[54] = 0;
out_709514566261682476[55] = 0;
out_709514566261682476[56] = 0;
out_709514566261682476[57] = 0;
out_709514566261682476[58] = 0;
out_709514566261682476[59] = 0;
out_709514566261682476[60] = 1;
out_709514566261682476[61] = 0;
out_709514566261682476[62] = 0;
out_709514566261682476[63] = 0;
out_709514566261682476[64] = 0;
out_709514566261682476[65] = 0;
out_709514566261682476[66] = 0;
out_709514566261682476[67] = 0;
out_709514566261682476[68] = 0;
out_709514566261682476[69] = 0;
out_709514566261682476[70] = 0;
out_709514566261682476[71] = 0;
out_709514566261682476[72] = 1;
out_709514566261682476[73] = dt;
out_709514566261682476[74] = 0;
out_709514566261682476[75] = 0;
out_709514566261682476[76] = 0;
out_709514566261682476[77] = 0;
out_709514566261682476[78] = 0;
out_709514566261682476[79] = 0;
out_709514566261682476[80] = 0;
out_709514566261682476[81] = 0;
out_709514566261682476[82] = 0;
out_709514566261682476[83] = 0;
out_709514566261682476[84] = 1;
out_709514566261682476[85] = dt;
out_709514566261682476[86] = 0;
out_709514566261682476[87] = 0;
out_709514566261682476[88] = 0;
out_709514566261682476[89] = 0;
out_709514566261682476[90] = 0;
out_709514566261682476[91] = 0;
out_709514566261682476[92] = 0;
out_709514566261682476[93] = 0;
out_709514566261682476[94] = 0;
out_709514566261682476[95] = 0;
out_709514566261682476[96] = 1;
out_709514566261682476[97] = 0;
out_709514566261682476[98] = 0;
out_709514566261682476[99] = 0;
out_709514566261682476[100] = 0;
out_709514566261682476[101] = 0;
out_709514566261682476[102] = 0;
out_709514566261682476[103] = 0;
out_709514566261682476[104] = 0;
out_709514566261682476[105] = 0;
out_709514566261682476[106] = 0;
out_709514566261682476[107] = 0;
out_709514566261682476[108] = 1;
out_709514566261682476[109] = 0;
out_709514566261682476[110] = 0;
out_709514566261682476[111] = 0;
out_709514566261682476[112] = 0;
out_709514566261682476[113] = 0;
out_709514566261682476[114] = 0;
out_709514566261682476[115] = 0;
out_709514566261682476[116] = 0;
out_709514566261682476[117] = 0;
out_709514566261682476[118] = 0;
out_709514566261682476[119] = 0;
out_709514566261682476[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
out_8206867657584711155[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
void h_6(double *state, double *sat_pos, double *out_2648966289655583926) {
out_2648966289655583926[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
}
void H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
out_2748205046550560630[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2748205046550560630[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2748205046550560630[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_2748205046550560630[3] = 0;
out_2748205046550560630[4] = 0;
out_2748205046550560630[5] = 0;
out_2748205046550560630[6] = 1;
out_2748205046550560630[7] = 0;
out_2748205046550560630[8] = 0;
out_2748205046550560630[9] = 0;
out_2748205046550560630[10] = 0;
void H_6(double *state, double *sat_pos, double *out_1516634108393932450) {
out_1516634108393932450[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1516634108393932450[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1516634108393932450[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_1516634108393932450[3] = 0;
out_1516634108393932450[4] = 0;
out_1516634108393932450[5] = 0;
out_1516634108393932450[6] = 1;
out_1516634108393932450[7] = 0;
out_1516634108393932450[8] = 0;
out_1516634108393932450[9] = 0;
out_1516634108393932450[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
out_3144072620339266513[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
void h_20(double *state, double *sat_pos, double *out_1322673160422067744) {
out_1322673160422067744[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
}
void H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
out_8303647801952025828[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8303647801952025828[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8303647801952025828[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_8303647801952025828[3] = 0;
out_8303647801952025828[4] = 0;
out_8303647801952025828[5] = 0;
out_8303647801952025828[6] = 1;
out_8303647801952025828[7] = 0;
out_8303647801952025828[8] = 0;
out_8303647801952025828[9] = 1;
out_8303647801952025828[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_6746823634573947585) {
out_6746823634573947585[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_6746823634573947585[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_6746823634573947585[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_6746823634573947585[3] = 0;
out_6746823634573947585[4] = 0;
out_6746823634573947585[5] = 0;
out_6746823634573947585[6] = 1;
out_6746823634573947585[7] = 0;
out_6746823634573947585[8] = 0;
out_6746823634573947585[9] = 1;
out_6746823634573947585[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
out_3768133833178917308[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[6] = 0;
out_1168235388167943403[7] = 1;
out_1168235388167943403[8] = 0;
out_1168235388167943403[9] = 0;
out_1168235388167943403[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
out_6978532755514735815[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[6] = 0;
out_6978532755514735815[7] = 1;
out_6978532755514735815[8] = 0;
out_6978532755514735815[9] = 0;
out_6978532755514735815[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
out_7730590468597126339[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
out_3768133833178917308[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
out_1168235388167943403[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_1168235388167943403[6] = 0;
out_1168235388167943403[7] = 1;
out_1168235388167943403[8] = 0;
out_1168235388167943403[9] = 0;
out_1168235388167943403[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
out_6978532755514735815[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_6978532755514735815[6] = 0;
out_6978532755514735815[7] = 1;
out_6978532755514735815[8] = 0;
out_6978532755514735815[9] = 0;
out_6978532755514735815[10] = 0;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660) {
err_fun(nom_x, delta_x, out_2351947283659598660);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745) {
err_fun(nom_x, delta_x, out_4281415265925619745);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
inv_err_fun(nom_x, true_x, out_8539310053726918653);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220) {
inv_err_fun(nom_x, true_x, out_8058048663833159220);
}
void gnss_H_mod_fun(double *state, double *out_2843304307356815987) {
H_mod_fun(state, out_2843304307356815987);
void gnss_H_mod_fun(double *state, double *out_3690945488544052836) {
H_mod_fun(state, out_3690945488544052836);
}
void gnss_f_fun(double *state, double dt, double *out_774769938980785647) {
f_fun(state, dt, out_774769938980785647);
void gnss_f_fun(double *state, double dt, double *out_2954456239899519205) {
f_fun(state, dt, out_2954456239899519205);
}
void gnss_F_fun(double *state, double dt, double *out_4467768114936024642) {
F_fun(state, dt, out_4467768114936024642);
void gnss_F_fun(double *state, double dt, double *out_709514566261682476) {
F_fun(state, dt, out_709514566261682476);
}
void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
h_6(state, sat_pos, out_8206867657584711155);
void gnss_h_6(double *state, double *sat_pos, double *out_2648966289655583926) {
h_6(state, sat_pos, out_2648966289655583926);
}
void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
H_6(state, sat_pos, out_2748205046550560630);
void gnss_H_6(double *state, double *sat_pos, double *out_1516634108393932450) {
H_6(state, sat_pos, out_1516634108393932450);
}
void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
h_20(state, sat_pos, out_3144072620339266513);
void gnss_h_20(double *state, double *sat_pos, double *out_1322673160422067744) {
h_20(state, sat_pos, out_1322673160422067744);
}
void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
H_20(state, sat_pos, out_8303647801952025828);
void gnss_H_20(double *state, double *sat_pos, double *out_6746823634573947585) {
H_20(state, sat_pos, out_6746823634573947585);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
h_7(state, sat_pos_vel, out_7730590468597126339);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
h_7(state, sat_pos_vel, out_3768133833178917308);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
H_7(state, sat_pos_vel, out_1168235388167943403);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
H_7(state, sat_pos_vel, out_6978532755514735815);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
h_21(state, sat_pos_vel, out_7730590468597126339);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308) {
h_21(state, sat_pos_vel, out_3768133833178917308);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
H_21(state, sat_pos_vel, out_1168235388167943403);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815) {
H_21(state, sat_pos_vel, out_6978532755514735815);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/gnss.h
+13 -13
View File
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_2351947283659598660);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653);
void gnss_H_mod_fun(double *state, double *out_2843304307356815987);
void gnss_f_fun(double *state, double dt, double *out_774769938980785647);
void gnss_F_fun(double *state, double dt, double *out_4467768114936024642);
void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155);
void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630);
void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513);
void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_4281415265925619745);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8058048663833159220);
void gnss_H_mod_fun(double *state, double *out_3690945488544052836);
void gnss_f_fun(double *state, double dt, double *out_2954456239899519205);
void gnss_F_fun(double *state, double dt, double *out_709514566261682476);
void gnss_h_6(double *state, double *sat_pos, double *out_2648966289655583926);
void gnss_H_6(double *state, double *sat_pos, double *out_1516634108393932450);
void gnss_h_20(double *state, double *sat_pos, double *out_1322673160422067744);
void gnss_H_20(double *state, double *sat_pos, double *out_6746823634573947585);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_3768133833178917308);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_6978532755514735815);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_3768133833178917308);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_6978532755514735815);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.cpp
+1702 -1702
View File
File diff suppressed because it is too large Load Diff
selfdrive/locationd/models/generated/live.h
+24 -24
View File
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_H(double *in_vec, double *out_8687680363762493539);
void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885);
void live_H_mod_fun(double *state, double *out_7453067886472422609);
void live_f_fun(double *state, double dt, double *out_3547963935686363060);
void live_F_fun(double *state, double dt, double *out_3254137990445528781);
void live_h_4(double *state, double *unused, double *out_4410787759120580449);
void live_H_4(double *state, double *unused, double *out_5512660098689845475);
void live_h_9(double *state, double *unused, double *out_4264504783842197092);
void live_H_9(double *state, double *unused, double *out_5271470452060254830);
void live_h_10(double *state, double *unused, double *out_1276705416788479855);
void live_H_10(double *state, double *unused, double *out_6589591562994162559);
void live_h_12(double *state, double *unused, double *out_4489758644873872208);
void live_H_12(double *state, double *unused, double *out_493203690657883680);
void live_h_35(double *state, double *unused, double *out_8696122944070004010);
void live_H_35(double *state, double *unused, double *out_2252359341667130029);
void live_h_32(double *state, double *unused, double *out_2592146495080937647);
void live_H_32(double *state, double *unused, double *out_8522971196901590084);
void live_h_13(double *state, double *unused, double *out_5320318911182793356);
void live_H_13(double *state, double *unused, double *out_6260235854627074248);
void live_h_14(double *state, double *unused, double *out_4264504783842197092);
void live_H_14(double *state, double *unused, double *out_5271470452060254830);
void live_h_33(double *state, double *unused, double *out_57093419937807770);
void live_H_33(double *state, double *unused, double *out_5402916346305987633);
void live_H(double *in_vec, double *out_907497564161450989);
void live_err_fun(double *nom_x, double *delta_x, double *out_403017012035771763);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_2932713747370631150);
void live_H_mod_fun(double *state, double *out_6036032580712144890);
void live_f_fun(double *state, double dt, double *out_653667760006229873);
void live_F_fun(double *state, double dt, double *out_8944506884882647575);
void live_h_4(double *state, double *unused, double *out_6967130088459663654);
void live_H_4(double *state, double *unused, double *out_2805776108884249108);
void live_h_9(double *state, double *unused, double *out_5944074116138401933);
void live_H_9(double *state, double *unused, double *out_2564586462254658463);
void live_h_10(double *state, double *unused, double *out_3024176660760462318);
void live_H_10(double *state, double *unused, double *out_3594957374532062379);
void live_h_12(double *state, double *unused, double *out_1781567485764299655);
void live_H_12(double *state, double *unused, double *out_2213680299147712687);
void live_h_35(double *state, double *unused, double *out_3392135172365373817);
void live_H_35(double *state, double *unused, double *out_4959243331472726396);
void live_h_32(double *state, double *unused, double *out_5713026107165599538);
void live_H_32(double *state, double *unused, double *out_8507087671459885125);
void live_h_13(double *state, double *unused, double *out_8591731605088062953);
void live_H_13(double *state, double *unused, double *out_1226506526531533769);
void live_h_14(double *state, double *unused, double *out_5944074116138401933);
void live_H_14(double *state, double *unused, double *out_2564586462254658463);
void live_h_33(double *state, double *unused, double *out_3376577180674409820);
void live_H_33(double *state, double *unused, double *out_3711442953127215872);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/ubloxd
BIN
View File
Binary file not shown.
selfdrive/manager/manager.py
+1 -1
View File
@@ -141,7 +141,7 @@ def manager_thread() -> None:
dp_mapd = params.get_bool('dp_mapd')
ignore += ['mapd'] if not dp_mapd else []
ignore += ['gpxd'] if not dp_otisserv and not dp_mapd and not params.get_bool('dp_gpxd') else []
ignore += ['uploader'] if not params.get_bool('dp_api_custom') and (int(params.get('dp_atl', encoding='utf8')) > 0 or params.get_bool('dp_jetson')) else []
ignore += ['uploader'] if not params.get_bool('dp_api_custom') and params.get_bool('dp_jetson') else []
if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
ignore += ["manage_athenad", "uploader"]
selfdrive/modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/sensord/_sensord
BIN
View File
Binary file not shown.
selfdrive/ui/_ui
BIN
View File
Binary file not shown.
selfdrive/ui/_ui_nonav
BIN
View File
Binary file not shown.
selfdrive/ui/qt/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/qt/text
BIN
View File
Binary file not shown.
selfdrive/ui/soundd/_soundd
BIN
View File
Binary file not shown.
selfdrive/updated.py
+1 -1
View File
@@ -222,7 +222,7 @@ def handle_agnos_update() -> None:
set_offroad_alert("Offroad_NeosUpdate", False)
def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
def handle_neos_update() -> None:
from system.hardware.eon.neos import download_neos_update
cur_neos = HARDWARE.get_os_version()
system/camerad/camerad
BIN
View File
Binary file not shown.