diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 2c376df97..68cfe0b03 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -4,6 +4,8 @@ dragonpilot [latest] - EON/C2 Release * NEW: Added back auto shutdown toggle. * NEW: Added back On-Road Dashcam toggle. (You should not replace this with your dashcam!!!) * NEW: Added back LQR tune toggle. +* NEW: MAZDA - Ability to enable/disable below steer speed alert. (Thanks to @TheCrowd) +* NEW: Included all vehicles. * TWEAK: Panda Recovery should kill boardd process first. * FIXED: Fixed stop working issue when mapd crashed. @@ -41,9 +43,14 @@ dragonpilot 2022.07.12 - EON/C2 Release * Works on EON/C2 + white/grey/black panda (tested only on Toyotas). * Buggy! -dragonpilot 2022.06.25 +dragonpilot [2022.08.07] ======================== * Synced to openpilot master 2022.08.07 commits. +* NEW: Added back auto shutdown toggle. +* NEW: Added back LQR tune toggle. +* NEW: MAZDA - Ability to enable/disable below steer speed alert. (Thanks to @TheCrowd) +* TWEAKED: Panda Recovery should kill boardd process first. +* FIXED: Fixed stop working issue when mapd crashed. dragonpilot 2022.07.27 ======================== diff --git a/cereal/dp.capnp b/cereal/dp.capnp index 1b3735378..11e2e13c5 100644 --- a/cereal/dp.capnp +++ b/cereal/dp.capnp @@ -25,4 +25,5 @@ struct DragonConf { dpUseLanelines @17 :Bool; dpMapd @18 :Bool; dpDashcamd @19 :Bool; + dpMazdaSteerAlert @20 :Bool; } diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index fb2e801ea..c9c97f18e 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/cereal/messaging/messaging_pyx.cpp b/cereal/messaging/messaging_pyx.cpp index 534740c01..e6cf5df84 100644 --- a/cereal/messaging/messaging_pyx.cpp +++ b/cereal/messaging/messaging_pyx.cpp @@ -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 #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); } diff --git a/cereal/messaging/messaging_pyx.so b/cereal/messaging/messaging_pyx.so index e5973fa87..468a30848 100755 Binary files a/cereal/messaging/messaging_pyx.so and b/cereal/messaging/messaging_pyx.so differ diff --git a/cereal/visionipc/visionipc_pyx.so b/cereal/visionipc/visionipc_pyx.so index 1f575f26e..9b292d6a9 100755 Binary files a/cereal/visionipc/visionipc_pyx.so and b/cereal/visionipc/visionipc_pyx.so differ diff --git a/common/clock.cpp b/common/clock.cpp index c2a5b2412..887714eb4 100644 --- a/common/clock.cpp +++ b/common/clock.cpp @@ -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 #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); } diff --git a/common/clock.so b/common/clock.so index 93fbdf0cb..849db7ad2 100755 Binary files a/common/clock.so and b/common/clock.so differ diff --git a/common/dp_conf.py b/common/dp_conf.py index 9cee66fc0..7311ae4e6 100644 --- a/common/dp_conf.py +++ b/common/dp_conf.py @@ -80,6 +80,8 @@ confs = [ # # auto shutdown {'name': 'dp_auto_shutdown', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, {'name': 'dp_auto_shutdown_in', 'default': 90, 'type': 'UInt16', 'min': 0, 'max': 600, 'depends': [{'name': 'dp_auto_shutdown', 'vals': [True]}], 'conf_type': ['param']}, + + {'name': 'dp_mazda_steer_alert', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']}, # # service # {'name': 'dp_updated', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, # {'name': 'dp_logger', 'default': False, 'type': 'Bool', 'conf_type': ['param']}, diff --git a/common/kalman/simple_kalman_impl.cpp b/common/kalman/simple_kalman_impl.cpp index faa41d50f..5d5b9ee54 100644 --- a/common/kalman/simple_kalman_impl.cpp +++ b/common/kalman/simple_kalman_impl.cpp @@ -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 #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); } diff --git a/common/kalman/simple_kalman_impl.so b/common/kalman/simple_kalman_impl.so index 4fb04cb93..a6e48593a 100755 Binary files a/common/kalman/simple_kalman_impl.so and b/common/kalman/simple_kalman_impl.so differ diff --git a/common/params_pyx.so b/common/params_pyx.so index aaa763690..7d76f25dd 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/common/transformations/transformations.cpp b/common/transformations/transformations.cpp index 4080d4a8b..f08fb62f8 100644 --- a/common/transformations/transformations.cpp +++ b/common/transformations/transformations.cpp @@ -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 #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); } diff --git a/common/transformations/transformations.so b/common/transformations/transformations.so index dd810f7f0..f168bb89b 100755 Binary files a/common/transformations/transformations.so and b/common/transformations/transformations.so differ diff --git a/common/version.h b/common/version.h index 5a230697f..e0cc35f43 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "2022.08.11" +#define COMMA_VERSION "2022.08.17" diff --git a/opendbc/can/libdbc.so b/opendbc/can/libdbc.so index 8e6b13ba2..03f1951eb 100755 Binary files a/opendbc/can/libdbc.so and b/opendbc/can/libdbc.so differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 155f4f9a7..06582823b 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index dc8200df4..2765b8d98 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda.bin.sspoof.signed b/panda/board/obj/panda.bin.sspoof.signed index ffbacf9cf..8425f842d 100644 Binary files a/panda/board/obj/panda.bin.sspoof.signed and b/panda/board/obj/panda.bin.sspoof.signed differ diff --git a/panda/board/obj/panda.bin.testing.signed b/panda/board/obj/panda.bin.testing.signed index b490d6711..9df478d0b 100644 Binary files a/panda/board/obj/panda.bin.testing.signed and b/panda/board/obj/panda.bin.testing.signed differ diff --git a/panda/board/obj/panda.bin.testing.sspoof.signed b/panda/board/obj/panda.bin.testing.sspoof.signed index 8a524239e..a09eb3d94 100644 Binary files a/panda/board/obj/panda.bin.testing.sspoof.signed and b/panda/board/obj/panda.bin.testing.sspoof.signed differ diff --git a/rednose/helpers/ekf_sym_pyx.so b/rednose/helpers/ekf_sym_pyx.so index 339a99818..3683f2adf 100755 Binary files a/rednose/helpers/ekf_sym_pyx.so and b/rednose/helpers/ekf_sym_pyx.so differ diff --git a/selfdrive/assets/locales/ja-JP/LC_MESSAGES/events.mo b/selfdrive/assets/locales/ja-JP/LC_MESSAGES/events.mo index 449f39fcb..2fd9d8964 100644 Binary files a/selfdrive/assets/locales/ja-JP/LC_MESSAGES/events.mo and b/selfdrive/assets/locales/ja-JP/LC_MESSAGES/events.mo differ diff --git a/selfdrive/assets/locales/ko-KR/LC_MESSAGES/events.mo b/selfdrive/assets/locales/ko-KR/LC_MESSAGES/events.mo index 4d5a66178..6ff1e54b6 100644 Binary files a/selfdrive/assets/locales/ko-KR/LC_MESSAGES/events.mo and b/selfdrive/assets/locales/ko-KR/LC_MESSAGES/events.mo differ diff --git a/selfdrive/assets/locales/zh-CN/LC_MESSAGES/events.mo b/selfdrive/assets/locales/zh-CN/LC_MESSAGES/events.mo index 68ed6e5ba..8ca4c78f8 100644 Binary files a/selfdrive/assets/locales/zh-CN/LC_MESSAGES/events.mo and b/selfdrive/assets/locales/zh-CN/LC_MESSAGES/events.mo differ diff --git a/selfdrive/assets/locales/zh-TW/LC_MESSAGES/events.mo b/selfdrive/assets/locales/zh-TW/LC_MESSAGES/events.mo index c88079f2e..98e9c6bca 100644 Binary files a/selfdrive/assets/locales/zh-TW/LC_MESSAGES/events.mo and b/selfdrive/assets/locales/zh-TW/LC_MESSAGES/events.mo differ diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 32bc92059..93ba6964a 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -85,8 +85,15 @@ def register(show_spinner=False) -> Optional[str]: backoff = min(backoff + 1, 15) time.sleep(backoff) - if time.monotonic() - start_time > 60 and show_spinner: - spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") + time_diff = time.monotonic() - start_time + if time_diff > 29 and show_spinner: + timeout = 30 - time_diff + spinner.update(f"registering device ({timeout}) - serial: {serial}, IMEI: ({imei1}, {imei2})") + + # go unregistered device + if time.monotonic() - start_time > 30 and show_spinner: + dongle_id = UNREGISTERED_DONGLE_ID + break if show_spinner: spinner.close() diff --git a/selfdrive/boardd/boardd b/selfdrive/boardd/boardd index 7553d8bdc..c4f3f940e 100755 Binary files a/selfdrive/boardd/boardd and b/selfdrive/boardd/boardd differ diff --git a/selfdrive/boardd/boardd_api_impl.cpp b/selfdrive/boardd/boardd_api_impl.cpp index 814e8a01d..a17d8b490 100644 --- a/selfdrive/boardd/boardd_api_impl.cpp +++ b/selfdrive/boardd/boardd_api_impl.cpp @@ -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 #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); } diff --git a/selfdrive/boardd/boardd_api_impl.so b/selfdrive/boardd/boardd_api_impl.so index 87d774858..7065b1598 100755 Binary files a/selfdrive/boardd/boardd_api_impl.so and b/selfdrive/boardd/boardd_api_impl.so differ diff --git a/selfdrive/car/body/__init__.py b/selfdrive/car/body/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/body/bodycan.py b/selfdrive/car/body/bodycan.py new file mode 100644 index 000000000..580e5025a --- /dev/null +++ b/selfdrive/car/body/bodycan.py @@ -0,0 +1,7 @@ +def create_control(packer, torque_l, torque_r): + values = { + "TORQUE_L": torque_l, + "TORQUE_R": torque_r, + } + + return packer.make_can_msg("TORQUE_CMD", 0, values) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py new file mode 100644 index 000000000..0d5d780bd --- /dev/null +++ b/selfdrive/car/body/carcontroller.py @@ -0,0 +1,90 @@ +import numpy as np + +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car.body import bodycan +from selfdrive.car.body.values import SPEED_FROM_RPM +from selfdrive.controls.lib.pid import PIDController + + +MAX_TORQUE = 500 +MAX_TORQUE_RATE = 50 +MAX_ANGLE_ERROR = np.radians(7) +MAX_POS_INTEGRATOR = 0.2 # meters +MAX_TURN_INTEGRATOR = 0.1 # meters + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.frame = 0 + self.packer = CANPacker(dbc_name) + + # Speed, balance and turn PIDs + self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL) + self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL) + self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) + + self.torque_r_filtered = 0. + self.torque_l_filtered = 0. + + @staticmethod + def deadband_filter(torque, deadband): + if torque > 0: + torque += deadband + else: + torque -= deadband + return torque + + def update(self, CC, CS): + + torque_l = 0 + torque_r = 0 + + llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0 + if CC.enabled and llk_valid: + # Read these from the joystick + # TODO: this isn't acceleration, okay? + speed_desired = CC.actuators.accel / 5. + speed_diff_desired = -CC.actuators.steer + + speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. + speed_error = speed_desired - speed_measured + + freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or + (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR)) + angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator) + + # Clip angle error, this is enough to get up from stands + angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR) + angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.) + torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate) + + speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) + turn_error = speed_diff_measured - speed_diff_desired + freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or + (turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR)) + torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator) + + # Combine 2 PIDs outputs + torque_r = torque + torque_diff + torque_l = torque - torque_diff + + # Torque rate limits + self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10), + self.torque_r_filtered - MAX_TORQUE_RATE, + self.torque_r_filtered + MAX_TORQUE_RATE) + self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), + self.torque_l_filtered - MAX_TORQUE_RATE, + self.torque_l_filtered + MAX_TORQUE_RATE) + torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) + torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) + + can_sends = [] + can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) + + new_actuators = CC.actuators.copy() + new_actuators.accel = torque_l + new_actuators.steer = torque_r + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py new file mode 100644 index 000000000..dbbd85950 --- /dev/null +++ b/selfdrive/car/body/carstate.py @@ -0,0 +1,60 @@ +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.body.values import DBC + +STARTUP_TICKS = 100 + +class CarState(CarStateBase): + def update(self, cp): + ret = car.CarState.new_message() + + ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] + ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R'] + + ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor + + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = False + + ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'], + cp.vl['VAR_VALUES']['FAULT']]) + + ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1 + ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100 + + # irrelevant for non-car + ret.gearShifter = car.CarState.GearShifter.drive + ret.cruiseState.enabled = True + ret.cruiseState.available = True + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("SPEED_L", "MOTORS_DATA"), + ("SPEED_R", "MOTORS_DATA"), + ("ELEC_ANGLE_L", "MOTORS_DATA"), + ("ELEC_ANGLE_R", "MOTORS_DATA"), + ("COUNTER", "MOTORS_DATA"), + ("CHECKSUM", "MOTORS_DATA"), + ("IGNITION", "VAR_VALUES"), + ("ENABLE_MOTORS", "VAR_VALUES"), + ("FAULT", "VAR_VALUES"), + ("MOTOR_ERR_L", "VAR_VALUES"), + ("MOTOR_ERR_R", "VAR_VALUES"), + ("MCU_TEMP", "BODY_DATA"), + ("BATT_VOLTAGE", "BODY_DATA"), + ("BATT_PERCENTAGE", "BODY_DATA"), + ("CHARGER_CONNECTED", "BODY_DATA"), + ] + + checks = [ + ("MOTORS_DATA", 100), + ("VAR_VALUES", 10), + ("BODY_DATA", 1), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py new file mode 100644 index 000000000..e85735b1a --- /dev/null +++ b/selfdrive/car/body/interface.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import math +from cereal import car +from common.realtime import DT_CTRL +from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.body.values import SPEED_FROM_RPM + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False): + + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.notCar = True + ret.carName = "body" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] + + ret.minSteerSpeed = -math.inf + ret.maxLateralAccel = math.inf # TODO: set to a reasonable value + ret.steerRatio = 0.5 + ret.steerLimitTimer = 1.0 + ret.steerActuatorDelay = 0. + + ret.mass = 9 + ret.wheelbase = 0.406 + ret.wheelSpeedFactor = SPEED_FROM_RPM + ret.centerToFront = ret.wheelbase * 0.44 + + ret.radarOffCan = True + ret.openpilotLongitudinalControl = True + ret.steerControlType = car.CarParams.SteerControlType.angle + + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + def _update(self, c): + ret = self.CS.update(self.cp) + + # wait for everything to init first + if self.frame > int(5. / DT_CTRL): + # body always wants to enable + ret.init('events', 1) + ret.events[0].name = car.CarEvent.EventName.pcmEnable + ret.events[0].enable = True + self.frame += 1 + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/body/radar_interface.py b/selfdrive/car/body/radar_interface.py new file mode 100644 index 000000000..b2f765113 --- /dev/null +++ b/selfdrive/car/body/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py new file mode 100644 index 000000000..0caf93b61 --- /dev/null +++ b/selfdrive/car/body/values.py @@ -0,0 +1,38 @@ +from typing import Dict + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +Ecu = car.CarParams.Ecu + +SPEED_FROM_RPM = 0.008587 + +class CarControllerParams: + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower + STEER_THRESHOLD = 1.0 + +class CAR: + BODY = "COMMA BODY" + +CAR_INFO: Dict[str, CarInfo] = { + CAR.BODY: CarInfo("comma body", package="All", harness=Harness.none), +} + +FW_VERSIONS = { + CAR.BODY: { + (Ecu.engine, 0x720, None): [ + b'0.0.01', + b'02/27/2022' + ], + (Ecu.debug, 0x721, None): [ + b'166bd860' # git hash of the firmware used + ], + }, +} + +DBC = { + CAR.BODY: dbc_dict('comma_body', None), +} diff --git a/selfdrive/car/chrysler/__init__.py b/selfdrive/car/chrysler/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py new file mode 100644 index 000000000..22abf1b67 --- /dev/null +++ b/selfdrive/car/chrysler/carcontroller.py @@ -0,0 +1,81 @@ +from opendbc.can.packer import CANPacker +from common.realtime import DT_CTRL +from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons +from selfdrive.car.chrysler.values import CAR, RAM_CARS, CarControllerParams + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.apply_steer_last = 0 + self.frame = 0 + + self.hud_count = 0 + self.last_lkas_falling_edge = 0 + self.lkas_control_bit_prev = False + self.last_button_frame = 0 + + self.packer = CANPacker(dbc_name) + self.params = CarControllerParams(CP) + + def update(self, CC, CS): + can_sends = [] + + # TODO: can we make this more sane? why is it different for all the cars? + lkas_control_bit = self.lkas_control_bit_prev + if CS.out.vEgo > self.CP.minSteerSpeed: + lkas_control_bit = True + elif self.CP.carFingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0): + lkas_control_bit = False + elif self.CP.carFingerprint in RAM_CARS: + if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5): + lkas_control_bit = False + + # EPS faults if LKAS re-enables too quickly + lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200) + lkas_active = CC.latActive and self.lkas_control_bit_prev + + # *** control msgs *** + + # cruise buttons + if (self.frame - self.last_button_frame)*DT_CTRL > 0.05: + das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0 + + # ACC cancellation + if CC.cruiseControl.cancel: + self.last_button_frame = self.frame + can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True)) + + # ACC resume from standstill + elif CC.cruiseControl.resume: + self.last_button_frame = self.frame + can_sends.append(create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True)) + + # HUD alerts + if self.frame % 25 == 0: + if CS.lkas_car_model != -1: + can_sends.append(create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) + self.hud_count += 1 + + # steering + if self.frame % 2 == 0: + # steer torque + new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) + if not lkas_active: + apply_steer = 0 + self.apply_steer_last = apply_steer + + can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit)) + + self.frame += 1 + if not lkas_control_bit and self.lkas_control_bit_prev: + self.last_lkas_falling_edge = self.frame + self.lkas_control_bit_prev = lkas_control_bit + + new_actuators = CC.actuators.copy() + new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + + return new_actuators, can_sends diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py new file mode 100644 index 000000000..fefd351a2 --- /dev/null +++ b/selfdrive/car/chrysler/carstate.py @@ -0,0 +1,205 @@ +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.CP = CP + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + self.auto_high_beam = 0 + self.button_counter = 0 + self.lkas_car_model = -1 + + if CP.carFingerprint in RAM_CARS: + self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"] + else: + self.shifter_values = can_define.dv["GEAR"]["PRNDL"] + + def update(self, cp, cp_cam): + + ret = car.CarState.new_message() + + # lock info + ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], + cp.vl["BCM_1"]["DOOR_OPEN_FR"], + cp.vl["BCM_1"]["DOOR_OPEN_RL"], + cp.vl["BCM_1"]["DOOR_OPEN_RR"]]) + ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1 + + # brake pedal + ret.brake = 0 + ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch + + # gas pedal + ret.gas = cp.vl["ECM_5"]["Accelerator_Position"] + ret.gasPressed = ret.gas > 1e-5 + + # car speed + if self.CP.carFingerprint in RAM_CARS: + ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None)) + else: + ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2. + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None)) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = not ret.vEgoRaw > 0.001 + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["ESP_6"]["WHEEL_SPEED_FL"], + cp.vl["ESP_6"]["WHEEL_SPEED_FR"], + cp.vl["ESP_6"]["WHEEL_SPEED_RL"], + cp.vl["ESP_6"]["WHEEL_SPEED_RR"], + unit=1, + ) + + # button presses + ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1 + ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2 + ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1 + + # steering wheel + ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"] + ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"] + ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"] + ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + + # cruise state + cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp + + ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1 + ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1 + ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS + ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet + ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1 + ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0 + + if self.CP.carFingerprint in RAM_CARS: + self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON'] # Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message + ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1 + else: + ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4 + + # blindspot sensors + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1 + ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1 + + self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] + self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] + + return ret + + @staticmethod + def get_cruise_signals(): + signals = [ + ("ACC_AVAILABLE", "DAS_3"), + ("ACC_ACTIVE", "DAS_3"), + ("ACC_FAULTED", "DAS_3"), + ("ACC_STANDSTILL", "DAS_3"), + ("COUNTER", "DAS_3"), + ("ACC_SET_SPEED_KPH", "DAS_4"), + ("ACC_STATE", "DAS_4"), + ] + checks = [ + ("DAS_3", 50), + ("DAS_4", 50), + ] + return signals, checks + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("DOOR_OPEN_FL", "BCM_1"), + ("DOOR_OPEN_FR", "BCM_1"), + ("DOOR_OPEN_RL", "BCM_1"), + ("DOOR_OPEN_RR", "BCM_1"), + ("Brake_Pedal_State", "ESP_1"), + ("Accelerator_Position", "ECM_5"), + ("WHEEL_SPEED_FL", "ESP_6"), + ("WHEEL_SPEED_RR", "ESP_6"), + ("WHEEL_SPEED_RL", "ESP_6"), + ("WHEEL_SPEED_FR", "ESP_6"), + ("STEERING_ANGLE", "STEERING"), + ("STEERING_ANGLE_HP", "STEERING"), + ("STEERING_RATE", "STEERING"), + ("TURN_SIGNALS", "STEERING_LEVERS"), + ("HIGH_BEAM_PRESSED", "STEERING_LEVERS"), + ("SEATBELT_DRIVER_UNLATCHED", "ORC_1"), + ("COUNTER", "EPS_2",), + ("COLUMN_TORQUE", "EPS_2"), + ("EPS_TORQUE_MOTOR", "EPS_2"), + ("LKAS_STATE", "EPS_2"), + ("COUNTER", "CRUISE_BUTTONS"), + ] + + checks = [ + # sig_address, frequency + ("ESP_1", 50), + ("EPS_2", 100), + ("ESP_6", 50), + ("STEERING", 100), + ("ECM_5", 50), + ("CRUISE_BUTTONS", 50), + ("STEERING_LEVERS", 10), + ("ORC_1", 2), + ("BCM_1", 1), + ] + + if CP.enableBsm: + signals += [ + ("RIGHT_STATUS", "BSM_1"), + ("LEFT_STATUS", "BSM_1"), + ] + checks.append(("BSM_1", 2)) + + if CP.carFingerprint in RAM_CARS: + signals += [ + ("DASM_FAULT", "EPS_3"), + ("Vehicle_Speed", "ESP_8"), + ("Gear_State", "Transmission_Status"), + ] + checks += [ + ("ESP_8", 50), + ("EPS_3", 50), + ("Transmission_Status", 50), + ] + else: + signals += [ + ("PRNDL", "GEAR"), + ("SPEED_LEFT", "SPEED_1"), + ("SPEED_RIGHT", "SPEED_1"), + ] + checks += [ + ("GEAR", 50), + ("SPEED_1", 100), + ] + signals += CarState.get_cruise_signals()[0] + checks += CarState.get_cruise_signals()[1] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address, default + ("CAR_MODEL", "DAS_6"), + ] + checks = [ + ("DAS_6", 4), + ] + + if CP.carFingerprint in RAM_CARS: + signals += [ + ("AUTO_HIGH_BEAM_ON", "DAS_6"), + ] + signals += CarState.get_cruise_signals()[0] + checks += CarState.get_cruise_signals()[1] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py new file mode 100644 index 000000000..10ed73e9f --- /dev/null +++ b/selfdrive/car/chrysler/chryslercan.py @@ -0,0 +1,71 @@ +from cereal import car +from selfdrive.car.chrysler.values import RAM_CARS + +GearShifter = car.CarState.GearShifter +VisualAlert = car.CarControl.HUDControl.VisualAlert + +def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): + # LKAS_HUD - Controls what lane-keeping icon is displayed + + # == Color == + # 0 hidden? + # 1 white + # 2 green + # 3 ldw + + # == Lines == + # 03 white Lines + # 04 grey lines + # 09 left lane close + # 0A right lane close + # 0B left Lane very close + # 0C right Lane very close + # 0D left cross cross + # 0E right lane cross + + # == Alerts == + # 7 Normal + # 6 lane departure place hands on wheel + + color = 2 if lkas_active else 1 + lines = 3 if lkas_active else 0 + alerts = 7 if lkas_active else 0 + + if hud_count < (1 * 4): # first 3 seconds, 4Hz + alerts = 1 + + if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired): + color = 4 + lines = 0 + alerts = 6 + + values = { + "LKAS_ICON_COLOR": color, + "CAR_MODEL": car_model, + "LKAS_LANE_LINES": lines, + "LKAS_ALERTS": alerts, + } + + if CP.carFingerprint in RAM_CARS: + values['AUTO_HIGH_BEAM_ON'] = auto_high_beam + + return packer.make_can_msg("DAS_6", 0, values) + + +def create_lkas_command(packer, CP, apply_steer, lkas_control_bit): + # LKAS_COMMAND Lane-keeping signal to turn the wheel + enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1 + values = { + "STEERING_TORQUE": apply_steer, + "LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0, + } + return packer.make_can_msg("LKAS_COMMAND", 0, values) + + +def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False): + values = { + "ACC_Cancel": cancel, + "ACC_Resume": resume, + "COUNTER": frame % 0x10, + } + return packer.make_can_msg("CRUISE_BUTTONS", bus, values) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py new file mode 100755 index 000000000..31a0fccf5 --- /dev/null +++ b/selfdrive/car/chrysler/interface.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.chrysler.values import CAR, DBC, RAM_CARS +from selfdrive.car.interfaces import CarInterfaceBase + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "chrysler" + + ret.radarOffCan = DBC[candidate]['radar'] is None + + param = Panda.FLAG_CHRYSLER_RAM_DT if candidate in RAM_CARS else None + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler, param)] + + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.4 + + ret.minSteerSpeed = 3.8 # m/s + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019): + # TODO: allow 2019 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. + + # Chrysler + if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020): + ret.mass = 2242. + STD_CARGO_KG + ret.wheelbase = 3.089 + ret.steerRatio = 16.2 # Pacifica Hybrid 2017 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 + + # Jeep + elif candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): + ret.mass = 1778 + STD_CARGO_KG + ret.wheelbase = 2.71 + ret.steerRatio = 16.7 + ret.steerActuatorDelay = 0.2 + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] + ret.lateralTuning.pid.kf = 0.00006 + + # Ram + elif candidate == CAR.RAM_1500: + ret.steerActuatorDelay = 0.2 + + ret.wheelbase = 3.88 + ret.steerRatio = 16.3 + ret.mass = 2493. + STD_CARGO_KG + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + + ret.minSteerSpeed = 14.5 + if car_fw is not None: + for fw in car_fw: + if fw.ecu == 'eps' and fw.fwVersion in (b"68312176AE", b"68312176AG", b"68273275AG"): + ret.minSteerSpeed = 0. + + else: + raise ValueError(f"Unsupported car: {candidate}") + + ret.centerToFront = ret.wheelbase * 0.44 + + # starting with reasonable value for civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # 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) + + ret.enableBsm = 720 in fingerprint[0] + + return ret + + 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): + self.low_speed_alert = True + elif ret.vEgo > (self.CP.minSteerSpeed + 1.): + self.low_speed_alert = False + if self.low_speed_alert: + events.add(car.CarEvent.EventName.belowSteerSpeed) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py new file mode 100755 index 000000000..348e3c363 --- /dev/null +++ b/selfdrive/car/chrysler/radar_interface.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +from opendbc.can.parser import CANParser +from cereal import car +from selfdrive.car.interfaces import RadarInterfaceBase +from selfdrive.car.chrysler.values import DBC + +RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724 +RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages +LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D) +NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) + +def _create_radar_can_parser(car_fingerprint): + dbc = DBC[car_fingerprint]['radar'] + if dbc is None: + return None + + msg_n = len(RADAR_MSGS_C) + # list of [(signal name, message name or number), (...)] + # [('RADAR_STATE', 1024), + # ('LONG_DIST', 1072), + # ('LONG_DIST', 1073), + # ('LONG_DIST', 1074), + # ('LONG_DIST', 1075), + + signals = list(zip(['LONG_DIST'] * msg_n + + ['LAT_DIST'] * msg_n + + ['REL_SPEED'] * msg_n, + RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST + RADAR_MSGS_D)) # REL_SPEED + + checks = list(zip(RADAR_MSGS_C + + RADAR_MSGS_D, + [20] * msg_n + # 20Hz (0.05s) + [20] * msg_n)) # 20Hz (0.05s) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) + +def _address_to_track(address): + if address in RADAR_MSGS_C: + return (address - RADAR_MSGS_C[0]) // 2 + if address in RADAR_MSGS_D: + return (address - RADAR_MSGS_D[0]) // 2 + raise ValueError("radar received unexpected address %d" % address) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.rcp = _create_radar_can_parser(CP.carFingerprint) + self.updated_messages = set() + self.trigger_msg = LAST_MSG + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + for ii in self.updated_messages: # ii should be the message ID as a number + cpt = self.rcp.vl[ii] + trackId = _address_to_track(ii) + + if trackId not in self.pts: + self.pts[trackId] = car.RadarData.RadarPoint.new_message() + self.pts[trackId].trackId = trackId + self.pts[trackId].aRel = float('nan') + self.pts[trackId].yvRel = float('nan') + self.pts[trackId].measured = True + + if 'LONG_DIST' in cpt: # c_* message + self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car + # our lat_dist is positive to the right in car's frame. + # TODO what does yRel want? + self.pts[trackId].yRel = cpt['LAT_DIST'] # in car frame's y axis, left is positive + else: # d_* message + self.pts[trackId].vRel = cpt['REL_SPEED'] + + # We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid. + ret.points = [x for x in self.pts.values() if x.dRel != 0] + + self.updated_messages.clear() + return ret \ No newline at end of file diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py new file mode 100644 index 000000000..c45a81a0a --- /dev/null +++ b/selfdrive/car/chrysler/values.py @@ -0,0 +1,191 @@ +from dataclasses import dataclass +from enum import Enum +from typing import Dict, List, Optional, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +Ecu = car.CarParams.Ecu + + +class CAR: + # Chrysler + PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" + PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" + PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" + PACIFICA_2018 = "CHRYSLER PACIFICA 2018" + PACIFICA_2020 = "CHRYSLER PACIFICA 2020" + + # Jeep + JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk + JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk + + # Ram + RAM_1500 = "RAM 1500 5TH GEN" + + +class CarControllerParams: + def __init__(self, CP): + self.STEER_MAX = 261 # higher than this faults the EPS on Chrysler/Jeep. Ram DT allows more + self.STEER_ERROR_MAX = 80 + + if CP.carFingerprint in RAM_CARS: + self.STEER_DELTA_UP = 6 + self.STEER_DELTA_DOWN = 6 + else: + self.STEER_DELTA_UP = 3 + self.STEER_DELTA_DOWN = 3 + +STEER_THRESHOLD = 120 + +RAM_CARS = {CAR.RAM_1500, } + +@dataclass +class ChryslerCarInfo(CarInfo): + package: str = "Adaptive Cruise Control" + harness: Enum = Harness.fca + +CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { + CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017-18"), + CAR.PACIFICA_2018_HYBRID: None, # same platforms + CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-22"), + CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), + CAR.PACIFICA_2020: [ + ChryslerCarInfo("Chrysler Pacifica 2019-20"), + ChryslerCarInfo("Chrysler Pacifica 2021", package="All"), + ], + CAR.JEEP_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), + CAR.JEEP_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), + CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-22", harness=Harness.ram), +} + +# Unique CAN messages: +# Only the hybrids have 270: 8 +# Only the gas have 55: 8, 416: 7 +# For 564, All 2017 have length 4, whereas 2018-19 have length 8. +# For 924, Pacifica 2017 has length 3, whereas all 2018-19 have length 8. +# For 560, All 2019 have length 8, whereas all 2017-18 have length 4. + +# Jeep Grand Cherokee unique messages: +# 2017 Trailhawk: 618: 8 +# For 924, Trailhawk 2017 has length 3, whereas 2018 V6 has length 8. + +FINGERPRINTS = { + CAR.PACIFICA_2017_HYBRID: [{ + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 788:3, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 3, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1892: 8, 2016: 8, 2024: 8 + }], + CAR.PACIFICA_2018: [{ + 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 + }, + { + 55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 + }], + CAR.PACIFICA_2020: [{ + 55: 8, 179: 8, 181: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 536: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 776: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 886: 8, 897: 8, 906: 8, 924: 8, 926: 3, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 7, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1284: 8, 1543: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 2015: 8, 2016: 8, 2017:8, 2024: 8, 2025: 8 + }], + CAR.PACIFICA_2018_HYBRID: [{ + 68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 + }, + # based on 9ae7821dc4e92455|2019-07-01--16-42-55 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }], + CAR.PACIFICA_2019_HYBRID: [{ + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8 + }, + # Based on 0607d2516fc2148f|2019-02-13--23-03-16 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8 + }, + # Based on 3c7ce223e3571b54|2019-05-11--20-16-14 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 + }, + # Based on "8190c7275a24557b|2020-02-24--09-57-23" + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 711: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 793: 8, 794: 8, 795: 8, 796: 8, 797: 8, 798: 8, 799: 8, 800: 8, 801: 8, 802: 8, 803: 8, 804: 8, 805: 8, 807: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 886: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1536: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + }], + CAR.JEEP_CHEROKEE: [{ + 55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 874: 2, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 975: 8, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1543: 8, 1562: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + # Based on c88f65eeaee4003a|2022-08-04--15-37-16 + { + 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 678: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1235: 8, 1242: 8, 1252: 8, 1792: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1892: 8, 1927: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], + CAR.JEEP_CHEROKEE_2019: [{ + # Jeep Grand Cherokee 2019, including most 2020 models + 55: 8, 168: 8, 179: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 341: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 640: 1, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 840: 8, 844: 5, 847: 1, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 960: 4, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1223: 8, 1225: 8, 1227: 8, 1235: 8, 1242: 8, 1250: 8, 1251: 8, 1252: 8, 1254: 8, 1264: 8, 1284: 8, 1536: 8, 1537: 8, 1543: 8, 1545: 8, 1562: 8, 1568: 8, 1570: 8, 1572: 8, 1593: 8, 1856: 8, 1858: 8, 1860: 8, 1863: 8, 1865: 8, 1867: 8, 1875: 8, 1882: 8, 1890: 8, 1891: 8, 1892: 8, 1894: 8, 1896: 8, 1904: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }], +} + +FW_VERSIONS = { + CAR.RAM_1500: { + (Ecu.combinationMeter, 0x742, None): [ + b'68294063AH', + b'68294063AG', + b'68434860AC', + b'68527375AD', + b'68453503AC', + ], + (Ecu.srs, 0x744, None): [ + b'68441329AB', + b'68490898AA', + b'68428609AB', + b'68500728AA', + ], + (Ecu.esp, 0x747, None): [ + b'68432418AD', + b'68432418AB', + b'68436004AE', + b'68438454AD', + b'68436004AD', + b'68535469AB', + b'68438454AC', + ], + (Ecu.fwdRadar, 0x753, None): [ + b'68320950AL', + b'68320950AJ', + b'68454268AB', + b'68475160AG', + b'04672892AB', + b'68475160AE', + ], + (Ecu.eps, 0x75A, None): [ + b'68273275AG', + b'68469901AA', + b'68552788AA', + ], + (Ecu.engine, 0x7e0, None): [ + b'68448163AJ', + b'68500630AD', + b'68539650AD', + ], + (Ecu.transmission, 0x7e1, None): [ + b'68360078AL', + b'68384328AD', + b'68360085AL', + b'68360081AM', + b'68502994AD', + b'68445533AB', + b'68540431AB', + b'68484467AC', + ], + (Ecu.gateway, 0x18DACBF1, None): [ + b'68402660AB', + b'68445283AB', + b'68533631AB', + b'68500483AB', + ], + }, +} + +DBC = { + CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.JEEP_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.JEEP_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), + CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None), +} diff --git a/selfdrive/car/ford/__init__.py b/selfdrive/car/ford/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py new file mode 100644 index 000000000..11d9bb3c7 --- /dev/null +++ b/selfdrive/car/ford/carcontroller.py @@ -0,0 +1,89 @@ +from cereal import car +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.ford import fordcan +from selfdrive.car.ford.values import CarControllerParams + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): + # rate limit + steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) + rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN + max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) + apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff)) + + return apply_steer + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.VM = VM + self.packer = CANPacker(dbc_name) + self.frame = 0 + + self.apply_steer_last = 0 + self.main_on_last = False + self.lkas_enabled_last = False + self.steer_alert_last = False + + def update(self, CC, CS): + can_sends = [] + + actuators = CC.actuators + hud_control = CC.hudControl + + main_on = CS.out.cruiseState.available + steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + + if CC.cruiseControl.cancel: + # cancel stock ACC + can_sends.append(fordcan.spam_cancel_button(self.packer)) + + # apply rate limits + new_steer = actuators.steeringAngleDeg + apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) + + # send steering commands at 20Hz + if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0: + lca_rq = 1 if CC.latActive else 0 + + # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented + path_angle = apply_steer + + # convert steer angle to curvature + curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) + + # TODO: get other actuators + curvature_rate = 0 + path_offset = 0 + + ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately + precision = 0 # 0=Comfortable, 1=Precise + + self.apply_steer_last = apply_steer + can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) + can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, + path_offset, path_angle, curvature_rate, curvature)) + + send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) + + # send lkas ui command at 1Hz or if ui state changes + if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: + can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) + + # send acc ui command at 20Hz or if ui state changes + if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: + can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) + + self.main_on_last = main_on + self.lkas_enabled_last = CC.latActive + self.steer_alert_last = steer_alert + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_steer + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py new file mode 100644 index 000000000..aaa3af612 --- /dev/null +++ b/selfdrive/car/ford/carstate.py @@ -0,0 +1,215 @@ +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.ford.values import CANBUS, DBC + +GearShifter = car.CarState.GearShifter +TransmissionType = car.CarParams.TransmissionType + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + # car speed + ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG + ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 + + # gas pedal + ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. + ret.gasPressed = ret.gas > 1e-6 + + # brake pedal + ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm + ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2 + ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2) + + # steering wheel + ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] + ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] + ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0 + ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 + ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) + # ret.espDisabled = False # TODO: find traction control signal + + # cruise state + ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS + ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) + ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) + + # gear + if self.CP.transmissionType == TransmissionType.automatic: + gear = self.shifter_values.get(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"], None) + ret.gearShifter = self.parse_gear_shifter(gear) + elif self.CP.transmissionType == TransmissionType.manual: + ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 + if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]): + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.drive + + # safety + ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) + ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled + + # button presses + ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 + ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 + ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + + # lock info + ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], + cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]]) + ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2 + + # blindspot sensors + if self.CP.enableBsm: + ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 + ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 + + # Stock values from IPMA so that we can retain some stock functionality + self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] + self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) + ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) + ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped + ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status + ("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct) + ("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm) + ("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed + ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) + # The units might change with IPC settings? + ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status + ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) + # Calculates steering angle (and offset) from pinion + # angle and driving measurements. + # StePinRelInit_An_Sns is the pinion angle, initialised + # to zero at the beginning of the drive. + ("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) + ("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status + ("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel + ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch + ("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle + ("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver + ("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger + ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left + ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right + ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver + ] + + checks = [ + # sig_address, frequency + ("EngVehicleSpThrottle2", 50), + ("Yaw_Data_FD1", 100), + ("DesiredTorqBrk", 50), + ("EngVehicleSpThrottle", 100), + ("BrakeSnData_4", 50), + ("EngBrakeData", 10), + ("SteeringPinion_Data", 100), + ("EPAS_INFO", 50), + ("Lane_Assist_Data3_FD1", 33), + ("Steering_Data_FD1", 10), + ("BodyInfo_3_FD1", 2), + ("RCMStatusMessage2_FD1", 10), + ] + + if CP.transmissionType == TransmissionType.automatic: + signals += [ + ("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position + ] + checks += [ + ("Gear_Shift_by_Wire_FD1", 10), + ] + elif CP.transmissionType == TransmissionType.manual: + signals += [ + ("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct) + ("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light + ] + checks += [ + ("Engine_Clutch_Data", 33), + ("BCM_Lamp_Stat_FD1", 1), + ] + + if CP.enableBsm: + signals += [ + ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left + ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right + ] + checks += [ + ("Side_Detect_L_Stat", 5), + ("Side_Detect_R_Stat", 5), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address + ("HaDsply_No_Cs", "ACCDATA_3"), + ("HaDsply_No_Cnt", "ACCDATA_3"), + ("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message + ("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance + ("AccStopRes_B_Dsply", "ACCDATA_3"), + ("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning + ("Tja_D_Stat", "ACCDATA_3"), # TJA status + ("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text + ("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon + ("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text + ("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled + ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting + ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting + ("CadsAlignIncplt_B_Actl", "ACCDATA_3"), + ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting + ("CadsRadrBlck_B_Actl", "ACCDATA_3"), + ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status + ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting + ("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting + ("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text + ("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning + ("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert + ("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert + ("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap + ("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting + ("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting + + ("FeatConfigIpmaActl", "IPMA_Data"), + ("FeatNoIpmaActl", "IPMA_Data"), + ("PersIndexIpma_D_Actl", "IPMA_Data"), + ("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping + ("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) + ("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error + ("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime + ("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? + ("CamraStats_D_Dsply", "IPMA_Data"), # Camera status + ("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level + ("DasStats_D_Dsply", "IPMA_Data"), # DAS status + ("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning + ("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status + ("Set_Me_X1", "IPMA_Data"), + ] + + checks = [ + # sig_address, frequency + ("ACCDATA_3", 5), + ("IPMA_Data", 1), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera) diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py new file mode 100644 index 000000000..29fec1f2d --- /dev/null +++ b/selfdrive/car/ford/fordcan.py @@ -0,0 +1,144 @@ +from common.numpy_fast import clip + + +def create_lkas_command(packer, angle_deg: float, curvature: float): + """ + Creates a CAN message for the Ford LKAS Command. + + This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the + PSCM lockout. + + Frequency is 20Hz. + """ + + values = { + "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] + "LkaActvStats_D2_Req": 0, # action [0|7] + "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees + "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick + "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter + "LdwActvStats_D_Req": 0, # LDW status [0|7] + "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength + } + return packer.make_can_msg("Lane_Assist_Data1", 0, values) + + +def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): + """ + Creates a CAN message for the Ford TJA/LCA Command. + + This command can apply "Lane Centering" manoeuvres: continuous lane centering + for traffic jam assist and highway driving. It is not subject to the PSCM + lockout. + + The PSCM should be configured to accept TJA/LCA commands before these + commands will be processed. This can be done using tools such as Forscan. + + Frequency is 20Hz. + """ + + values = { + "LatCtlRng_L_Max": 0, # Unknown [0|126] meter + "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] + "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] + "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] + "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] + "LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter + "LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians + "LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2 + "LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter + } + return packer.make_can_msg("LateralMotionControl", 0, values) + + +def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values): + """ + Creates a CAN message for the Ford IPC IPMA/LKAS status. + + Show the LKAS status with the "driver assist" lines in the IPC. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 1Hz. + """ + + # LaActvStats_D_Dsply + # TODO: get LDW state from OP + if enabled: + lines = 6 + elif main_on: + lines = 0 + else: + lines = 30 + + values = { + "FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535] + "FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535] + "PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7] + "AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3] + "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] + "LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1] + "LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed + "CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1] + "CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3] + "DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7] + "DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3] + "DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3] + "AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3] + "Set_Me_X1": stock_values["Set_Me_X1"], # [0|15] + } + return packer.make_can_msg("IPMA_Data", 0, values) + + +def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values): + """ + Creates a CAN message for the Ford IPC adaptive cruise, forward collision + warning and traffic jam assist status. + + Stock functionality is maintained by passing through unmodified signals. + + Frequency is 20Hz. + """ + + values = { + "HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255] + "HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15] + "AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3] + "AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15] + "AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1] + "TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7] + "Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7] + "TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7] + "IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3] + "AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15] + "FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1] + "FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1] + "AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1] + "CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1] + "AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1] + "CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1] + "CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1] + "AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1] + "FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3] + "FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7] + "AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3] + "FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1] + "FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1] + "AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7] + "AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1] + "FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1] + } + return packer.make_can_msg("ACCDATA_3", 0, values) + + +def spam_cancel_button(packer, cancel=1): + """ + Creates a CAN message for the Ford SCCM buttons/switches. + + Includes cruise control buttons, turn lights and more. + """ + + values = { + "CcAslButtnCnclPress": cancel, # CC cancel button + } + return packer.make_can_msg("Steering_Data_FD1", 0, values) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py new file mode 100644 index 000000000..00af0536c --- /dev/null +++ b/selfdrive/car/ford/interface.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint +from selfdrive.car.ford.values import CAR, TransmissionType, GearShifter +from selfdrive.car.interfaces import CarInterfaceBase + + +EventName = car.CarEvent.EventName + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + + ret.carName = "ford" + #ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] + ret.dashcamOnly = True + + # Angle-based steering + # TODO: use curvature control when ready + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 1.0 + + # TODO: detect stop-and-go vehicles + stop_and_go = False + + if candidate == CAR.ESCAPE_MK4: + ret.wheelbase = 2.71 + ret.steerRatio = 14.3 # Copied from Focus + tire_stiffness_factor = 0.5328 # Copied from Focus + ret.mass = 1750 + STD_CARGO_KG + + elif candidate == CAR.FOCUS_MK4: + ret.wheelbase = 2.7 + ret.steerRatio = 14.3 + tire_stiffness_factor = 0.5328 + ret.mass = 1350 + STD_CARGO_KG + + else: + raise ValueError(f"Unsupported car: ${candidate}") + + # Auto Transmission: Gear_Shift_by_Wire_FD1 + # TODO: detect transmission in car_fw? + if 0x5A in fingerprint[0]: + ret.transmissionType = TransmissionType.automatic + else: + ret.transmissionType = TransmissionType.manual + + # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat + # TODO: detect bsm in car_fw? + ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. + ret.minEnableSpeed = -1. if (stop_and_go) else 20. * CV.MPH_TO_MS + # LCA can steer down to zero + ret.minSteerSpeed = 0. + + ret.centerToFront = ret.wheelbase * 0.44 + + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, + tire_stiffness_factor=tire_stiffness_factor) + + return ret + + 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 + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py new file mode 100644 index 000000000..866602cf0 --- /dev/null +++ b/selfdrive/car/ford/radar_interface.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python3 +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from selfdrive.car.ford.values import CANBUS, DBC +from selfdrive.car.interfaces import RadarInterfaceBase + +RADAR_MSGS = list(range(0x500, 0x540)) + + +def _create_radar_can_parser(car_fingerprint): + if DBC[car_fingerprint]['radar'] is None: + return None + + msg_n = len(RADAR_MSGS) + signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, + RADAR_MSGS * 3)) + checks = list(zip(RADAR_MSGS, [20] * msg_n)) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CANBUS.radar) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.validCnt = {key: 0 for key in RADAR_MSGS} + self.track_id = 0 + + self.rcp = _create_radar_can_parser(CP.carFingerprint) + self.trigger_msg = 0x53f + self.updated_messages = set() + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + ret.errors = errors + + for ii in sorted(self.updated_messages): + cpt = self.rcp.vl[ii] + + if cpt['X_Rel'] > 0.00001: + self.validCnt[ii] = 0 # reset counter + + if cpt['X_Rel'] > 0.00001: + self.validCnt[ii] += 1 + else: + self.validCnt[ii] = max(self.validCnt[ii] - 1, 0) + #print ii, self.validCnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle'] + + # radar point only valid if there have been enough valid measurements + if self.validCnt[ii] > 0: + if ii not in self.pts: + self.pts[ii] = car.RadarData.RadarPoint.new_message() + self.pts[ii].trackId = self.track_id + self.track_id += 1 + self.pts[ii].dRel = cpt['X_Rel'] # from front of car + self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive + self.pts[ii].vRel = cpt['V_Rel'] + self.pts[ii].aRel = float('nan') + self.pts[ii].yvRel = float('nan') + self.pts[ii].measured = True + else: + if ii in self.pts: + del self.pts[ii] + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py new file mode 100644 index 000000000..6b8396cf0 --- /dev/null +++ b/selfdrive/car/ford/values.py @@ -0,0 +1,85 @@ +from collections import namedtuple +from typing import Dict, List, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo + +Ecu = car.CarParams.Ecu +TransmissionType = car.CarParams.TransmissionType +GearShifter = car.CarState.GearShifter + +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) + + +class CarControllerParams: + # Messages: Lane_Assist_Data1, LateralMotionControl + LKAS_STEER_STEP = 5 + # Message: IPMA_Data + LKAS_UI_STEP = 100 + # Message: ACCDATA_3 + ACC_UI_STEP = 5 + + STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + + +class CANBUS: + main = 0 + radar = 1 + camera = 2 + + +class CAR: + ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" + FOCUS_MK4 = "FORD FOCUS 4TH GEN" + + +CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { + CAR.ESCAPE_MK4: CarInfo("Ford Escape", "NA"), + CAR.FOCUS_MK4: CarInfo("Ford Focus", "NA"), +} + + +FW_VERSIONS = { + CAR.ESCAPE_MK4: { + (Ecu.eps, 0x730, None): [ + b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x760, None): [ + b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + CAR.FOCUS_MK4: { + (Ecu.eps, 0x730, None): [ + b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x760, None): [ + b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7E0, None): [ + b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, +} + + +DBC = { + CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', None), + CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', None), +} diff --git a/selfdrive/car/gm/__init__.py b/selfdrive/car/gm/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py new file mode 100644 index 000000000..0e7406514 --- /dev/null +++ b/selfdrive/car/gm/carcontroller.py @@ -0,0 +1,128 @@ +from cereal import car +from common.conversions import Conversions as CV +from common.numpy_fast import interp +from common.realtime import DT_CTRL +from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car.gm import gmcan +from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, EV_CAR + +VisualAlert = car.CarControl.HUDControl.VisualAlert +NetworkLocation = car.CarParams.NetworkLocation + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.start_time = 0. + self.apply_steer_last = 0 + self.apply_gas = 0 + self.apply_brake = 0 + self.frame = 0 + + self.lka_steering_cmd_counter_last = -1 + self.lka_icon_status_last = (False, False) + + self.params = CarControllerParams() + + self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt']) + self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) + self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + hud_alert = hud_control.visualAlert + hud_v_cruise = hud_control.setSpeed + if hud_v_cruise > 70: + hud_v_cruise = 0 + + # Send CAN commands. + can_sends = [] + + # Steering (50Hz) + # Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we just received the + # next Panda loopback confirmation in the current CS frame. + if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: + self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter + elif (self.frame % self.params.STEER_STEP) == 0: + lkas_enabled = CC.latActive and CS.out.vEgo > self.params.MIN_STEER_SPEED + if lkas_enabled: + new_steer = int(round(actuators.steer * self.params.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + else: + apply_steer = 0 + + self.apply_steer_last = apply_steer + # GM EPS faults on any gap in received message counters. To handle transient OP/Panda safety sync issues at the + # moment of disengaging, increment the counter based on the last message known to pass Panda safety checks. + idx = (CS.lka_steering_cmd_counter + 1) % 4 + + can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) + + if self.CP.openpilotLongitudinalControl: + # Gas/regen, brakes, and UI commands - all at 25Hz + if self.frame % 4 == 0: + if not CC.longActive: + # Stock ECU sends max regen when not enabled + self.apply_gas = self.params.MAX_ACC_REGEN + self.apply_brake = 0 + else: + if self.CP.carFingerprint in EV_CAR: + self.apply_gas = int(round(interp(actuators.accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + else: + self.apply_gas = int(round(interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) + self.apply_brake = int(round(interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + + idx = (self.frame // 4) % 4 + + at_full_stop = CC.longActive and CS.out.standstill + near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) + # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop)) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, self.apply_brake, idx, near_stop, at_full_stop)) + + # Send dashboard UI commands (ACC status) + send_fcw = hud_alert == VisualAlert.fcw + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, + hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) + + # Radar needs to know current speed and yaw rate (50hz), + # and that ADAS is alive (10hz) + if not self.CP.radarOffCan: + tt = self.frame * DT_CTRL + time_and_headlights_step = 10 + if self.frame % time_and_headlights_step == 0: + idx = (self.frame // time_and_headlights_step) % 4 + can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) + + speed_and_accelerometer_step = 2 + if self.frame % speed_and_accelerometer_step == 0: + idx = (self.frame // speed_and_accelerometer_step) % 4 + can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) + + if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0: + can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) + + # Show green icon when LKA torque is applied, and + # alarming orange icon when approaching torque limit. + # If not sent again, LKA icon disappears in about 5 seconds. + # Conveniently, sending camera message periodically also works as a keepalive. + lka_active = CS.lkas_status == 1 + lka_critical = lka_active and abs(actuators.steer) > 0.9 + lka_icon_status = (lka_active, lka_critical) + if self.CP.networkLocation != NetworkLocation.fwdCamera and (self.frame % self.params.CAMERA_KEEPALIVE_STEP == 0 or lka_icon_status != self.lka_icon_status_last): + steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) + can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer_alert)) + self.lka_icon_status_last = lka_icon_status + + new_actuators = actuators.copy() + new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX + new_actuators.gas = self.apply_gas + new_actuators.brake = self.apply_brake + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py new file mode 100644 index 000000000..b174d0327 --- /dev/null +++ b/selfdrive/car/gm/carstate.py @@ -0,0 +1,148 @@ +from cereal import car +from common.numpy_fast import mean +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser +from selfdrive.car.interfaces import CarStateBase +from selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD + +TransmissionType = car.CarParams.TransmissionType + + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"] + self.lka_steering_cmd_counter = 0 + + def update(self, pt_cp, loopback_cp): + ret = car.CarState.new_message() + + self.prev_cruise_buttons = self.cruise_buttons + self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] + + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"], + pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"], + pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"], + pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"], + ) + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 + + if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1: + ret.gearShifter = self.parse_gear_shifter("T") + else: + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None)) + + # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. + ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 + ret.brakePressed = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] >= 10 + + # Regen braking is braking + if self.CP.transmissionType == TransmissionType.direct: + ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 + + ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. + ret.gasPressed = ret.gas > 1e-5 + + ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"] + ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"] + ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"] + ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD + self.lka_steering_cmd_counter = loopback_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"] + + # 0 inactive, 1 active, 2 temporarily limited, 3 failed + self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"] + ret.steerFaultTemporary = self.lkas_status == 2 + ret.steerFaultPermanent = self.lkas_status == 3 + + # 1 - open, 0 - closed + ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1) + + # 1 - latched + ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0 + ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1 + ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 + + ret.parkingBrake = pt_cp.vl["VehicleIgnitionAlt"]["ParkBrake"] == 1 + ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 + ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 + ret.accFaulted = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED + + ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF + ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("FrontLeftDoor", "BCMDoorBeltStatus"), + ("FrontRightDoor", "BCMDoorBeltStatus"), + ("RearLeftDoor", "BCMDoorBeltStatus"), + ("RearRightDoor", "BCMDoorBeltStatus"), + ("LeftSeatBelt", "BCMDoorBeltStatus"), + ("RightSeatBelt", "BCMDoorBeltStatus"), + ("TurnSignals", "BCMTurnSignals"), + ("AcceleratorPedal2", "AcceleratorPedal2"), + ("CruiseState", "AcceleratorPedal2"), + ("ACCButtons", "ASCMSteeringButton"), + ("SteeringWheelAngle", "PSCMSteeringAngle"), + ("SteeringWheelRate", "PSCMSteeringAngle"), + ("FLWheelSpd", "EBCMWheelSpdFront"), + ("FRWheelSpd", "EBCMWheelSpdFront"), + ("RLWheelSpd", "EBCMWheelSpdRear"), + ("RRWheelSpd", "EBCMWheelSpdRear"), + ("PRNDL2", "ECMPRDNL2"), + ("ManualMode", "ECMPRDNL2"), + ("LKADriverAppldTrq", "PSCMStatus"), + ("LKATorqueDelivered", "PSCMStatus"), + ("LKATorqueDeliveredStatus", "PSCMStatus"), + ("TractionControlOn", "ESPStatus"), + ("ParkBrake", "VehicleIgnitionAlt"), + ("CruiseMainOn", "ECMEngineStatus"), + ] + + checks = [ + ("BCMTurnSignals", 1), + ("ECMPRDNL2", 10), + ("PSCMStatus", 10), + ("ESPStatus", 10), + ("BCMDoorBeltStatus", 10), + ("VehicleIgnitionAlt", 10), + ("EBCMWheelSpdFront", 20), + ("EBCMWheelSpdRear", 20), + ("AcceleratorPedal2", 33), + ("ASCMSteeringButton", 33), + ("ECMEngineStatus", 100), + ("PSCMSteeringAngle", 100), + ("EBCMBrakePedalPosition", 100), + ] + + if CP.transmissionType == TransmissionType.direct: + signals.append(("RegenPaddle", "EBCMRegenPaddle")) + checks.append(("EBCMRegenPaddle", 50)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) + + @staticmethod + def get_loopback_can_parser(CP): + signals = [ + ("RollingCounter", "ASCMLKASteeringCmd"), + ] + + checks = [ + ("ASCMLKASteeringCmd", 10), # 10 Hz is the stock inactive rate (every 100ms). + # While active 50 Hz (every 20 ms) is normal + # EPS will tolerate around 200ms when active before faulting + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.LOOPBACK) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py new file mode 100644 index 000000000..47e6090a0 --- /dev/null +++ b/selfdrive/car/gm/gmcan.py @@ -0,0 +1,125 @@ +from selfdrive.car import make_can_msg + +def create_steering_control(packer, bus, apply_steer, idx, lkas_active): + + values = { + "LKASteeringCmdActive": lkas_active, + "LKASteeringCmd": apply_steer, + "RollingCounter": idx, + "LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_steer & 0x7ff) - idx + } + + return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) + +def create_adas_keepalive(bus): + dat = b"\x00\x00\x00\x00\x00\x00\x00" + return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] + +def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): + values = { + "GasRegenCmdActive": acc_engaged, + "RollingCounter": idx, + "GasRegenCmdActiveInv": 1 - acc_engaged, + "GasRegenCmd": throttle, + "GasRegenFullStopActive": at_full_stop, + "GasRegenAlwaysOne": 1, + "GasRegenAlwaysOne2": 1, + "GasRegenAlwaysOne3": 1, + } + + dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] + values["GasRegenChecksum"] = (((0xff - dat[1]) & 0xff) << 16) | \ + (((0xff - dat[2]) & 0xff) << 8) | \ + ((0x100 - dat[3] - idx) & 0xff) + + return packer.make_can_msg("ASCMGasRegenCmd", bus, values) + +def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_full_stop): + mode = 0x1 + if apply_brake > 0: + mode = 0xa + if at_full_stop: + mode = 0xd + + # TODO: this is to have GM bringing the car to complete stop, + # but currently it conflicts with OP controls, so turned off. + #elif near_stop: + # mode = 0xb + + brake = (0x1000 - apply_brake) & 0xfff + checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff + + values = { + "RollingCounter" : idx, + "FrictionBrakeMode" : mode, + "FrictionBrakeChecksum": checksum, + "FrictionBrakeCmd" : -apply_brake + } + + return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) + +def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lead_car_in_sight, fcw): + # Not a bit shift, dash can round up based on low 4 bits. + target_speed = int(target_speed_kph * 16) & 0xfff + + values = { + "ACCAlwaysOne" : 1, + "ACCResumeButton" : 0, + "ACCSpeedSetpoint" : target_speed, + "ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive" + "ACCCmdActive" : acc_engaged, + "ACCAlwaysOne2" : 1, + "ACCLeadCar" : lead_car_in_sight, + "FCWAlert": 0x3 if fcw else 0 + } + + return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) + +def create_adas_time_status(bus, tt, idx): + dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff, + ((tt & 0xf) << 4) + (idx << 2)] + chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] + chksum = chksum & 0xfff + dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] + return make_can_msg(0xa1, bytes(dat), bus) + +def create_adas_steering_status(bus, idx): + dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] + chksum = 0x60 + sum(dat) + dat += [chksum >> 8, chksum & 0xff] + return make_can_msg(0x306, bytes(dat), bus) + +def create_adas_accelerometer_speed_status(bus, speed_ms, idx): + spd = int(speed_ms * 16) & 0xfff + accel = 0 & 0xfff + # 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L + #stick = 0x08 + near_range_cutoff = 0x27 + near_range_mode = 1 if spd <= near_range_cutoff else 0 + far_range_mode = 1 - near_range_mode + dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] + chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] + dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] + return make_can_msg(0x308, bytes(dat), bus) + +def create_adas_headlights_status(packer, bus): + values = { + "Always42": 0x42, + "Always4": 0x4, + } + return packer.make_can_msg("ASCMHeadlight", bus, values) + +def create_lka_icon_command(bus, active, critical, steer): + if active and steer == 1: + if critical: + dat = b"\x50\xc0\x14" + else: + dat = b"\x50\x40\x18" + elif active: + if critical: + dat = b"\x40\xc0\x14" + else: + dat = b"\x40\x40\x18" + else: + dat = b"\x00\x00\x00" + return make_can_msg(0x104c006c, dat, bus) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py new file mode 100755 index 000000000..7a0535f40 --- /dev/null +++ b/selfdrive/car/gm/interface.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +from cereal import car +from math import fabs + +from common.conversions import Conversions as CV +from selfdrive.car import STD_CARGO_KG, create_button_enable_events, create_button_event, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams +from selfdrive.car.interfaces import CarInterfaceBase + +ButtonType = car.CarState.ButtonEvent.Type +EventName = car.CarEvent.EventName +GearShifter = car.CarState.GearShifter +TransmissionType = car.CarParams.TransmissionType +NetworkLocation = car.CarParams.NetworkLocation +BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, + CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + params = CarControllerParams() + return params.ACCEL_MIN, params.ACCEL_MAX + + # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. + @staticmethod + def get_steer_feedforward_volt(desired_angle, v_ego): + desired_angle *= 0.02904609 + sigmoid = desired_angle / (1 + fabs(desired_angle)) + return 0.10006696 * sigmoid * (v_ego + 3.12485927) + + @staticmethod + def get_steer_feedforward_acadia(desired_angle, v_ego): + desired_angle *= 0.09760208 + sigmoid = desired_angle / (1 + fabs(desired_angle)) + return 0.04689655 * sigmoid * (v_ego + 10.028217) + + def get_steer_feedforward_function(self): + if self.CP.carFingerprint == CAR.VOLT: + return self.get_steer_feedforward_volt + elif self.CP.carFingerprint == CAR.ACADIA: + return self.get_steer_feedforward_acadia + else: + return CarInterfaceBase.get_steer_feedforward_default + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "gm" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] + ret.pcmCruise = False # For ASCM, stock non-adaptive cruise control is kept off + ret.radarOffCan = False # For ASCM, radar exists + ret.transmissionType = TransmissionType.automatic + # NetworkLocation.gateway: OBD-II harness (typically ASCM), NetworkLocation.fwdCamera: non-ASCM + ret.networkLocation = NetworkLocation.gateway + + # These cars have been put into dashcam only due to both a lack of users and test coverage. + # These cars likely still work fine. Once a user confirms each car works and a test route is + # added to selfdrive/car/tests/routes.py, we can remove it from this list. + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} + + # Presence of a camera on the object bus is ok. + # Have to go to read_only if ASCM is online (ACC-enabled cars), + # or camera is on powertrain bus (LKA cars without ACC). + ret.openpilotLongitudinalControl = True + tire_stiffness_factor = 0.444 # not optimized yet + + # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. + ret.minSteerSpeed = 7 * CV.MPH_TO_MS + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] + ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 + ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.36] + + ret.steerLimitTimer = 0.4 + ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz + + # supports stop and go, but initial engage must (conservatively) be above 18mph + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + + if candidate == CAR.VOLT: + ret.transmissionType = TransmissionType.direct + ret.mass = 1607. + STD_CARGO_KG + ret.wheelbase = 2.69 + ret.steerRatio = 17.7 # Stock 15.7, LiveParameters + tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters + ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh + + ret.lateralTuning.pid.kpBP = [0., 40.] + ret.lateralTuning.pid.kpV = [0., 0.17] + ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kiV = [0.] + ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() + ret.steerActuatorDelay = 0.2 + + elif candidate == CAR.MALIBU: + ret.mass = 1496. + STD_CARGO_KG + ret.wheelbase = 2.83 + ret.steerRatio = 15.8 + ret.centerToFront = ret.wheelbase * 0.4 # wild guess + + elif candidate == CAR.HOLDEN_ASTRA: + ret.mass = 1363. + STD_CARGO_KG + ret.wheelbase = 2.662 + # Remaining parameters copied from Volt for now + ret.centerToFront = ret.wheelbase * 0.4 + ret.steerRatio = 15.7 + + elif candidate == CAR.ACADIA: + ret.minEnableSpeed = -1. # engage speed is decided by pcm + ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.86 + ret.steerRatio = 14.4 # end to end is 13.46 + ret.centerToFront = ret.wheelbase * 0.4 + ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() + ret.longitudinalActuatorDelayUpperBound = 0.5 # large delay to initially start braking + + elif candidate == CAR.BUICK_REGAL: + ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 + ret.wheelbase = 2.83 # 111.4 inches in meters + ret.steerRatio = 14.4 # guess for tourx + ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx + + elif candidate == CAR.CADILLAC_ATS: + ret.mass = 1601. + STD_CARGO_KG + ret.wheelbase = 2.78 + ret.steerRatio = 15.3 + ret.centerToFront = ret.wheelbase * 0.49 + + elif candidate == CAR.ESCALADE_ESV: + ret.minEnableSpeed = -1. # engage speed is decided by pcm + ret.mass = 2739. + STD_CARGO_KG + ret.wheelbase = 3.302 + ret.steerRatio = 17.3 + ret.centerToFront = ret.wheelbase * 0.49 + ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] + ret.lateralTuning.pid.kf = 0.000045 + tire_stiffness_factor = 1.0 + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # 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) + + return ret + + # returns a car.CarState + def _update(self, c): + ret = self.CS.update(self.cp, 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: + be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) + + # Suppress resume button if we're resuming from stop so we don't adjust speed. + if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): + be.type = ButtonType.unknown + + ret.buttonEvents = [be] + + events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, + GearShifter.eco, GearShifter.manumatic], + pcm_enable=self.CP.pcmCruise) + events = self.dp_atl_warning(ret, events) + + if ret.vEgo < self.CP.minEnableSpeed: + events.add(EventName.belowEngageSpeed) + if ret.cruiseState.standstill: + events.add(EventName.resumeRequired) + if ret.vEgo < self.CP.minSteerSpeed: + events.add(car.CarEvent.EventName.belowSteerSpeed) + + # handle button presses + events.events.extend(create_button_enable_events(ret.buttonEvents, pcm_cruise=self.CP.pcmCruise)) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py new file mode 100755 index 000000000..6904e6f89 --- /dev/null +++ b/selfdrive/car/gm/radar_interface.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +import math +from cereal import car +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from selfdrive.car.gm.values import DBC, CAR, CanBus +from selfdrive.car.interfaces import RadarInterfaceBase + +RADAR_HEADER_MSG = 1120 +SLOT_1_MSG = RADAR_HEADER_MSG + 1 +NUM_SLOTS = 20 + +# Actually it's 0x47f, but can parser only reports +# messages that are present in DBC +LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS + + +def create_radar_can_parser(car_fingerprint): + if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): + return None + + # C1A-ARS3-A by Continental + radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) + signals = list(zip(['FLRRNumValidTargets', + 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', + 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', + 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, + [RADAR_HEADER_MSG] * 7 + radar_targets * 6)) + + checks = list({(s[1], 14) for s in signals}) + + return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + + self.rcp = create_radar_can_parser(CP.carFingerprint) + + self.trigger_msg = LAST_RADAR_MSG + self.updated_messages = set() + self.radar_ts = CP.radarTimeStep + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + vls = self.rcp.update_strings(can_strings) + self.updated_messages.update(vls) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + header = self.rcp.vl[RADAR_HEADER_MSG] + fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ + header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \ + header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt'] + errors = [] + if not self.rcp.can_valid: + errors.append("canError") + if fault: + errors.append("fault") + ret.errors = errors + + currentTargets = set() + num_targets = header['FLRRNumValidTargets'] + + # Not all radar messages describe targets, + # no need to monitor all of the self.rcp.msgs_upd + for ii in self.updated_messages: + if ii == RADAR_HEADER_MSG: + continue + + if num_targets == 0: + break + + cpt = self.rcp.vl[ii] + # Zero distance means it's an empty target slot + if cpt['TrkRange'] > 0.0: + targetId = cpt['TrkObjectID'] + currentTargets.add(targetId) + if targetId not in self.pts: + self.pts[targetId] = car.RadarData.RadarPoint.new_message() + self.pts[targetId].trackId = targetId + distance = cpt['TrkRange'] + self.pts[targetId].dRel = distance # from front of car + # From driver's pov, left is positive + self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance + self.pts[targetId].vRel = cpt['TrkRangeRate'] + self.pts[targetId].aRel = float('nan') + self.pts[targetId].yvRel = float('nan') + + for oldTarget in list(self.pts.keys()): + if oldTarget not in currentTargets: + del self.pts[oldTarget] + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py new file mode 100644 index 000000000..016910a40 --- /dev/null +++ b/selfdrive/car/gm/values.py @@ -0,0 +1,160 @@ +from collections import defaultdict +from dataclasses import dataclass, field +from enum import Enum +from typing import Dict, List, Union + +from cereal import car +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output + STEER_STEP = 2 # Control frames per command (50hz) + STEER_DELTA_UP = 7 # Delta rates require review due to observed EPS weakness + STEER_DELTA_DOWN = 17 + MIN_STEER_SPEED = 3. # m/s + STEER_DRIVER_ALLOWANCE = 50 + STEER_DRIVER_MULTIPLIER = 4 + STEER_DRIVER_FACTOR = 100 + NEAR_STOP_BRAKE_PHASE = 0.5 # m/s + + # Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera" + ADAS_KEEPALIVE_STEP = 100 + CAMERA_KEEPALIVE_STEP = 100 + + # Volt gasbrake lookups + # TODO: These values should be confirmed on non-Volt vehicles + MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill. + ZERO_GAS = 2048 # Coasting + MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen + MAX_ACC_REGEN = 1404 # Max ACC regen is slightly less than max paddle regen + + # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we + # perform the closed loop control, and might need some + # to apply some more braking if we're on a downhill slope. + # Our controller should still keep the 2 second average above + # -3.5 m/s^2 as per planner limits + ACCEL_MAX = 2. # m/s^2 + ACCEL_MIN = -4. # m/s^2 + + EV_GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX] + EV_BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.] + + # ICE has much less engine braking force compared to regen in EVs, + # lower threshold removes some braking deadzone + GAS_LOOKUP_BP = [-0.1, 0., ACCEL_MAX] + BRAKE_LOOKUP_BP = [ACCEL_MIN, -0.1] + + GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] + BRAKE_LOOKUP_V = [MAX_BRAKE, 0.] + + +class CAR: + HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" + VOLT = "CHEVROLET VOLT PREMIER 2017" + CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" + MALIBU = "CHEVROLET MALIBU PREMIER 2017" + ACADIA = "GMC ACADIA DENALI 2018" + BUICK_REGAL = "BUICK REGAL ESSENCE 2018" + ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" + + +EV_CAR = {CAR.VOLT} +STEER_THRESHOLD = 1.0 + + +class Footnote(Enum): + OBD_II = CarFootnote( + 'Requires a community built ASCM harness. ' + + 'NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).', + Column.MODEL) + + +@dataclass +class GMCarInfo(CarInfo): + package: str = "Adaptive Cruise Control" + harness: Enum = Harness.obd_ii + footnotes: List[Enum] = field(default_factory=lambda: [Footnote.OBD_II]) + + +CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { + CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), + CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0), + CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), + CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), + CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), + CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), + CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), +} + + +class CruiseButtons: + INIT = 0 + UNPRESS = 1 + RES_ACCEL = 2 + DECEL_SET = 3 + MAIN = 5 + CANCEL = 6 + +class AccState: + OFF = 0 + ACTIVE = 1 + FAULTED = 3 + STANDSTILL = 4 + +class CanBus: + POWERTRAIN = 0 + OBSTACLE = 1 + CHASSIS = 2 + SW_GMLAN = 3 + LOOPBACK = 128 + DROPPED = 192 + +FINGERPRINTS = { + CAR.HOLDEN_ASTRA: [ + # Astra BK MY17, ASCM unplugged + { + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7, + }], + CAR.VOLT: [ + # Volt Premier w/ ACC 2017 + { + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 + }, + # Volt Premier w/ ACC 2018 + { + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8 + }], + CAR.BUICK_REGAL : [ + # Regal TourX Essence w/ ACC 2018 + { + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8 + }], + CAR.CADILLAC_ATS: [ + # Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], + CAR.MALIBU: [ + # Malibu Premier w/ ACC 2017 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8, + }], + CAR.ACADIA: [ + # Acadia Denali w/ACC 2018 + { + 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7 + }, + # Acadia Denali w/ /ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], + CAR.ESCALADE_ESV: [ + { + 309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8 + }], +} + +DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 5fa475fe0..5beaaf08d 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -117,7 +117,7 @@ class CarController: self.gas = 0.0 self.brake = 0.0 - def update(self, CC, CS): + def update(self, CC, CS, dragonconf): actuators = CC.actuators hud_control = CC.hudControl hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255 @@ -190,14 +190,20 @@ class CarController: if not self.CP.openpilotLongitudinalControl: if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint)) - # If using stock ACC, spam cancel command to kill gas when OP disengages. - if pcm_cancel_cmd: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint)) - elif CC.cruiseControl.resume: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) + if dragonconf.dpAtl != 1: + # If using stock ACC, spam cancel command to kill gas when OP disengages. + if pcm_cancel_cmd: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint)) + elif CC.cruiseControl.resume: + can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) else: # Send gas and brake commands. + if not CS.out.cruiseActualEnabled: + accel = 0. + brake = 0. + self.brake_last = 0. + if self.frame % 2 == 0: ts = self.frame * DT_CTRL diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index edcd78080..b55ca3e42 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -386,4 +386,4 @@ class CarInterface(CarInterfaceBase): # pass in a car.CarControl # to be called @ 100hz def apply(self, c): - return self.CC.update(c, self.CS) + return self.CC.update(c, self.CS, self.dragonconf) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 0b4a1149b..f08d9346d 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -75,7 +75,7 @@ class CarInterface(CarInterfaceBase): if self.CS.lkas_disabled: events.add(EventName.lkasDisabled) - elif self.CS.low_speed_alert: + elif self.dragonconf.dpMazdaSteerAlert and self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/nissan/__init__.py b/selfdrive/car/nissan/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py new file mode 100644 index 000000000..dbc2b33c6 --- /dev/null +++ b/selfdrive/car/nissan/carcontroller.py @@ -0,0 +1,89 @@ +from cereal import car +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.nissan import nissancan +from selfdrive.car.nissan.values import CAR, CarControllerParams + +VisualAlert = car.CarControl.HUDControl.VisualAlert + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.car_fingerprint = CP.carFingerprint + self.frame = 0 + + self.lkas_max_torque = 0 + self.last_angle = 0 + + self.packer = CANPacker(dbc_name) + + def update(self, CC, CS): + actuators = CC.actuators + hud_control = CC.hudControl + pcm_cancel_cmd = CC.cruiseControl.cancel + + can_sends = [] + + ### STEER ### + lkas_hud_msg = CS.lkas_hud_msg + lkas_hud_info_msg = CS.lkas_hud_info_msg + apply_angle = actuators.steeringAngleDeg + + steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 + + if CC.latActive: + # windup slower + if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) + else: + angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU) + + apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) + + # Max torque from driver before EPS will give up and not apply torque + if not bool(CS.out.steeringPressed): + self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE + else: + # Scale max torque based on how much torque the driver is applying to the wheel + self.lkas_max_torque = max( + # Scale max torque down to half LKAX_MAX_TORQUE as a minimum + CarControllerParams.LKAS_MAX_TORQUE * 0.5, + # Start scaling torque at STEER_THRESHOLD + CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD) + ) + + else: + apply_angle = CS.out.steeringAngleDeg + self.lkas_max_torque = 0 + + self.last_angle = apply_angle + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: + can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) + + # TODO: Find better way to cancel! + # For some reason spamming the cancel button is unreliable on the Leaf + # We now cancel by making propilot think the seatbelt is unlatched, + # this generates a beep and a warning message every time you disengage + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0: + can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) + + can_sends.append(nissancan.create_steering_control( + self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque)) + + if lkas_hud_msg and lkas_hud_info_msg: + if self.frame % 2 == 0: + can_sends.append(nissancan.create_lkas_hud_msg( + self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) + + if self.frame % 50 == 0: + can_sends.append(nissancan.create_lkas_hud_info_msg( + self.packer, lkas_hud_info_msg, steer_hud_alert + )) + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py new file mode 100644 index 000000000..a5ca23711 --- /dev/null +++ b/selfdrive/car/nissan/carstate.py @@ -0,0 +1,350 @@ +import copy +from collections import deque +from cereal import car +from opendbc.can.can_define import CANDefine +from selfdrive.car.interfaces import CarStateBase +from common.conversions import Conversions as CV +from opendbc.can.parser import CANParser +from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams + +TORQUE_SAMPLES = 12 + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) + + self.lkas_hud_msg = None + self.lkas_hud_info_msg = None + + self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) + self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] + + def update(self, cp, cp_adas, cp_cam): + ret = car.CarState.new_message() + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] + + ret.gasPressed = bool(ret.gas > 3) + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) + + ret.wheelSpeeds = self.get_wheel_speeds( + cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"], + cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"], + cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"], + cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"], + ) + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 + + if self.CP.carFingerprint == CAR.ALTIMA: + ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + else: + ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) + + if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 + ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) + elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + if self.CP.carFingerprint == CAR.LEAF: + ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 + elif self.CP.carFingerprint == CAR.LEAF_IC: + ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1 + ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) + elif self.CP.carFingerprint == CAR.ALTIMA: + ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 + ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"]) + + if self.CP.carFingerprint == CAR.ALTIMA: + speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"] + else: + speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] + + if speed != 255: + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS + else: + conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS + ret.cruiseState.speed = speed * conversion + ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus + + if self.CP.carFingerprint == CAR.ALTIMA: + ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + else: + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] + + self.steeringTorqueSamples.append(ret.steeringTorque) + # Filtering driver torque to prevent steeringPressed false positives + ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD) + + ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + + ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"]) + ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"]) + + ret.doorOpen = any([cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RR"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_RL"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FR"], + cp.vl["DOORS_LIGHTS"]["DOOR_OPEN_FL"]]) + + ret.espDisabled = bool(cp.vl["ESP"]["ESP_DISABLED"]) + + can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + + if self.CP.carFingerprint == CAR.ALTIMA: + self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + else: + self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) + + self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) + + if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) + + if self.CP.carFingerprint != CAR.ALTIMA: + self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) + self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"), + + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), + + ("DOOR_OPEN_FR", "DOORS_LIGHTS"), + ("DOOR_OPEN_FL", "DOORS_LIGHTS"), + ("DOOR_OPEN_RR", "DOORS_LIGHTS"), + ("DOOR_OPEN_RL", "DOORS_LIGHTS"), + + ("RIGHT_BLINKER", "LIGHTS"), + ("LEFT_BLINKER", "LIGHTS"), + + ("ESP_DISABLED", "ESP"), + + ("GEAR_SHIFTER", "GEARBOX"), + ] + + checks = [ + # sig_address, frequency + ("STEER_ANGLE_SENSOR", 100), + ("WHEEL_SPEEDS_REAR", 50), + ("WHEEL_SPEEDS_FRONT", 50), + ("ESP", 25), + ("GEARBOX", 25), + ("DOORS_LIGHTS", 10), + ("LIGHTS", 10), + ] + + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + signals += [ + ("USER_BRAKE_PRESSED", "DOORS_LIGHTS"), + + ("GAS_PEDAL", "GAS_PEDAL"), + ("SEATBELT_DRIVER_LATCHED", "HUD"), + ("SPEED_MPH", "HUD"), + + ("PROPILOT_BUTTON", "CRUISE_THROTTLE"), + ("CANCEL_BUTTON", "CRUISE_THROTTLE"), + ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"), + ("SET_BUTTON", "CRUISE_THROTTLE"), + ("RES_BUTTON", "CRUISE_THROTTLE"), + ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"), + ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("NEW_SIGNAL_2", "CRUISE_THROTTLE"), + ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"), + ("unsure1", "CRUISE_THROTTLE"), + ("unsure2", "CRUISE_THROTTLE"), + ("unsure3", "CRUISE_THROTTLE"), + ] + + checks += [ + ("GAS_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ("HUD", 25), + ] + + elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + signals += [ + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("CRUISE_AVAILABLE", "CRUISE_THROTTLE"), + ("SPEED_MPH", "HUD_SETTINGS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT"), + + # Copy other values, we use this to cancel + ("CANCEL_SEATBELT", "CANCEL_MSG"), + ("NEW_SIGNAL_1", "CANCEL_MSG"), + ("NEW_SIGNAL_2", "CANCEL_MSG"), + ("NEW_SIGNAL_3", "CANCEL_MSG"), + ] + checks += [ + ("BRAKE_PEDAL", 100), + ("CRUISE_THROTTLE", 50), + ("CANCEL_MSG", 50), + ("HUD_SETTINGS", 25), + ("SEATBELT", 10), + ] + + if CP.carFingerprint == CAR.ALTIMA: + signals += [ + ("LKAS_ENABLED", "LKAS_SETTINGS"), + ("CRUISE_ENABLED", "CRUISE_STATE"), + ("SET_SPEED", "PROPILOT_HUD"), + ] + checks += [ + ("CRUISE_STATE", 10), + ("LKAS_SETTINGS", 10), + ("PROPILOT_HUD", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) + + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + @staticmethod + def get_adas_can_parser(CP): + # this function generates lists for signal, messages and initial values + + if CP.carFingerprint == CAR.ALTIMA: + signals = [ + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + ("CRUISE_ON", "PRO_PILOT"), + ] + checks = [ + ("LKAS", 100), + ("PRO_PILOT", 100), + ] + else: + signals = [ + # sig_name, sig_address + ("LKAS_ENABLED", "LKAS_SETTINGS"), + + ("CRUISE_ENABLED", "CRUISE_STATE"), + + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + # Below are the HUD messages. We copy the stock message and modify + ("LARGE_WARNING_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"), + ("LEAD_CAR", "PROPILOT_HUD"), + ("LEAD_CAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"), + ("LKAS_ERROR_FLASHING", "PROPILOT_HUD"), + ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"), + ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("FOLLOW_DISTANCE", "PROPILOT_HUD"), + ("AUDIBLE_TONE", "PROPILOT_HUD"), + ("SPEED_SET_ICON", "PROPILOT_HUD"), + ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"), + ("unknown59", "PROPILOT_HUD"), + ("unknown55", "PROPILOT_HUD"), + ("unknown26", "PROPILOT_HUD"), + ("unknown28", "PROPILOT_HUD"), + ("unknown31", "PROPILOT_HUD"), + ("SET_SPEED", "PROPILOT_HUD"), + ("unknown43", "PROPILOT_HUD"), + ("unknown08", "PROPILOT_HUD"), + ("unknown05", "PROPILOT_HUD"), + ("unknown02", "PROPILOT_HUD"), + + ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"), + ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"), + ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"), + ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"), + ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"), + ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"), + ("unknown07", "PROPILOT_HUD_INFO_MSG"), + ("unknown10", "PROPILOT_HUD_INFO_MSG"), + ("unknown15", "PROPILOT_HUD_INFO_MSG"), + ("unknown23", "PROPILOT_HUD_INFO_MSG"), + ("unknown19", "PROPILOT_HUD_INFO_MSG"), + ("unknown31", "PROPILOT_HUD_INFO_MSG"), + ("unknown32", "PROPILOT_HUD_INFO_MSG"), + ("unknown46", "PROPILOT_HUD_INFO_MSG"), + ("unknown61", "PROPILOT_HUD_INFO_MSG"), + ("unknown55", "PROPILOT_HUD_INFO_MSG"), + ("unknown50", "PROPILOT_HUD_INFO_MSG"), + ] + + checks = [ + ("PROPILOT_HUD_INFO_MSG", 2), + ("LKAS_SETTINGS", 10), + ("CRUISE_STATE", 50), + ("PROPILOT_HUD", 50), + ("LKAS", 100), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + checks = [] + + if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + signals.append(("CRUISE_ON", "PRO_PILOT")) + checks.append(("PRO_PILOT", 100)) + elif CP.carFingerprint == CAR.ALTIMA: + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py new file mode 100644 index 000000000..0d5334818 --- /dev/null +++ b/selfdrive/car/nissan/interface.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +from cereal import car +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.nissan.values import CAR + + +class CarInterface(CarInterfaceBase): + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "nissan" + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] + + ret.steerLimitTimer = 1.0 + + ret.steerActuatorDelay = 0.1 + + if candidate in (CAR.ROGUE, CAR.XTRAIL): + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.705 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 17 + elif candidate in (CAR.LEAF, CAR.LEAF_IC): + ret.mass = 1610 + STD_CARGO_KG + ret.wheelbase = 2.705 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 17 + elif candidate == CAR.ALTIMA: + # Altima has EPS on C-CAN unlike the others that have it on V-CAN + ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus + ret.mass = 1492 + STD_CARGO_KG + ret.wheelbase = 2.824 + ret.centerToFront = ret.wheelbase * 0.44 + ret.steerRatio = 17 + + ret.steerControlType = car.CarParams.SteerControlType.angle + ret.radarOffCan = True + + # TODO: get actual value, for now starting with reasonable value for + # civic and scaling by mass and wheelbase + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + + # TODO: start from empirically derived lateral slip stiffness for the civic and scale by + # 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) + + return ret + + # 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() + be.type = car.CarState.ButtonEvent.Type.accelCruise + 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) + + ret.events = events.to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py new file mode 100644 index 000000000..f59714b8d --- /dev/null +++ b/selfdrive/car/nissan/nissancan.py @@ -0,0 +1,70 @@ +import copy +import crcmod +from selfdrive.car.nissan.values import CAR + +# TODO: add this checksum to the CANPacker +nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) + + +def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque): + values = { + "COUNTER": frame % 0x10, + "DESIRED_ANGLE": apply_steer, + "SET_0x80_2": 0x80, + "SET_0x80": 0x80, + "MAX_TORQUE": lkas_max_torque if steer_on else 0, + "LKA_ACTIVE": steer_on, + } + + dat = packer.make_can_msg("LKAS", 0, values)[2] + + values["CHECKSUM"] = nissan_checksum(dat[:7]) + return packer.make_can_msg("LKAS", 0, values) + + +def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg): + values = copy.copy(cruise_throttle_msg) + can_bus = 2 + + if car_fingerprint == CAR.ALTIMA: + can_bus = 1 + + values["CANCEL_BUTTON"] = 1 + values["NO_BUTTON_PRESSED"] = 0 + values["PROPILOT_BUTTON"] = 0 + values["SET_BUTTON"] = 0 + values["RES_BUTTON"] = 0 + values["FOLLOW_DISTANCE_BUTTON"] = 0 + + return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values) + + +def create_cancel_msg(packer, cancel_msg, cruise_cancel): + values = copy.copy(cancel_msg) + + if cruise_cancel: + values["CANCEL_SEATBELT"] = 1 + + return packer.make_can_msg("CANCEL_MSG", 2, values) + + +def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart): + values = lkas_hud_msg + + values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0 + values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0 + + values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0 + values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0 + values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0 + + return packer.make_can_msg("PROPILOT_HUD", 0, values) + + +def create_lkas_hud_info_msg(packer, lkas_hud_info_msg, steer_hud_alert): + values = lkas_hud_info_msg + + if steer_hud_alert: + values["HANDS_ON_WHEEL_WARNING"] = 1 + + return packer.make_can_msg("PROPILOT_HUD_INFO_MSG", 0, values) diff --git a/selfdrive/car/nissan/radar_interface.py b/selfdrive/car/nissan/radar_interface.py new file mode 100644 index 000000000..b2f765113 --- /dev/null +++ b/selfdrive/car/nissan/radar_interface.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase + +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py new file mode 100644 index 000000000..2126e550e --- /dev/null +++ b/selfdrive/car/nissan/values.py @@ -0,0 +1,141 @@ +from dataclasses import dataclass +from typing import Dict, List, Optional, Union +from enum import Enum + +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo, Harness +from cereal import car +Ecu = car.CarParams.Ecu + + +class CarControllerParams: + ANGLE_DELTA_BP = [0., 5., 15.] + ANGLE_DELTA_V = [5., .8, .15] # windup limit + ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit + LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower + STEER_THRESHOLD = 1.0 + + +class CAR: + XTRAIL = "NISSAN X-TRAIL 2017" + LEAF = "NISSAN LEAF 2018" + # Leaf with ADAS ECU found behind instrument cluster instead of glovebox + # Currently the only known difference between them is the inverted seatbelt signal. + LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" + ROGUE = "NISSAN ROGUE 2019" + ALTIMA = "NISSAN ALTIMA 2020" + + +@dataclass +class NissanCarInfo(CarInfo): + package: str = "ProPILOT Assist" + harness: Enum = Harness.nissan_a + + +CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { + CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), + CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"), + CAR.LEAF_IC: None, # same platforms + CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), + CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b), +} + +FINGERPRINTS = { + CAR.XTRAIL: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8 + }, + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8 + }, + ], + CAR.LEAF: [ + { + 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8 + }, + # 2020 Leaf SV Plus + { + 2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 + }, + ], + CAR.LEAF_IC: [ + { + 2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8 + }, + ], + CAR.ROGUE: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + ], + CAR.ALTIMA: [ + { + 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + ] +} + +FW_VERSIONS = { + CAR.ALTIMA: { + (Ecu.fwdCamera, 0x707, None): [ + b'284N86CA1D', + ], + (Ecu.eps, 0x742, None): [ + b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80', + ], + (Ecu.engine, 0x7e0, None): [ + b'237109HE2B', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U29HE0A', + ], + }, + CAR.LEAF_IC: { + (Ecu.fwdCamera, 0x707, None): [ + b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', + b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80', + ], + (Ecu.esp, 0x740, None): [ + b'476605SH1D', + b'476605SK2A', + ], + (Ecu.eps, 0x742, None): [ + b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80', + b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U25SH3A', + b'284U25SK2D', + ], + }, + CAR.XTRAIL: { + (Ecu.fwdCamera, 0x707, None): [ + b'284N86FR2A', + ], + (Ecu.esp, 0x740, None): [ + b'6FU1BD\x11\x02\x00\x02e\x95e\x80iX#\x01\x00\x00\x00\x00\x00\x80', + b'6FU0AD\x11\x02\x00\x02e\x95e\x80iQ#\x01\x00\x00\x00\x00\x00\x80', + ], + (Ecu.eps, 0x742, None): [ + b'6FP2A\x99A\x05\x02N123F\x18\x02\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.combinationMeter, 0x743, None): [ + b'6FR2A\x18B\x05\x17\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.engine, 0x7e0, None): [ + b'6FU9B\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + b'6FR9A\xa0A\x06\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x80', + ], + (Ecu.gateway, 0x18dad0f1, None): [ + b'284U26FR0E', + ], + }, +} + +DBC = { + CAR.XTRAIL: dbc_dict('nissan_x_trail_2017', None), + CAR.LEAF: dbc_dict('nissan_leaf_2018', None), + CAR.LEAF_IC: dbc_dict('nissan_leaf_2018', None), + CAR.ROGUE: dbc_dict('nissan_x_trail_2017', None), + CAR.ALTIMA: dbc_dict('nissan_x_trail_2017', None), +} diff --git a/selfdrive/car/tesla/__init__.py b/selfdrive/car/tesla/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py new file mode 100644 index 000000000..cf43b8ef0 --- /dev/null +++ b/selfdrive/car/tesla/carcontroller.py @@ -0,0 +1,69 @@ +from common.numpy_fast import clip, interp +from opendbc.can.packer import CANPacker +from selfdrive.car.tesla.teslacan import TeslaCAN +from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams + + +class CarController: + def __init__(self, dbc_name, CP, VM): + self.CP = CP + self.frame = 0 + self.last_angle = 0 + self.packer = CANPacker(dbc_name) + self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) + self.tesla_can = TeslaCAN(self.packer, self.pt_packer) + + def update(self, CC, CS): + actuators = CC.actuators + pcm_cancel_cmd = CC.cruiseControl.cancel + + can_sends = [] + + # Temp disable steering on a hands_on_fault, and allow for user override + hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 + lkas_enabled = CC.latActive and not hands_on_fault + + if lkas_enabled: + apply_angle = actuators.steeringAngleDeg + + # Angular rate limit based on speed + steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) + rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN + max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) + apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff) + + # To not fault the EPS + apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) + else: + apply_angle = CS.out.steeringAngleDeg + + self.last_angle = apply_angle + can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) + + # Longitudinal control (in sync with stock message, about 40Hz) + if self.CP.openpilotLongitudinalControl: + target_accel = actuators.accel + target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0) + max_accel = 0 if target_accel < 0 else target_accel + min_accel = 0 if target_accel > 0 else target_accel + + while len(CS.das_control_counters) > 0: + can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft())) + + # Cancel on user steering override, since there is no steering torque blending + if hands_on_fault: + pcm_cancel_cmd = True + + if self.frame % 10 == 0 and pcm_cancel_cmd: + # Spam every possible counter value, otherwise it might not be accepted + for counter in range(16): + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter)) + can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter)) + + # TODO: HUD control + + new_actuators = actuators.copy() + new_actuators.steeringAngleDeg = apply_angle + + self.frame += 1 + return new_actuators, can_sends diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py new file mode 100644 index 000000000..0f373842f --- /dev/null +++ b/selfdrive/car/tesla/carstate.py @@ -0,0 +1,193 @@ +import copy +from collections import deque +from cereal import car +from common.conversions import Conversions as CV +from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from selfdrive.car.interfaces import CarStateBase +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine + +class CarState(CarStateBase): + def __init__(self, CP): + super().__init__(CP) + self.button_states = {button.event_type: False for button in BUTTONS} + self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis']) + + # Needed by carcontroller + self.msg_stw_actn_req = None + self.hands_on_level = 0 + self.steer_warning = None + self.acc_state = 0 + self.das_control_counters = deque(maxlen=32) + + def update(self, cp, cp_cam): + ret = car.CarState.new_message() + + # Vehicle speed + ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = (ret.vEgo < 0.1) + + # Gas pedal + ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0 + ret.gasPressed = (ret.gas > 0) + + # Brake pedal + ret.brake = 0 + ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) + + # Steering wheel + self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"] + self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None) + steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None) + + ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"] + ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate + ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"] + ret.steeringPressed = (self.hands_on_level > 0) + ret.steerFaultPermanent = steer_status == "EAC_FAULT" + ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) + + # Cruise state + cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None) + speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None) + + acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")) + + ret.cruiseState.enabled = acc_enabled + if speed_units == "KPH": + ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS + elif speed_units == "MPH": + ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS + ret.cruiseState.available = ((cruise_state == "STANDBY") or ret.cruiseState.enabled) + ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special + + # Gear + ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] + + # Buttons + buttonEvents = [] + for button in BUTTONS: + state = (cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + buttonEvents.append(event) + self.button_states[button.event_type] = state + ret.buttonEvents = buttonEvents + + # Doors + ret.doorOpen = any([(self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS]) + + # Blinkers + ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1) + ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) + + # Seatbelt + ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) + + # TODO: blindspot + + # AEB + ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1) + + # Messages needed by carcontroller + self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"]) + self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"] + self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"]) + + return ret + + @staticmethod + def get_can_parser(CP): + signals = [ + # sig_name, sig_address + ("ESP_vehicleSpeed", "ESP_B"), + ("DI_pedalPos", "DI_torque1"), + ("DI_brakePedal", "DI_torque2"), + ("StW_AnglHP", "STW_ANGLHP_STAT"), + ("StW_AnglHP_Spd", "STW_ANGLHP_STAT"), + ("EPAS_handsOnLevel", "EPAS_sysStatus"), + ("EPAS_torsionBarTorque", "EPAS_sysStatus"), + ("EPAS_internalSAS", "EPAS_sysStatus"), + ("EPAS_eacStatus", "EPAS_sysStatus"), + ("EPAS_eacErrorCode", "EPAS_sysStatus"), + ("DI_cruiseState", "DI_state"), + ("DI_digitalSpeed", "DI_state"), + ("DI_speedUnits", "DI_state"), + ("DI_gear", "DI_torque2"), + ("DOOR_STATE_FL", "GTW_carState"), + ("DOOR_STATE_FR", "GTW_carState"), + ("DOOR_STATE_RL", "GTW_carState"), + ("DOOR_STATE_RR", "GTW_carState"), + ("DOOR_STATE_FrontTrunk", "GTW_carState"), + ("BOOT_STATE", "GTW_carState"), + ("BC_indicatorLStatus", "GTW_carState"), + ("BC_indicatorRStatus", "GTW_carState"), + ("SDM_bcklDrivStatus", "SDM1"), + ("driverBrakeStatus", "BrakeMessage"), + + # We copy this whole message when spamming cancel + ("SpdCtrlLvr_Stat", "STW_ACTN_RQ"), + ("VSL_Enbl_Rq", "STW_ACTN_RQ"), + ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"), + ("DTR_Dist_Rq", "STW_ACTN_RQ"), + ("TurnIndLvr_Stat", "STW_ACTN_RQ"), + ("HiBmLvr_Stat", "STW_ACTN_RQ"), + ("WprWashSw_Psd", "STW_ACTN_RQ"), + ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"), + ("StW_Lvr_Stat", "STW_ACTN_RQ"), + ("StW_Cond_Flt", "STW_ACTN_RQ"), + ("StW_Cond_Psd", "STW_ACTN_RQ"), + ("HrnSw_Psd", "STW_ACTN_RQ"), + ("StW_Sw00_Psd", "STW_ACTN_RQ"), + ("StW_Sw01_Psd", "STW_ACTN_RQ"), + ("StW_Sw02_Psd", "STW_ACTN_RQ"), + ("StW_Sw03_Psd", "STW_ACTN_RQ"), + ("StW_Sw04_Psd", "STW_ACTN_RQ"), + ("StW_Sw05_Psd", "STW_ACTN_RQ"), + ("StW_Sw06_Psd", "STW_ACTN_RQ"), + ("StW_Sw07_Psd", "STW_ACTN_RQ"), + ("StW_Sw08_Psd", "STW_ACTN_RQ"), + ("StW_Sw09_Psd", "STW_ACTN_RQ"), + ("StW_Sw10_Psd", "STW_ACTN_RQ"), + ("StW_Sw11_Psd", "STW_ACTN_RQ"), + ("StW_Sw12_Psd", "STW_ACTN_RQ"), + ("StW_Sw13_Psd", "STW_ACTN_RQ"), + ("StW_Sw14_Psd", "STW_ACTN_RQ"), + ("StW_Sw15_Psd", "STW_ACTN_RQ"), + ("WprSw6Posn", "STW_ACTN_RQ"), + ("MC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ] + + checks = [ + # sig_address, frequency + ("ESP_B", 50), + ("DI_torque1", 100), + ("DI_torque2", 100), + ("STW_ANGLHP_STAT", 100), + ("EPAS_sysStatus", 25), + ("DI_state", 10), + ("STW_ACTN_RQ", 10), + ("GTW_carState", 10), + ("SDM1", 10), + ("BrakeMessage", 50), + ] + + return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.chassis) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + # sig_name, sig_address + ("DAS_accState", "DAS_control"), + ("DAS_aebEvent", "DAS_control"), + ("DAS_controlCounter", "DAS_control"), + ] + checks = [ + # sig_address, frequency + ("DAS_control", 40), + ] + return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py new file mode 100755 index 000000000..5e78c72b6 --- /dev/null +++ b/selfdrive/car/tesla/interface.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +from cereal import car +from panda import Panda +from selfdrive.car.tesla.values import CANBUS, CAR +from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config +from selfdrive.car.interfaces import CarInterfaceBase + + +class CarInterface(CarInterfaceBase): + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): + ret = CarInterfaceBase.get_std_params(candidate, fingerprint) + ret.carName = "tesla" + + # There is no safe way to do steer blending with user torque, + # so the steering behaves like autopilot. This is not + # how openpilot should be, hence dashcamOnly + ret.dashcamOnly = True + + ret.steerControlType = car.CarParams.SteerControlType.angle + + # Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command + ret.longitudinalTuning.kpBP = [0] + ret.longitudinalTuning.kpV = [0] + ret.longitudinalTuning.kiBP = [0] + ret.longitudinalTuning.kiV = [0] + ret.stopAccel = 0.0 + ret.longitudinalActuatorDelayUpperBound = 0.5 # s + ret.radarTimeStep = (1.0 / 8) # 8Hz + + # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus + # If so, we assume that it is connected to the longitudinal harness. + if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): + ret.openpilotLongitudinalControl = True + ret.safetyConfigs = [ + get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL), + get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN), + ] + else: + ret.openpilotLongitudinalControl = False + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + + ret.steerLimitTimer = 1.0 + ret.steerActuatorDelay = 0.25 + + if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): + ret.mass = 2100. + STD_CARGO_KG + ret.wheelbase = 2.959 + ret.centerToFront = ret.wheelbase * 0.5 + ret.steerRatio = 15.0 + else: + raise ValueError(f"Unsupported car: {candidate}") + + ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) + ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) + + return ret + + def _update(self, c): + ret = self.CS.update(self.cp, self.cp_cam) + + ret.events = self.create_common_events(ret).to_msg() + + return ret + + def apply(self, c): + return self.CC.update(c, self.CS) diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py new file mode 100755 index 000000000..a09f53e75 --- /dev/null +++ b/selfdrive/car/tesla/radar_interface.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +from cereal import car +from opendbc.can.parser import CANParser +from selfdrive.car.tesla.values import DBC, CANBUS +from selfdrive.car.interfaces import RadarInterfaceBase + +RADAR_MSGS_A = list(range(0x310, 0x36E, 3)) +RADAR_MSGS_B = list(range(0x311, 0x36F, 3)) +NUM_POINTS = len(RADAR_MSGS_A) + +def get_radar_can_parser(CP): + # Status messages + signals = [ + ('RADC_HWFail', 'TeslaRadarSguInfo'), + ('RADC_SGUFail', 'TeslaRadarSguInfo'), + ('RADC_SensorDirty', 'TeslaRadarSguInfo'), + ] + + checks = [ + ('TeslaRadarSguInfo', 10), + ] + + # Radar tracks. There are also raw point clouds available, + # we don't use those. + for i in range(NUM_POINTS): + msg_id_a = RADAR_MSGS_A[i] + msg_id_b = RADAR_MSGS_B[i] + + # There is a bunch more info in the messages, + # but these are the only things actually used in openpilot + signals.extend([ + ('LongDist', msg_id_a), + ('LongSpeed', msg_id_a), + ('LatDist', msg_id_a), + ('LongAccel', msg_id_a), + ('Meas', msg_id_a), + ('Tracked', msg_id_a), + ('Index', msg_id_a), + + ('LatSpeed', msg_id_b), + ('Index2', msg_id_b), + ]) + + checks.extend([ + (msg_id_a, 8), + (msg_id_b, 8), + ]) + + return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, CANBUS.radar) + +class RadarInterface(RadarInterfaceBase): + def __init__(self, CP): + super().__init__(CP) + self.rcp = get_radar_can_parser(CP) + self.updated_messages = set() + self.track_id = 0 + self.trigger_msg = RADAR_MSGS_B[-1] + + def update(self, can_strings): + if self.rcp is None: + return super().update(None) + + values = self.rcp.update_strings(can_strings) + self.updated_messages.update(values) + + if self.trigger_msg not in self.updated_messages: + return None + + ret = car.RadarData.new_message() + + # Errors + errors = [] + sgu_info = self.rcp.vl['TeslaRadarSguInfo'] + if not self.rcp.can_valid: + errors.append('canError') + if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']: + errors.append('fault') + ret.errors = errors + + # Radar tracks + for i in range(NUM_POINTS): + msg_a = self.rcp.vl[RADAR_MSGS_A[i]] + msg_b = self.rcp.vl[RADAR_MSGS_B[i]] + + # Make sure msg A and B are together + if msg_a['Index'] != msg_b['Index2']: + continue + + # Check if it's a valid track + if not msg_a['Tracked']: + if i in self.pts: + del self.pts[i] + continue + + # New track! + if i not in self.pts: + self.pts[i] = car.RadarData.RadarPoint.new_message() + self.pts[i].trackId = self.track_id + self.track_id += 1 + + # Parse track data + self.pts[i].dRel = msg_a['LongDist'] + self.pts[i].yRel = msg_a['LatDist'] + self.pts[i].vRel = msg_a['LongSpeed'] + self.pts[i].aRel = msg_a['LongAccel'] + self.pts[i].yvRel = msg_b['LatSpeed'] + self.pts[i].measured = bool(msg_a['Meas']) + + ret.points = list(self.pts.values()) + self.updated_messages.clear() + return ret diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py new file mode 100644 index 000000000..e5d904f80 --- /dev/null +++ b/selfdrive/car/tesla/teslacan.py @@ -0,0 +1,62 @@ +import copy +import crcmod + +from common.conversions import Conversions as CV +from selfdrive.car.tesla.values import CANBUS, CarControllerParams + + +class TeslaCAN: + def __init__(self, packer, pt_packer): + self.packer = packer + self.pt_packer = pt_packer + self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) + + @staticmethod + def checksum(msg_id, dat): + # TODO: get message ID from name instead + ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) + ret += sum(dat) + return ret & 0xFF + + def create_steering_control(self, angle, enabled, frame): + values = { + "DAS_steeringAngleRequest": -angle, + "DAS_steeringHapticRequest": 0, + "DAS_steeringControlType": 1 if enabled else 0, + "DAS_steeringControlCounter": (frame % 16), + } + + data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[2] + values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3]) + return self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values) + + def create_action_request(self, msg_stw_actn_req, cancel, bus, counter): + values = copy.copy(msg_stw_actn_req) + + if cancel: + values["SpdCtrlLvr_Stat"] = 1 + values["MC_STW_ACTN_RQ"] = counter + + data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2] + values["CRC_STW_ACTN_RQ"] = self.crc(data[:7]) + return self.packer.make_can_msg("STW_ACTN_RQ", bus, values) + + def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt): + messages = [] + values = { + "DAS_setSpeed": speed * CV.MS_TO_KPH, + "DAS_accState": acc_state, + "DAS_aebEvent": 0, + "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN, + "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX, + "DAS_accelMin": min_accel, + "DAS_accelMax": max_accel, + "DAS_controlCounter": cnt, + "DAS_controlChecksum": 0, + } + + for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]: + data = packer.make_can_msg("DAS_control", bus, values)[2] + values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7]) + messages.append(packer.make_can_msg("DAS_control", bus, values)) + return messages diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py new file mode 100644 index 000000000..296169587 --- /dev/null +++ b/selfdrive/car/tesla/values.py @@ -0,0 +1,77 @@ +from collections import namedtuple +from typing import Dict, List, Union + +from selfdrive.car import dbc_dict +from selfdrive.car.docs_definitions import CarInfo +from cereal import car + +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) + + +class CAR: + AP1_MODELS = 'TESLA AP1 MODEL S' + AP2_MODELS = 'TESLA AP2 MODEL S' + + +CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { + CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"), + CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"), +} + +FINGERPRINTS = { + CAR.AP2_MODELS: [ + { + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 518: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 538: 8, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 576: 3, 577: 8, 582: 5, 583: 8, 584: 4, 585: 8, 590: 8, 601: 8, 606: 8, 608: 1, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 692: 8, 693: 8, 695: 8, 696: 8, 697: 8, 699: 8, 700: 8, 701: 8, 702: 8, 703: 8, 704: 8, 708: 8, 709: 8, 710: 8, 711: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 811: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 845: 8, 846: 5, 848: 8, 852: 8, 853: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 876: 8, 877: 8, 879: 8, 880: 8, 882: 8, 884: 8, 888: 8, 893: 8, 894: 8, 901: 6, 904: 3, 905: 8, 906: 8, 908: 2, 909: 8, 910: 8, 912: 8, 920: 8, 921: 8, 925: 4, 926: 6, 936: 8, 941: 8, 949: 8, 952: 8, 953: 6, 968: 8, 969: 6, 970: 8, 971: 8, 977: 8, 984: 8, 987: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1007: 8, 1008: 8, 1010: 6, 1014: 1, 1015: 8, 1016: 8, 1017: 8, 1018: 8, 1020: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1049: 8, 1061: 8, 1064: 8, 1065: 8, 1070: 8, 1080: 8, 1081: 8, 1097: 8, 1113: 8, 1129: 8, 1145: 8, 1160: 4, 1177: 8, 1281: 8, 1328: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1353: 8, 1368: 8, 1412: 8, 1436: 8, 1476: 8, 1481: 8, 1497: 8, 1513: 8, 1519: 8, 1601: 8, 1605: 8, 1617: 8, 1621: 8, 1625: 8, 1665: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1824: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2015: 8, 2043: 5, 2045: 4 + }, + ], + CAR.AP1_MODELS: [ + { + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4 + }, + ], +} + +DBC = { + CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), + CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), +} + +class CANBUS: + # Lateral harness + chassis = 0 + radar = 1 + autopilot_chassis = 2 + + # Longitudinal harness + powertrain = 4 + private = 5 + autopilot_powertrain = 6 + +GEAR_MAP = { + "DI_GEAR_INVALID": car.CarState.GearShifter.unknown, + "DI_GEAR_P": car.CarState.GearShifter.park, + "DI_GEAR_R": car.CarState.GearShifter.reverse, + "DI_GEAR_N": car.CarState.GearShifter.neutral, + "DI_GEAR_D": car.CarState.GearShifter.drive, + "DI_GEAR_SNA": car.CarState.GearShifter.unknown, +} + +DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"] + +# Make sure the message and addr is also in the CAN parser! +BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]), + Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), + Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), +] + +class CarControllerParams: + RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + JERK_LIMIT_MAX = 8 + JERK_LIMIT_MIN = -8 + ACCEL_TO_SPEED_MULTIPLIER = 3 diff --git a/selfdrive/car/tests/__init__.py b/selfdrive/car/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py new file mode 100755 index 000000000..412874c81 --- /dev/null +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +import math +import unittest +import importlib +from parameterized import parameterized + +from cereal import car +from selfdrive.car.fingerprints import all_known_cars +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS + +class TestCarInterfaces(unittest.TestCase): + + @parameterized.expand([(car,) for car in all_known_cars()]) + def test_car_interfaces(self, car_name): + if car_name in FINGERPRINTS: + fingerprint = FINGERPRINTS[car_name][0] + else: + fingerprint = {} + + CarInterface, CarController, CarState = interfaces[car_name] + fingerprints = { + 0: fingerprint, + 1: fingerprint, + 2: fingerprint, + } + + car_fw = [] + + car_params = CarInterface.get_params(car_name, fingerprints, car_fw) + car_interface = CarInterface(car_params, CarController, CarState) + assert car_params + assert car_interface + + self.assertGreater(car_params.mass, 1) + self.assertGreater(car_params.maxLateralAccel, 0) + + if car_params.steerControlType != car.CarParams.SteerControlType.angle: + tuning = car_params.lateralTuning.which() + if tuning == 'pid': + self.assertTrue(len(car_params.lateralTuning.pid.kpV)) + elif tuning == 'torque': + kf = car_params.lateralTuning.torque.kf + self.assertTrue(not math.isnan(kf) and kf > 0) + self.assertTrue(not math.isnan(car_params.lateralTuning.torque.friction)) + elif tuning == 'indi': + self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV)) + + # Run car interface + CC = car.CarControl.new_message() + for _ in range(10): + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) + + CC = car.CarControl.new_message() + CC.enabled = True + for _ in range(10): + car_interface.update(CC, []) + car_interface.apply(CC) + car_interface.apply(CC) + + # Test radar interface + RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface + radar_interface = RadarInterface(car_params) + assert radar_interface + + # Run radar interface once + radar_interface.update([]) + if not car_params.radarOffCan and radar_interface.rcp is not None and \ + hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'): + radar_interface._update([radar_interface.trigger_msg]) + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/locationd/locationd b/selfdrive/locationd/locationd index 290bbe578..8ca26cbb7 100755 Binary files a/selfdrive/locationd/locationd and b/selfdrive/locationd/locationd differ diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 18d9b4faf..a7052e167 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -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_7500597990915213513) { - out_7500597990915213513[0] = delta_x[0] + nom_x[0]; - out_7500597990915213513[1] = delta_x[1] + nom_x[1]; - out_7500597990915213513[2] = delta_x[2] + nom_x[2]; - out_7500597990915213513[3] = delta_x[3] + nom_x[3]; - out_7500597990915213513[4] = delta_x[4] + nom_x[4]; - out_7500597990915213513[5] = delta_x[5] + nom_x[5]; - out_7500597990915213513[6] = delta_x[6] + nom_x[6]; - out_7500597990915213513[7] = delta_x[7] + nom_x[7]; - out_7500597990915213513[8] = delta_x[8] + nom_x[8]; -} -void inv_err_fun(double *nom_x, double *true_x, double *out_2781571296576732525) { - out_2781571296576732525[0] = -nom_x[0] + true_x[0]; - out_2781571296576732525[1] = -nom_x[1] + true_x[1]; - out_2781571296576732525[2] = -nom_x[2] + true_x[2]; - out_2781571296576732525[3] = -nom_x[3] + true_x[3]; - out_2781571296576732525[4] = -nom_x[4] + true_x[4]; - out_2781571296576732525[5] = -nom_x[5] + true_x[5]; - out_2781571296576732525[6] = -nom_x[6] + true_x[6]; - out_2781571296576732525[7] = -nom_x[7] + true_x[7]; - out_2781571296576732525[8] = -nom_x[8] + true_x[8]; -} -void H_mod_fun(double *state, double *out_3562418887379080574) { - out_3562418887379080574[0] = 1.0; - out_3562418887379080574[1] = 0; - out_3562418887379080574[2] = 0; - out_3562418887379080574[3] = 0; - out_3562418887379080574[4] = 0; - out_3562418887379080574[5] = 0; - out_3562418887379080574[6] = 0; - out_3562418887379080574[7] = 0; - out_3562418887379080574[8] = 0; - out_3562418887379080574[9] = 0; - out_3562418887379080574[10] = 1.0; - out_3562418887379080574[11] = 0; - out_3562418887379080574[12] = 0; - out_3562418887379080574[13] = 0; - out_3562418887379080574[14] = 0; - out_3562418887379080574[15] = 0; - out_3562418887379080574[16] = 0; - out_3562418887379080574[17] = 0; - out_3562418887379080574[18] = 0; - out_3562418887379080574[19] = 0; - out_3562418887379080574[20] = 1.0; - out_3562418887379080574[21] = 0; - out_3562418887379080574[22] = 0; - out_3562418887379080574[23] = 0; - out_3562418887379080574[24] = 0; - out_3562418887379080574[25] = 0; - out_3562418887379080574[26] = 0; - out_3562418887379080574[27] = 0; - out_3562418887379080574[28] = 0; - out_3562418887379080574[29] = 0; - out_3562418887379080574[30] = 1.0; - out_3562418887379080574[31] = 0; - out_3562418887379080574[32] = 0; - out_3562418887379080574[33] = 0; - out_3562418887379080574[34] = 0; - out_3562418887379080574[35] = 0; - out_3562418887379080574[36] = 0; - out_3562418887379080574[37] = 0; - out_3562418887379080574[38] = 0; - out_3562418887379080574[39] = 0; - out_3562418887379080574[40] = 1.0; - out_3562418887379080574[41] = 0; - out_3562418887379080574[42] = 0; - out_3562418887379080574[43] = 0; - out_3562418887379080574[44] = 0; - out_3562418887379080574[45] = 0; - out_3562418887379080574[46] = 0; - out_3562418887379080574[47] = 0; - out_3562418887379080574[48] = 0; - out_3562418887379080574[49] = 0; - out_3562418887379080574[50] = 1.0; - out_3562418887379080574[51] = 0; - out_3562418887379080574[52] = 0; - out_3562418887379080574[53] = 0; - out_3562418887379080574[54] = 0; - out_3562418887379080574[55] = 0; - out_3562418887379080574[56] = 0; - out_3562418887379080574[57] = 0; - out_3562418887379080574[58] = 0; - out_3562418887379080574[59] = 0; - out_3562418887379080574[60] = 1.0; - out_3562418887379080574[61] = 0; - out_3562418887379080574[62] = 0; - out_3562418887379080574[63] = 0; - out_3562418887379080574[64] = 0; - out_3562418887379080574[65] = 0; - out_3562418887379080574[66] = 0; - out_3562418887379080574[67] = 0; - out_3562418887379080574[68] = 0; - out_3562418887379080574[69] = 0; - out_3562418887379080574[70] = 1.0; - out_3562418887379080574[71] = 0; - out_3562418887379080574[72] = 0; - out_3562418887379080574[73] = 0; - out_3562418887379080574[74] = 0; - out_3562418887379080574[75] = 0; - out_3562418887379080574[76] = 0; - out_3562418887379080574[77] = 0; - out_3562418887379080574[78] = 0; - out_3562418887379080574[79] = 0; - out_3562418887379080574[80] = 1.0; -} -void f_fun(double *state, double dt, double *out_8261543717010253727) { - out_8261543717010253727[0] = state[0]; - out_8261543717010253727[1] = state[1]; - out_8261543717010253727[2] = state[2]; - out_8261543717010253727[3] = state[3]; - out_8261543717010253727[4] = state[4]; - out_8261543717010253727[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_8261543717010253727[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_8261543717010253727[7] = state[7]; - out_8261543717010253727[8] = state[8]; -} -void F_fun(double *state, double dt, double *out_3007543019148089031) { - out_3007543019148089031[0] = 1; - out_3007543019148089031[1] = 0; - out_3007543019148089031[2] = 0; - out_3007543019148089031[3] = 0; - out_3007543019148089031[4] = 0; - out_3007543019148089031[5] = 0; - out_3007543019148089031[6] = 0; - out_3007543019148089031[7] = 0; - out_3007543019148089031[8] = 0; - out_3007543019148089031[9] = 0; - out_3007543019148089031[10] = 1; - out_3007543019148089031[11] = 0; - out_3007543019148089031[12] = 0; - out_3007543019148089031[13] = 0; - out_3007543019148089031[14] = 0; - out_3007543019148089031[15] = 0; - out_3007543019148089031[16] = 0; - out_3007543019148089031[17] = 0; - out_3007543019148089031[18] = 0; - out_3007543019148089031[19] = 0; - out_3007543019148089031[20] = 1; - out_3007543019148089031[21] = 0; - out_3007543019148089031[22] = 0; - out_3007543019148089031[23] = 0; - out_3007543019148089031[24] = 0; - out_3007543019148089031[25] = 0; - out_3007543019148089031[26] = 0; - out_3007543019148089031[27] = 0; - out_3007543019148089031[28] = 0; - out_3007543019148089031[29] = 0; - out_3007543019148089031[30] = 1; - out_3007543019148089031[31] = 0; - out_3007543019148089031[32] = 0; - out_3007543019148089031[33] = 0; - out_3007543019148089031[34] = 0; - out_3007543019148089031[35] = 0; - out_3007543019148089031[36] = 0; - out_3007543019148089031[37] = 0; - out_3007543019148089031[38] = 0; - out_3007543019148089031[39] = 0; - out_3007543019148089031[40] = 1; - out_3007543019148089031[41] = 0; - out_3007543019148089031[42] = 0; - out_3007543019148089031[43] = 0; - out_3007543019148089031[44] = 0; - out_3007543019148089031[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_3007543019148089031[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3007543019148089031[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3007543019148089031[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3007543019148089031[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_3007543019148089031[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3007543019148089031[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3007543019148089031[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3007543019148089031[53] = -9.8000000000000007*dt; - out_3007543019148089031[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_3007543019148089031[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3007543019148089031[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3007543019148089031[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3007543019148089031[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_3007543019148089031[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3007543019148089031[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_3007543019148089031[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3007543019148089031[62] = 0; - out_3007543019148089031[63] = 0; - out_3007543019148089031[64] = 0; - out_3007543019148089031[65] = 0; - out_3007543019148089031[66] = 0; - out_3007543019148089031[67] = 0; - out_3007543019148089031[68] = 0; - out_3007543019148089031[69] = 0; - out_3007543019148089031[70] = 1; - out_3007543019148089031[71] = 0; - out_3007543019148089031[72] = 0; - out_3007543019148089031[73] = 0; - out_3007543019148089031[74] = 0; - out_3007543019148089031[75] = 0; - out_3007543019148089031[76] = 0; - out_3007543019148089031[77] = 0; - out_3007543019148089031[78] = 0; - out_3007543019148089031[79] = 0; - out_3007543019148089031[80] = 1; -} -void h_25(double *state, double *unused, double *out_5344993741381892957) { - out_5344993741381892957[0] = state[6]; -} -void H_25(double *state, double *unused, double *out_3493880848013174199) { - out_3493880848013174199[0] = 0; - out_3493880848013174199[1] = 0; - out_3493880848013174199[2] = 0; - out_3493880848013174199[3] = 0; - out_3493880848013174199[4] = 0; - out_3493880848013174199[5] = 0; - out_3493880848013174199[6] = 1; - out_3493880848013174199[7] = 0; - out_3493880848013174199[8] = 0; -} -void h_24(double *state, double *unused, double *out_7269781498191373830) { - out_7269781498191373830[0] = state[4]; - out_7269781498191373830[1] = state[5]; -} -void H_24(double *state, double *unused, double *out_1321231249007674633) { - out_1321231249007674633[0] = 0; - out_1321231249007674633[1] = 0; - out_1321231249007674633[2] = 0; - out_1321231249007674633[3] = 0; - out_1321231249007674633[4] = 1; - out_1321231249007674633[5] = 0; - out_1321231249007674633[6] = 0; - out_1321231249007674633[7] = 0; - out_1321231249007674633[8] = 0; - out_1321231249007674633[9] = 0; - out_1321231249007674633[10] = 0; - out_1321231249007674633[11] = 0; - out_1321231249007674633[12] = 0; - out_1321231249007674633[13] = 0; - out_1321231249007674633[14] = 1; - out_1321231249007674633[15] = 0; - out_1321231249007674633[16] = 0; - out_1321231249007674633[17] = 0; -} -void h_30(double *state, double *unused, double *out_1707544213127870607) { - out_1707544213127870607[0] = state[4]; -} -void H_30(double *state, double *unused, double *out_1033815482114433999) { - out_1033815482114433999[0] = 0; - out_1033815482114433999[1] = 0; - out_1033815482114433999[2] = 0; - out_1033815482114433999[3] = 0; - out_1033815482114433999[4] = 1; - out_1033815482114433999[5] = 0; - out_1033815482114433999[6] = 0; - out_1033815482114433999[7] = 0; - out_1033815482114433999[8] = 0; -} -void h_26(double *state, double *unused, double *out_7274424958878970158) { - out_7274424958878970158[0] = state[7]; -} -void H_26(double *state, double *unused, double *out_247622470860882025) { - out_247622470860882025[0] = 0; - out_247622470860882025[1] = 0; - out_247622470860882025[2] = 0; - out_247622470860882025[3] = 0; - out_247622470860882025[4] = 0; - out_247622470860882025[5] = 0; - out_247622470860882025[6] = 0; - out_247622470860882025[7] = 1; - out_247622470860882025[8] = 0; -} -void h_27(double *state, double *unused, double *out_343320473721047312) { - out_343320473721047312[0] = state[3]; -} -void H_27(double *state, double *unused, double *out_3208578793914858910) { - out_3208578793914858910[0] = 0; - out_3208578793914858910[1] = 0; - out_3208578793914858910[2] = 0; - out_3208578793914858910[3] = 1; - out_3208578793914858910[4] = 0; - out_3208578793914858910[5] = 0; - out_3208578793914858910[6] = 0; - out_3208578793914858910[7] = 0; - out_3208578793914858910[8] = 0; -} -void h_29(double *state, double *unused, double *out_3294129054532975038) { - out_3294129054532975038[0] = state[1]; -} -void H_29(double *state, double *unused, double *out_523584137800041815) { - out_523584137800041815[0] = 0; - out_523584137800041815[1] = 1; - out_523584137800041815[2] = 0; - out_523584137800041815[3] = 0; - out_523584137800041815[4] = 0; - out_523584137800041815[5] = 0; - out_523584137800041815[6] = 0; - out_523584137800041815[7] = 0; - out_523584137800041815[8] = 0; -} -void h_28(double *state, double *unused, double *out_8374516446147645014) { - out_8374516446147645014[0] = state[0]; -} -void H_28(double *state, double *unused, double *out_1440046133765284436) { - out_1440046133765284436[0] = 1; - out_1440046133765284436[1] = 0; - out_1440046133765284436[2] = 0; - out_1440046133765284436[3] = 0; - out_1440046133765284436[4] = 0; - out_1440046133765284436[5] = 0; - out_1440046133765284436[6] = 0; - out_1440046133765284436[7] = 0; - out_1440046133765284436[8] = 0; -} -void h_31(double *state, double *unused, double *out_2115565022866995368) { - out_2115565022866995368[0] = state[8]; -} -void H_31(double *state, double *unused, double *out_873830573094233501) { - out_873830573094233501[0] = 0; - out_873830573094233501[1] = 0; - out_873830573094233501[2] = 0; - out_873830573094233501[3] = 0; - out_873830573094233501[4] = 0; - out_873830573094233501[5] = 0; - out_873830573094233501[6] = 0; - out_873830573094233501[7] = 0; - out_873830573094233501[8] = 1; +void err_fun(double *nom_x, double *delta_x, double *out_6814212569071197423) { + out_6814212569071197423[0] = delta_x[0] + nom_x[0]; + out_6814212569071197423[1] = delta_x[1] + nom_x[1]; + out_6814212569071197423[2] = delta_x[2] + nom_x[2]; + out_6814212569071197423[3] = delta_x[3] + nom_x[3]; + out_6814212569071197423[4] = delta_x[4] + nom_x[4]; + out_6814212569071197423[5] = delta_x[5] + nom_x[5]; + out_6814212569071197423[6] = delta_x[6] + nom_x[6]; + out_6814212569071197423[7] = delta_x[7] + nom_x[7]; + out_6814212569071197423[8] = delta_x[8] + nom_x[8]; +} +void inv_err_fun(double *nom_x, double *true_x, double *out_7666174205888484741) { + out_7666174205888484741[0] = -nom_x[0] + true_x[0]; + out_7666174205888484741[1] = -nom_x[1] + true_x[1]; + out_7666174205888484741[2] = -nom_x[2] + true_x[2]; + out_7666174205888484741[3] = -nom_x[3] + true_x[3]; + out_7666174205888484741[4] = -nom_x[4] + true_x[4]; + out_7666174205888484741[5] = -nom_x[5] + true_x[5]; + out_7666174205888484741[6] = -nom_x[6] + true_x[6]; + out_7666174205888484741[7] = -nom_x[7] + true_x[7]; + out_7666174205888484741[8] = -nom_x[8] + true_x[8]; +} +void H_mod_fun(double *state, double *out_8014857022495098622) { + out_8014857022495098622[0] = 1.0; + out_8014857022495098622[1] = 0; + out_8014857022495098622[2] = 0; + out_8014857022495098622[3] = 0; + out_8014857022495098622[4] = 0; + out_8014857022495098622[5] = 0; + out_8014857022495098622[6] = 0; + out_8014857022495098622[7] = 0; + out_8014857022495098622[8] = 0; + out_8014857022495098622[9] = 0; + out_8014857022495098622[10] = 1.0; + out_8014857022495098622[11] = 0; + out_8014857022495098622[12] = 0; + out_8014857022495098622[13] = 0; + out_8014857022495098622[14] = 0; + out_8014857022495098622[15] = 0; + out_8014857022495098622[16] = 0; + out_8014857022495098622[17] = 0; + out_8014857022495098622[18] = 0; + out_8014857022495098622[19] = 0; + out_8014857022495098622[20] = 1.0; + out_8014857022495098622[21] = 0; + out_8014857022495098622[22] = 0; + out_8014857022495098622[23] = 0; + out_8014857022495098622[24] = 0; + out_8014857022495098622[25] = 0; + out_8014857022495098622[26] = 0; + out_8014857022495098622[27] = 0; + out_8014857022495098622[28] = 0; + out_8014857022495098622[29] = 0; + out_8014857022495098622[30] = 1.0; + out_8014857022495098622[31] = 0; + out_8014857022495098622[32] = 0; + out_8014857022495098622[33] = 0; + out_8014857022495098622[34] = 0; + out_8014857022495098622[35] = 0; + out_8014857022495098622[36] = 0; + out_8014857022495098622[37] = 0; + out_8014857022495098622[38] = 0; + out_8014857022495098622[39] = 0; + out_8014857022495098622[40] = 1.0; + out_8014857022495098622[41] = 0; + out_8014857022495098622[42] = 0; + out_8014857022495098622[43] = 0; + out_8014857022495098622[44] = 0; + out_8014857022495098622[45] = 0; + out_8014857022495098622[46] = 0; + out_8014857022495098622[47] = 0; + out_8014857022495098622[48] = 0; + out_8014857022495098622[49] = 0; + out_8014857022495098622[50] = 1.0; + out_8014857022495098622[51] = 0; + out_8014857022495098622[52] = 0; + out_8014857022495098622[53] = 0; + out_8014857022495098622[54] = 0; + out_8014857022495098622[55] = 0; + out_8014857022495098622[56] = 0; + out_8014857022495098622[57] = 0; + out_8014857022495098622[58] = 0; + out_8014857022495098622[59] = 0; + out_8014857022495098622[60] = 1.0; + out_8014857022495098622[61] = 0; + out_8014857022495098622[62] = 0; + out_8014857022495098622[63] = 0; + out_8014857022495098622[64] = 0; + out_8014857022495098622[65] = 0; + out_8014857022495098622[66] = 0; + out_8014857022495098622[67] = 0; + out_8014857022495098622[68] = 0; + out_8014857022495098622[69] = 0; + out_8014857022495098622[70] = 1.0; + out_8014857022495098622[71] = 0; + out_8014857022495098622[72] = 0; + out_8014857022495098622[73] = 0; + out_8014857022495098622[74] = 0; + out_8014857022495098622[75] = 0; + out_8014857022495098622[76] = 0; + out_8014857022495098622[77] = 0; + out_8014857022495098622[78] = 0; + out_8014857022495098622[79] = 0; + out_8014857022495098622[80] = 1.0; +} +void f_fun(double *state, double dt, double *out_6791547762604559707) { + out_6791547762604559707[0] = state[0]; + out_6791547762604559707[1] = state[1]; + out_6791547762604559707[2] = state[2]; + out_6791547762604559707[3] = state[3]; + out_6791547762604559707[4] = state[4]; + out_6791547762604559707[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_6791547762604559707[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_6791547762604559707[7] = state[7]; + out_6791547762604559707[8] = state[8]; +} +void F_fun(double *state, double dt, double *out_3683219731909566018) { + out_3683219731909566018[0] = 1; + out_3683219731909566018[1] = 0; + out_3683219731909566018[2] = 0; + out_3683219731909566018[3] = 0; + out_3683219731909566018[4] = 0; + out_3683219731909566018[5] = 0; + out_3683219731909566018[6] = 0; + out_3683219731909566018[7] = 0; + out_3683219731909566018[8] = 0; + out_3683219731909566018[9] = 0; + out_3683219731909566018[10] = 1; + out_3683219731909566018[11] = 0; + out_3683219731909566018[12] = 0; + out_3683219731909566018[13] = 0; + out_3683219731909566018[14] = 0; + out_3683219731909566018[15] = 0; + out_3683219731909566018[16] = 0; + out_3683219731909566018[17] = 0; + out_3683219731909566018[18] = 0; + out_3683219731909566018[19] = 0; + out_3683219731909566018[20] = 1; + out_3683219731909566018[21] = 0; + out_3683219731909566018[22] = 0; + out_3683219731909566018[23] = 0; + out_3683219731909566018[24] = 0; + out_3683219731909566018[25] = 0; + out_3683219731909566018[26] = 0; + out_3683219731909566018[27] = 0; + out_3683219731909566018[28] = 0; + out_3683219731909566018[29] = 0; + out_3683219731909566018[30] = 1; + out_3683219731909566018[31] = 0; + out_3683219731909566018[32] = 0; + out_3683219731909566018[33] = 0; + out_3683219731909566018[34] = 0; + out_3683219731909566018[35] = 0; + out_3683219731909566018[36] = 0; + out_3683219731909566018[37] = 0; + out_3683219731909566018[38] = 0; + out_3683219731909566018[39] = 0; + out_3683219731909566018[40] = 1; + out_3683219731909566018[41] = 0; + out_3683219731909566018[42] = 0; + out_3683219731909566018[43] = 0; + out_3683219731909566018[44] = 0; + out_3683219731909566018[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_3683219731909566018[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3683219731909566018[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3683219731909566018[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3683219731909566018[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_3683219731909566018[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3683219731909566018[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3683219731909566018[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3683219731909566018[53] = -9.8000000000000007*dt; + out_3683219731909566018[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_3683219731909566018[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3683219731909566018[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3683219731909566018[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3683219731909566018[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_3683219731909566018[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3683219731909566018[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_3683219731909566018[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3683219731909566018[62] = 0; + out_3683219731909566018[63] = 0; + out_3683219731909566018[64] = 0; + out_3683219731909566018[65] = 0; + out_3683219731909566018[66] = 0; + out_3683219731909566018[67] = 0; + out_3683219731909566018[68] = 0; + out_3683219731909566018[69] = 0; + out_3683219731909566018[70] = 1; + out_3683219731909566018[71] = 0; + out_3683219731909566018[72] = 0; + out_3683219731909566018[73] = 0; + out_3683219731909566018[74] = 0; + out_3683219731909566018[75] = 0; + out_3683219731909566018[76] = 0; + out_3683219731909566018[77] = 0; + out_3683219731909566018[78] = 0; + out_3683219731909566018[79] = 0; + out_3683219731909566018[80] = 1; +} +void h_25(double *state, double *unused, double *out_2172051214329875381) { + out_2172051214329875381[0] = state[6]; +} +void H_25(double *state, double *unused, double *out_3364051928398716628) { + out_3364051928398716628[0] = 0; + out_3364051928398716628[1] = 0; + out_3364051928398716628[2] = 0; + out_3364051928398716628[3] = 0; + out_3364051928398716628[4] = 0; + out_3364051928398716628[5] = 0; + out_3364051928398716628[6] = 1; + out_3364051928398716628[7] = 0; + out_3364051928398716628[8] = 0; +} +void h_24(double *state, double *unused, double *out_7587553502939940622) { + out_7587553502939940622[0] = state[4]; + out_7587553502939940622[1] = state[5]; +} +void H_24(double *state, double *unused, double *out_2519055319615021229) { + out_2519055319615021229[0] = 0; + out_2519055319615021229[1] = 0; + out_2519055319615021229[2] = 0; + out_2519055319615021229[3] = 0; + out_2519055319615021229[4] = 1; + out_2519055319615021229[5] = 0; + out_2519055319615021229[6] = 0; + out_2519055319615021229[7] = 0; + out_2519055319615021229[8] = 0; + out_2519055319615021229[9] = 0; + out_2519055319615021229[10] = 0; + out_2519055319615021229[11] = 0; + out_2519055319615021229[12] = 0; + out_2519055319615021229[13] = 0; + out_2519055319615021229[14] = 1; + out_2519055319615021229[15] = 0; + out_2519055319615021229[16] = 0; + out_2519055319615021229[17] = 0; +} +void h_30(double *state, double *unused, double *out_6112207235155681847) { + out_6112207235155681847[0] = state[4]; +} +void H_30(double *state, double *unused, double *out_3552638413092900127) { + out_3552638413092900127[0] = 0; + out_3552638413092900127[1] = 0; + out_3552638413092900127[2] = 0; + out_3552638413092900127[3] = 0; + out_3552638413092900127[4] = 1; + out_3552638413092900127[5] = 0; + out_3552638413092900127[6] = 0; + out_3552638413092900127[7] = 0; + out_3552638413092900127[8] = 0; +} +void h_26(double *state, double *unused, double *out_967317172040034310) { + out_967317172040034310[0] = state[7]; +} +void H_26(double *state, double *unused, double *out_7105555247272772852) { + out_7105555247272772852[0] = 0; + out_7105555247272772852[1] = 0; + out_7105555247272772852[2] = 0; + out_7105555247272772852[3] = 0; + out_7105555247272772852[4] = 0; + out_7105555247272772852[5] = 0; + out_7105555247272772852[6] = 0; + out_7105555247272772852[7] = 1; + out_7105555247272772852[8] = 0; +} +void h_27(double *state, double *unused, double *out_351929141530343503) { + out_351929141530343503[0] = state[3]; +} +void H_27(double *state, double *unused, double *out_1377875101292475216) { + out_1377875101292475216[0] = 0; + out_1377875101292475216[1] = 0; + out_1377875101292475216[2] = 0; + out_1377875101292475216[3] = 1; + out_1377875101292475216[4] = 0; + out_1377875101292475216[5] = 0; + out_1377875101292475216[6] = 0; + out_1377875101292475216[7] = 0; + out_1377875101292475216[8] = 0; +} +void h_29(double *state, double *unused, double *out_627123203814849392) { + out_627123203814849392[0] = state[1]; +} +void H_29(double *state, double *unused, double *out_335487625577075817) { + out_335487625577075817[0] = 0; + out_335487625577075817[1] = 1; + out_335487625577075817[2] = 0; + out_335487625577075817[3] = 0; + out_335487625577075817[4] = 0; + out_335487625577075817[5] = 0; + out_335487625577075817[6] = 0; + out_335487625577075817[7] = 0; + out_335487625577075817[8] = 0; +} +void h_28(double *state, double *unused, double *out_2252476179278354148) { + out_2252476179278354148[0] = state[0]; +} +void H_28(double *state, double *unused, double *out_5417886642646606391) { + out_5417886642646606391[0] = 1; + out_5417886642646606391[1] = 0; + out_5417886642646606391[2] = 0; + out_5417886642646606391[3] = 0; + out_5417886642646606391[4] = 0; + out_5417886642646606391[5] = 0; + out_5417886642646606391[6] = 0; + out_5417886642646606391[7] = 0; + out_5417886642646606391[8] = 0; +} +void h_31(double *state, double *unused, double *out_8511427602559003794) { + out_8511427602559003794[0] = state[8]; +} +void H_31(double *state, double *unused, double *out_3333405966521756200) { + out_3333405966521756200[0] = 0; + out_3333405966521756200[1] = 0; + out_3333405966521756200[2] = 0; + out_3333405966521756200[3] = 0; + out_3333405966521756200[4] = 0; + out_3333405966521756200[5] = 0; + out_3333405966521756200[6] = 0; + out_3333405966521756200[7] = 0; + out_3333405966521756200[8] = 1; } #include #include @@ -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_7500597990915213513) { - err_fun(nom_x, delta_x, out_7500597990915213513); +void car_err_fun(double *nom_x, double *delta_x, double *out_6814212569071197423) { + err_fun(nom_x, delta_x, out_6814212569071197423); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2781571296576732525) { - inv_err_fun(nom_x, true_x, out_2781571296576732525); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7666174205888484741) { + inv_err_fun(nom_x, true_x, out_7666174205888484741); } -void car_H_mod_fun(double *state, double *out_3562418887379080574) { - H_mod_fun(state, out_3562418887379080574); +void car_H_mod_fun(double *state, double *out_8014857022495098622) { + H_mod_fun(state, out_8014857022495098622); } -void car_f_fun(double *state, double dt, double *out_8261543717010253727) { - f_fun(state, dt, out_8261543717010253727); +void car_f_fun(double *state, double dt, double *out_6791547762604559707) { + f_fun(state, dt, out_6791547762604559707); } -void car_F_fun(double *state, double dt, double *out_3007543019148089031) { - F_fun(state, dt, out_3007543019148089031); +void car_F_fun(double *state, double dt, double *out_3683219731909566018) { + F_fun(state, dt, out_3683219731909566018); } -void car_h_25(double *state, double *unused, double *out_5344993741381892957) { - h_25(state, unused, out_5344993741381892957); +void car_h_25(double *state, double *unused, double *out_2172051214329875381) { + h_25(state, unused, out_2172051214329875381); } -void car_H_25(double *state, double *unused, double *out_3493880848013174199) { - H_25(state, unused, out_3493880848013174199); +void car_H_25(double *state, double *unused, double *out_3364051928398716628) { + H_25(state, unused, out_3364051928398716628); } -void car_h_24(double *state, double *unused, double *out_7269781498191373830) { - h_24(state, unused, out_7269781498191373830); +void car_h_24(double *state, double *unused, double *out_7587553502939940622) { + h_24(state, unused, out_7587553502939940622); } -void car_H_24(double *state, double *unused, double *out_1321231249007674633) { - H_24(state, unused, out_1321231249007674633); +void car_H_24(double *state, double *unused, double *out_2519055319615021229) { + H_24(state, unused, out_2519055319615021229); } -void car_h_30(double *state, double *unused, double *out_1707544213127870607) { - h_30(state, unused, out_1707544213127870607); +void car_h_30(double *state, double *unused, double *out_6112207235155681847) { + h_30(state, unused, out_6112207235155681847); } -void car_H_30(double *state, double *unused, double *out_1033815482114433999) { - H_30(state, unused, out_1033815482114433999); +void car_H_30(double *state, double *unused, double *out_3552638413092900127) { + H_30(state, unused, out_3552638413092900127); } -void car_h_26(double *state, double *unused, double *out_7274424958878970158) { - h_26(state, unused, out_7274424958878970158); +void car_h_26(double *state, double *unused, double *out_967317172040034310) { + h_26(state, unused, out_967317172040034310); } -void car_H_26(double *state, double *unused, double *out_247622470860882025) { - H_26(state, unused, out_247622470860882025); +void car_H_26(double *state, double *unused, double *out_7105555247272772852) { + H_26(state, unused, out_7105555247272772852); } -void car_h_27(double *state, double *unused, double *out_343320473721047312) { - h_27(state, unused, out_343320473721047312); +void car_h_27(double *state, double *unused, double *out_351929141530343503) { + h_27(state, unused, out_351929141530343503); } -void car_H_27(double *state, double *unused, double *out_3208578793914858910) { - H_27(state, unused, out_3208578793914858910); +void car_H_27(double *state, double *unused, double *out_1377875101292475216) { + H_27(state, unused, out_1377875101292475216); } -void car_h_29(double *state, double *unused, double *out_3294129054532975038) { - h_29(state, unused, out_3294129054532975038); +void car_h_29(double *state, double *unused, double *out_627123203814849392) { + h_29(state, unused, out_627123203814849392); } -void car_H_29(double *state, double *unused, double *out_523584137800041815) { - H_29(state, unused, out_523584137800041815); +void car_H_29(double *state, double *unused, double *out_335487625577075817) { + H_29(state, unused, out_335487625577075817); } -void car_h_28(double *state, double *unused, double *out_8374516446147645014) { - h_28(state, unused, out_8374516446147645014); +void car_h_28(double *state, double *unused, double *out_2252476179278354148) { + h_28(state, unused, out_2252476179278354148); } -void car_H_28(double *state, double *unused, double *out_1440046133765284436) { - H_28(state, unused, out_1440046133765284436); +void car_H_28(double *state, double *unused, double *out_5417886642646606391) { + H_28(state, unused, out_5417886642646606391); } -void car_h_31(double *state, double *unused, double *out_2115565022866995368) { - h_31(state, unused, out_2115565022866995368); +void car_h_31(double *state, double *unused, double *out_8511427602559003794) { + h_31(state, unused, out_8511427602559003794); } -void car_H_31(double *state, double *unused, double *out_873830573094233501) { - H_31(state, unused, out_873830573094233501); +void car_H_31(double *state, double *unused, double *out_3333405966521756200) { + H_31(state, unused, out_3333405966521756200); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 716d1223d..3f11ea4a9 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -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_7500597990915213513); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_2781571296576732525); -void car_H_mod_fun(double *state, double *out_3562418887379080574); -void car_f_fun(double *state, double dt, double *out_8261543717010253727); -void car_F_fun(double *state, double dt, double *out_3007543019148089031); -void car_h_25(double *state, double *unused, double *out_5344993741381892957); -void car_H_25(double *state, double *unused, double *out_3493880848013174199); -void car_h_24(double *state, double *unused, double *out_7269781498191373830); -void car_H_24(double *state, double *unused, double *out_1321231249007674633); -void car_h_30(double *state, double *unused, double *out_1707544213127870607); -void car_H_30(double *state, double *unused, double *out_1033815482114433999); -void car_h_26(double *state, double *unused, double *out_7274424958878970158); -void car_H_26(double *state, double *unused, double *out_247622470860882025); -void car_h_27(double *state, double *unused, double *out_343320473721047312); -void car_H_27(double *state, double *unused, double *out_3208578793914858910); -void car_h_29(double *state, double *unused, double *out_3294129054532975038); -void car_H_29(double *state, double *unused, double *out_523584137800041815); -void car_h_28(double *state, double *unused, double *out_8374516446147645014); -void car_H_28(double *state, double *unused, double *out_1440046133765284436); -void car_h_31(double *state, double *unused, double *out_2115565022866995368); -void car_H_31(double *state, double *unused, double *out_873830573094233501); +void car_err_fun(double *nom_x, double *delta_x, double *out_6814212569071197423); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_7666174205888484741); +void car_H_mod_fun(double *state, double *out_8014857022495098622); +void car_f_fun(double *state, double dt, double *out_6791547762604559707); +void car_F_fun(double *state, double dt, double *out_3683219731909566018); +void car_h_25(double *state, double *unused, double *out_2172051214329875381); +void car_H_25(double *state, double *unused, double *out_3364051928398716628); +void car_h_24(double *state, double *unused, double *out_7587553502939940622); +void car_H_24(double *state, double *unused, double *out_2519055319615021229); +void car_h_30(double *state, double *unused, double *out_6112207235155681847); +void car_H_30(double *state, double *unused, double *out_3552638413092900127); +void car_h_26(double *state, double *unused, double *out_967317172040034310); +void car_H_26(double *state, double *unused, double *out_7105555247272772852); +void car_h_27(double *state, double *unused, double *out_351929141530343503); +void car_H_27(double *state, double *unused, double *out_1377875101292475216); +void car_h_29(double *state, double *unused, double *out_627123203814849392); +void car_H_29(double *state, double *unused, double *out_335487625577075817); +void car_h_28(double *state, double *unused, double *out_2252476179278354148); +void car_H_28(double *state, double *unused, double *out_5417886642646606391); +void car_h_31(double *state, double *unused, double *out_8511427602559003794); +void car_H_31(double *state, double *unused, double *out_3333405966521756200); 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); diff --git a/selfdrive/locationd/models/generated/gnss.cpp b/selfdrive/locationd/models/generated/gnss.cpp index 0f1339c40..4b64a230e 100644 --- a/selfdrive/locationd/models/generated/gnss.cpp +++ b/selfdrive/locationd/models/generated/gnss.cpp @@ -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_4620912923766298580) { - out_4620912923766298580[0] = delta_x[0] + nom_x[0]; - out_4620912923766298580[1] = delta_x[1] + nom_x[1]; - out_4620912923766298580[2] = delta_x[2] + nom_x[2]; - out_4620912923766298580[3] = delta_x[3] + nom_x[3]; - out_4620912923766298580[4] = delta_x[4] + nom_x[4]; - out_4620912923766298580[5] = delta_x[5] + nom_x[5]; - out_4620912923766298580[6] = delta_x[6] + nom_x[6]; - out_4620912923766298580[7] = delta_x[7] + nom_x[7]; - out_4620912923766298580[8] = delta_x[8] + nom_x[8]; - out_4620912923766298580[9] = delta_x[9] + nom_x[9]; - out_4620912923766298580[10] = delta_x[10] + nom_x[10]; -} -void inv_err_fun(double *nom_x, double *true_x, double *out_9187715529705882522) { - out_9187715529705882522[0] = -nom_x[0] + true_x[0]; - out_9187715529705882522[1] = -nom_x[1] + true_x[1]; - out_9187715529705882522[2] = -nom_x[2] + true_x[2]; - out_9187715529705882522[3] = -nom_x[3] + true_x[3]; - out_9187715529705882522[4] = -nom_x[4] + true_x[4]; - out_9187715529705882522[5] = -nom_x[5] + true_x[5]; - out_9187715529705882522[6] = -nom_x[6] + true_x[6]; - out_9187715529705882522[7] = -nom_x[7] + true_x[7]; - out_9187715529705882522[8] = -nom_x[8] + true_x[8]; - out_9187715529705882522[9] = -nom_x[9] + true_x[9]; - out_9187715529705882522[10] = -nom_x[10] + true_x[10]; -} -void H_mod_fun(double *state, double *out_922973610943044438) { - out_922973610943044438[0] = 1.0; - out_922973610943044438[1] = 0; - out_922973610943044438[2] = 0; - out_922973610943044438[3] = 0; - out_922973610943044438[4] = 0; - out_922973610943044438[5] = 0; - out_922973610943044438[6] = 0; - out_922973610943044438[7] = 0; - out_922973610943044438[8] = 0; - out_922973610943044438[9] = 0; - out_922973610943044438[10] = 0; - out_922973610943044438[11] = 0; - out_922973610943044438[12] = 1.0; - out_922973610943044438[13] = 0; - out_922973610943044438[14] = 0; - out_922973610943044438[15] = 0; - out_922973610943044438[16] = 0; - out_922973610943044438[17] = 0; - out_922973610943044438[18] = 0; - out_922973610943044438[19] = 0; - out_922973610943044438[20] = 0; - out_922973610943044438[21] = 0; - out_922973610943044438[22] = 0; - out_922973610943044438[23] = 0; - out_922973610943044438[24] = 1.0; - out_922973610943044438[25] = 0; - out_922973610943044438[26] = 0; - out_922973610943044438[27] = 0; - out_922973610943044438[28] = 0; - out_922973610943044438[29] = 0; - out_922973610943044438[30] = 0; - out_922973610943044438[31] = 0; - out_922973610943044438[32] = 0; - out_922973610943044438[33] = 0; - out_922973610943044438[34] = 0; - out_922973610943044438[35] = 0; - out_922973610943044438[36] = 1.0; - out_922973610943044438[37] = 0; - out_922973610943044438[38] = 0; - out_922973610943044438[39] = 0; - out_922973610943044438[40] = 0; - out_922973610943044438[41] = 0; - out_922973610943044438[42] = 0; - out_922973610943044438[43] = 0; - out_922973610943044438[44] = 0; - out_922973610943044438[45] = 0; - out_922973610943044438[46] = 0; - out_922973610943044438[47] = 0; - out_922973610943044438[48] = 1.0; - out_922973610943044438[49] = 0; - out_922973610943044438[50] = 0; - out_922973610943044438[51] = 0; - out_922973610943044438[52] = 0; - out_922973610943044438[53] = 0; - out_922973610943044438[54] = 0; - out_922973610943044438[55] = 0; - out_922973610943044438[56] = 0; - out_922973610943044438[57] = 0; - out_922973610943044438[58] = 0; - out_922973610943044438[59] = 0; - out_922973610943044438[60] = 1.0; - out_922973610943044438[61] = 0; - out_922973610943044438[62] = 0; - out_922973610943044438[63] = 0; - out_922973610943044438[64] = 0; - out_922973610943044438[65] = 0; - out_922973610943044438[66] = 0; - out_922973610943044438[67] = 0; - out_922973610943044438[68] = 0; - out_922973610943044438[69] = 0; - out_922973610943044438[70] = 0; - out_922973610943044438[71] = 0; - out_922973610943044438[72] = 1.0; - out_922973610943044438[73] = 0; - out_922973610943044438[74] = 0; - out_922973610943044438[75] = 0; - out_922973610943044438[76] = 0; - out_922973610943044438[77] = 0; - out_922973610943044438[78] = 0; - out_922973610943044438[79] = 0; - out_922973610943044438[80] = 0; - out_922973610943044438[81] = 0; - out_922973610943044438[82] = 0; - out_922973610943044438[83] = 0; - out_922973610943044438[84] = 1.0; - out_922973610943044438[85] = 0; - out_922973610943044438[86] = 0; - out_922973610943044438[87] = 0; - out_922973610943044438[88] = 0; - out_922973610943044438[89] = 0; - out_922973610943044438[90] = 0; - out_922973610943044438[91] = 0; - out_922973610943044438[92] = 0; - out_922973610943044438[93] = 0; - out_922973610943044438[94] = 0; - out_922973610943044438[95] = 0; - out_922973610943044438[96] = 1.0; - out_922973610943044438[97] = 0; - out_922973610943044438[98] = 0; - out_922973610943044438[99] = 0; - out_922973610943044438[100] = 0; - out_922973610943044438[101] = 0; - out_922973610943044438[102] = 0; - out_922973610943044438[103] = 0; - out_922973610943044438[104] = 0; - out_922973610943044438[105] = 0; - out_922973610943044438[106] = 0; - out_922973610943044438[107] = 0; - out_922973610943044438[108] = 1.0; - out_922973610943044438[109] = 0; - out_922973610943044438[110] = 0; - out_922973610943044438[111] = 0; - out_922973610943044438[112] = 0; - out_922973610943044438[113] = 0; - out_922973610943044438[114] = 0; - out_922973610943044438[115] = 0; - out_922973610943044438[116] = 0; - out_922973610943044438[117] = 0; - out_922973610943044438[118] = 0; - out_922973610943044438[119] = 0; - out_922973610943044438[120] = 1.0; -} -void f_fun(double *state, double dt, double *out_1079402724574081004) { - out_1079402724574081004[0] = dt*state[3] + state[0]; - out_1079402724574081004[1] = dt*state[4] + state[1]; - out_1079402724574081004[2] = dt*state[5] + state[2]; - out_1079402724574081004[3] = state[3]; - out_1079402724574081004[4] = state[4]; - out_1079402724574081004[5] = state[5]; - out_1079402724574081004[6] = dt*state[7] + state[6]; - out_1079402724574081004[7] = dt*state[8] + state[7]; - out_1079402724574081004[8] = state[8]; - out_1079402724574081004[9] = state[9]; - out_1079402724574081004[10] = state[10]; -} -void F_fun(double *state, double dt, double *out_3039166340276301210) { - out_3039166340276301210[0] = 1; - out_3039166340276301210[1] = 0; - out_3039166340276301210[2] = 0; - out_3039166340276301210[3] = dt; - out_3039166340276301210[4] = 0; - out_3039166340276301210[5] = 0; - out_3039166340276301210[6] = 0; - out_3039166340276301210[7] = 0; - out_3039166340276301210[8] = 0; - out_3039166340276301210[9] = 0; - out_3039166340276301210[10] = 0; - out_3039166340276301210[11] = 0; - out_3039166340276301210[12] = 1; - out_3039166340276301210[13] = 0; - out_3039166340276301210[14] = 0; - out_3039166340276301210[15] = dt; - out_3039166340276301210[16] = 0; - out_3039166340276301210[17] = 0; - out_3039166340276301210[18] = 0; - out_3039166340276301210[19] = 0; - out_3039166340276301210[20] = 0; - out_3039166340276301210[21] = 0; - out_3039166340276301210[22] = 0; - out_3039166340276301210[23] = 0; - out_3039166340276301210[24] = 1; - out_3039166340276301210[25] = 0; - out_3039166340276301210[26] = 0; - out_3039166340276301210[27] = dt; - out_3039166340276301210[28] = 0; - out_3039166340276301210[29] = 0; - out_3039166340276301210[30] = 0; - out_3039166340276301210[31] = 0; - out_3039166340276301210[32] = 0; - out_3039166340276301210[33] = 0; - out_3039166340276301210[34] = 0; - out_3039166340276301210[35] = 0; - out_3039166340276301210[36] = 1; - out_3039166340276301210[37] = 0; - out_3039166340276301210[38] = 0; - out_3039166340276301210[39] = 0; - out_3039166340276301210[40] = 0; - out_3039166340276301210[41] = 0; - out_3039166340276301210[42] = 0; - out_3039166340276301210[43] = 0; - out_3039166340276301210[44] = 0; - out_3039166340276301210[45] = 0; - out_3039166340276301210[46] = 0; - out_3039166340276301210[47] = 0; - out_3039166340276301210[48] = 1; - out_3039166340276301210[49] = 0; - out_3039166340276301210[50] = 0; - out_3039166340276301210[51] = 0; - out_3039166340276301210[52] = 0; - out_3039166340276301210[53] = 0; - out_3039166340276301210[54] = 0; - out_3039166340276301210[55] = 0; - out_3039166340276301210[56] = 0; - out_3039166340276301210[57] = 0; - out_3039166340276301210[58] = 0; - out_3039166340276301210[59] = 0; - out_3039166340276301210[60] = 1; - out_3039166340276301210[61] = 0; - out_3039166340276301210[62] = 0; - out_3039166340276301210[63] = 0; - out_3039166340276301210[64] = 0; - out_3039166340276301210[65] = 0; - out_3039166340276301210[66] = 0; - out_3039166340276301210[67] = 0; - out_3039166340276301210[68] = 0; - out_3039166340276301210[69] = 0; - out_3039166340276301210[70] = 0; - out_3039166340276301210[71] = 0; - out_3039166340276301210[72] = 1; - out_3039166340276301210[73] = dt; - out_3039166340276301210[74] = 0; - out_3039166340276301210[75] = 0; - out_3039166340276301210[76] = 0; - out_3039166340276301210[77] = 0; - out_3039166340276301210[78] = 0; - out_3039166340276301210[79] = 0; - out_3039166340276301210[80] = 0; - out_3039166340276301210[81] = 0; - out_3039166340276301210[82] = 0; - out_3039166340276301210[83] = 0; - out_3039166340276301210[84] = 1; - out_3039166340276301210[85] = dt; - out_3039166340276301210[86] = 0; - out_3039166340276301210[87] = 0; - out_3039166340276301210[88] = 0; - out_3039166340276301210[89] = 0; - out_3039166340276301210[90] = 0; - out_3039166340276301210[91] = 0; - out_3039166340276301210[92] = 0; - out_3039166340276301210[93] = 0; - out_3039166340276301210[94] = 0; - out_3039166340276301210[95] = 0; - out_3039166340276301210[96] = 1; - out_3039166340276301210[97] = 0; - out_3039166340276301210[98] = 0; - out_3039166340276301210[99] = 0; - out_3039166340276301210[100] = 0; - out_3039166340276301210[101] = 0; - out_3039166340276301210[102] = 0; - out_3039166340276301210[103] = 0; - out_3039166340276301210[104] = 0; - out_3039166340276301210[105] = 0; - out_3039166340276301210[106] = 0; - out_3039166340276301210[107] = 0; - out_3039166340276301210[108] = 1; - out_3039166340276301210[109] = 0; - out_3039166340276301210[110] = 0; - out_3039166340276301210[111] = 0; - out_3039166340276301210[112] = 0; - out_3039166340276301210[113] = 0; - out_3039166340276301210[114] = 0; - out_3039166340276301210[115] = 0; - out_3039166340276301210[116] = 0; - out_3039166340276301210[117] = 0; - out_3039166340276301210[118] = 0; - out_3039166340276301210[119] = 0; - out_3039166340276301210[120] = 1; -} -void h_6(double *state, double *sat_pos, double *out_3874365000426709074) { - out_3874365000426709074[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_3986922819635287129) { - out_3986922819635287129[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_3986922819635287129[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_3986922819635287129[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_3986922819635287129[3] = 0; - out_3986922819635287129[4] = 0; - out_3986922819635287129[5] = 0; - out_3986922819635287129[6] = 1; - out_3986922819635287129[7] = 0; - out_3986922819635287129[8] = 0; - out_3986922819635287129[9] = 0; - out_3986922819635287129[10] = 0; -} -void h_20(double *state, double *sat_pos, double *out_5069168270486129229) { - out_5069168270486129229[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_678918665408427203) { - out_678918665408427203[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_678918665408427203[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_678918665408427203[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_678918665408427203[3] = 0; - out_678918665408427203[4] = 0; - out_678918665408427203[5] = 0; - out_678918665408427203[6] = 1; - out_678918665408427203[7] = 0; - out_678918665408427203[8] = 0; - out_678918665408427203[9] = 1; - out_678918665408427203[10] = sat_pos[3]; -} -void h_7(double *state, double *sat_pos_vel, double *out_9105285764076347784) { - out_9105285764076347784[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_7685540822919128148) { - out_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[6] = 0; - out_7685540822919128148[7] = 1; - out_7685540822919128148[8] = 0; - out_7685540822919128148[9] = 0; - out_7685540822919128148[10] = 0; -} -void h_21(double *state, double *sat_pos_vel, double *out_9105285764076347784) { - out_9105285764076347784[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_7685540822919128148) { - out_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[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_7685540822919128148[6] = 0; - out_7685540822919128148[7] = 1; - out_7685540822919128148[8] = 0; - out_7685540822919128148[9] = 0; - out_7685540822919128148[10] = 0; +void err_fun(double *nom_x, double *delta_x, double *out_80014774094776572) { + out_80014774094776572[0] = delta_x[0] + nom_x[0]; + out_80014774094776572[1] = delta_x[1] + nom_x[1]; + out_80014774094776572[2] = delta_x[2] + nom_x[2]; + out_80014774094776572[3] = delta_x[3] + nom_x[3]; + out_80014774094776572[4] = delta_x[4] + nom_x[4]; + out_80014774094776572[5] = delta_x[5] + nom_x[5]; + out_80014774094776572[6] = delta_x[6] + nom_x[6]; + out_80014774094776572[7] = delta_x[7] + nom_x[7]; + out_80014774094776572[8] = delta_x[8] + nom_x[8]; + out_80014774094776572[9] = delta_x[9] + nom_x[9]; + out_80014774094776572[10] = delta_x[10] + nom_x[10]; +} +void inv_err_fun(double *nom_x, double *true_x, double *out_9144507820461143592) { + out_9144507820461143592[0] = -nom_x[0] + true_x[0]; + out_9144507820461143592[1] = -nom_x[1] + true_x[1]; + out_9144507820461143592[2] = -nom_x[2] + true_x[2]; + out_9144507820461143592[3] = -nom_x[3] + true_x[3]; + out_9144507820461143592[4] = -nom_x[4] + true_x[4]; + out_9144507820461143592[5] = -nom_x[5] + true_x[5]; + out_9144507820461143592[6] = -nom_x[6] + true_x[6]; + out_9144507820461143592[7] = -nom_x[7] + true_x[7]; + out_9144507820461143592[8] = -nom_x[8] + true_x[8]; + out_9144507820461143592[9] = -nom_x[9] + true_x[9]; + out_9144507820461143592[10] = -nom_x[10] + true_x[10]; +} +void H_mod_fun(double *state, double *out_3038444346073204801) { + out_3038444346073204801[0] = 1.0; + out_3038444346073204801[1] = 0; + out_3038444346073204801[2] = 0; + out_3038444346073204801[3] = 0; + out_3038444346073204801[4] = 0; + out_3038444346073204801[5] = 0; + out_3038444346073204801[6] = 0; + out_3038444346073204801[7] = 0; + out_3038444346073204801[8] = 0; + out_3038444346073204801[9] = 0; + out_3038444346073204801[10] = 0; + out_3038444346073204801[11] = 0; + out_3038444346073204801[12] = 1.0; + out_3038444346073204801[13] = 0; + out_3038444346073204801[14] = 0; + out_3038444346073204801[15] = 0; + out_3038444346073204801[16] = 0; + out_3038444346073204801[17] = 0; + out_3038444346073204801[18] = 0; + out_3038444346073204801[19] = 0; + out_3038444346073204801[20] = 0; + out_3038444346073204801[21] = 0; + out_3038444346073204801[22] = 0; + out_3038444346073204801[23] = 0; + out_3038444346073204801[24] = 1.0; + out_3038444346073204801[25] = 0; + out_3038444346073204801[26] = 0; + out_3038444346073204801[27] = 0; + out_3038444346073204801[28] = 0; + out_3038444346073204801[29] = 0; + out_3038444346073204801[30] = 0; + out_3038444346073204801[31] = 0; + out_3038444346073204801[32] = 0; + out_3038444346073204801[33] = 0; + out_3038444346073204801[34] = 0; + out_3038444346073204801[35] = 0; + out_3038444346073204801[36] = 1.0; + out_3038444346073204801[37] = 0; + out_3038444346073204801[38] = 0; + out_3038444346073204801[39] = 0; + out_3038444346073204801[40] = 0; + out_3038444346073204801[41] = 0; + out_3038444346073204801[42] = 0; + out_3038444346073204801[43] = 0; + out_3038444346073204801[44] = 0; + out_3038444346073204801[45] = 0; + out_3038444346073204801[46] = 0; + out_3038444346073204801[47] = 0; + out_3038444346073204801[48] = 1.0; + out_3038444346073204801[49] = 0; + out_3038444346073204801[50] = 0; + out_3038444346073204801[51] = 0; + out_3038444346073204801[52] = 0; + out_3038444346073204801[53] = 0; + out_3038444346073204801[54] = 0; + out_3038444346073204801[55] = 0; + out_3038444346073204801[56] = 0; + out_3038444346073204801[57] = 0; + out_3038444346073204801[58] = 0; + out_3038444346073204801[59] = 0; + out_3038444346073204801[60] = 1.0; + out_3038444346073204801[61] = 0; + out_3038444346073204801[62] = 0; + out_3038444346073204801[63] = 0; + out_3038444346073204801[64] = 0; + out_3038444346073204801[65] = 0; + out_3038444346073204801[66] = 0; + out_3038444346073204801[67] = 0; + out_3038444346073204801[68] = 0; + out_3038444346073204801[69] = 0; + out_3038444346073204801[70] = 0; + out_3038444346073204801[71] = 0; + out_3038444346073204801[72] = 1.0; + out_3038444346073204801[73] = 0; + out_3038444346073204801[74] = 0; + out_3038444346073204801[75] = 0; + out_3038444346073204801[76] = 0; + out_3038444346073204801[77] = 0; + out_3038444346073204801[78] = 0; + out_3038444346073204801[79] = 0; + out_3038444346073204801[80] = 0; + out_3038444346073204801[81] = 0; + out_3038444346073204801[82] = 0; + out_3038444346073204801[83] = 0; + out_3038444346073204801[84] = 1.0; + out_3038444346073204801[85] = 0; + out_3038444346073204801[86] = 0; + out_3038444346073204801[87] = 0; + out_3038444346073204801[88] = 0; + out_3038444346073204801[89] = 0; + out_3038444346073204801[90] = 0; + out_3038444346073204801[91] = 0; + out_3038444346073204801[92] = 0; + out_3038444346073204801[93] = 0; + out_3038444346073204801[94] = 0; + out_3038444346073204801[95] = 0; + out_3038444346073204801[96] = 1.0; + out_3038444346073204801[97] = 0; + out_3038444346073204801[98] = 0; + out_3038444346073204801[99] = 0; + out_3038444346073204801[100] = 0; + out_3038444346073204801[101] = 0; + out_3038444346073204801[102] = 0; + out_3038444346073204801[103] = 0; + out_3038444346073204801[104] = 0; + out_3038444346073204801[105] = 0; + out_3038444346073204801[106] = 0; + out_3038444346073204801[107] = 0; + out_3038444346073204801[108] = 1.0; + out_3038444346073204801[109] = 0; + out_3038444346073204801[110] = 0; + out_3038444346073204801[111] = 0; + out_3038444346073204801[112] = 0; + out_3038444346073204801[113] = 0; + out_3038444346073204801[114] = 0; + out_3038444346073204801[115] = 0; + out_3038444346073204801[116] = 0; + out_3038444346073204801[117] = 0; + out_3038444346073204801[118] = 0; + out_3038444346073204801[119] = 0; + out_3038444346073204801[120] = 1.0; +} +void f_fun(double *state, double dt, double *out_5331942404729498203) { + out_5331942404729498203[0] = dt*state[3] + state[0]; + out_5331942404729498203[1] = dt*state[4] + state[1]; + out_5331942404729498203[2] = dt*state[5] + state[2]; + out_5331942404729498203[3] = state[3]; + out_5331942404729498203[4] = state[4]; + out_5331942404729498203[5] = state[5]; + out_5331942404729498203[6] = dt*state[7] + state[6]; + out_5331942404729498203[7] = dt*state[8] + state[7]; + out_5331942404729498203[8] = state[8]; + out_5331942404729498203[9] = state[9]; + out_5331942404729498203[10] = state[10]; +} +void F_fun(double *state, double dt, double *out_3160440324773884395) { + out_3160440324773884395[0] = 1; + out_3160440324773884395[1] = 0; + out_3160440324773884395[2] = 0; + out_3160440324773884395[3] = dt; + out_3160440324773884395[4] = 0; + out_3160440324773884395[5] = 0; + out_3160440324773884395[6] = 0; + out_3160440324773884395[7] = 0; + out_3160440324773884395[8] = 0; + out_3160440324773884395[9] = 0; + out_3160440324773884395[10] = 0; + out_3160440324773884395[11] = 0; + out_3160440324773884395[12] = 1; + out_3160440324773884395[13] = 0; + out_3160440324773884395[14] = 0; + out_3160440324773884395[15] = dt; + out_3160440324773884395[16] = 0; + out_3160440324773884395[17] = 0; + out_3160440324773884395[18] = 0; + out_3160440324773884395[19] = 0; + out_3160440324773884395[20] = 0; + out_3160440324773884395[21] = 0; + out_3160440324773884395[22] = 0; + out_3160440324773884395[23] = 0; + out_3160440324773884395[24] = 1; + out_3160440324773884395[25] = 0; + out_3160440324773884395[26] = 0; + out_3160440324773884395[27] = dt; + out_3160440324773884395[28] = 0; + out_3160440324773884395[29] = 0; + out_3160440324773884395[30] = 0; + out_3160440324773884395[31] = 0; + out_3160440324773884395[32] = 0; + out_3160440324773884395[33] = 0; + out_3160440324773884395[34] = 0; + out_3160440324773884395[35] = 0; + out_3160440324773884395[36] = 1; + out_3160440324773884395[37] = 0; + out_3160440324773884395[38] = 0; + out_3160440324773884395[39] = 0; + out_3160440324773884395[40] = 0; + out_3160440324773884395[41] = 0; + out_3160440324773884395[42] = 0; + out_3160440324773884395[43] = 0; + out_3160440324773884395[44] = 0; + out_3160440324773884395[45] = 0; + out_3160440324773884395[46] = 0; + out_3160440324773884395[47] = 0; + out_3160440324773884395[48] = 1; + out_3160440324773884395[49] = 0; + out_3160440324773884395[50] = 0; + out_3160440324773884395[51] = 0; + out_3160440324773884395[52] = 0; + out_3160440324773884395[53] = 0; + out_3160440324773884395[54] = 0; + out_3160440324773884395[55] = 0; + out_3160440324773884395[56] = 0; + out_3160440324773884395[57] = 0; + out_3160440324773884395[58] = 0; + out_3160440324773884395[59] = 0; + out_3160440324773884395[60] = 1; + out_3160440324773884395[61] = 0; + out_3160440324773884395[62] = 0; + out_3160440324773884395[63] = 0; + out_3160440324773884395[64] = 0; + out_3160440324773884395[65] = 0; + out_3160440324773884395[66] = 0; + out_3160440324773884395[67] = 0; + out_3160440324773884395[68] = 0; + out_3160440324773884395[69] = 0; + out_3160440324773884395[70] = 0; + out_3160440324773884395[71] = 0; + out_3160440324773884395[72] = 1; + out_3160440324773884395[73] = dt; + out_3160440324773884395[74] = 0; + out_3160440324773884395[75] = 0; + out_3160440324773884395[76] = 0; + out_3160440324773884395[77] = 0; + out_3160440324773884395[78] = 0; + out_3160440324773884395[79] = 0; + out_3160440324773884395[80] = 0; + out_3160440324773884395[81] = 0; + out_3160440324773884395[82] = 0; + out_3160440324773884395[83] = 0; + out_3160440324773884395[84] = 1; + out_3160440324773884395[85] = dt; + out_3160440324773884395[86] = 0; + out_3160440324773884395[87] = 0; + out_3160440324773884395[88] = 0; + out_3160440324773884395[89] = 0; + out_3160440324773884395[90] = 0; + out_3160440324773884395[91] = 0; + out_3160440324773884395[92] = 0; + out_3160440324773884395[93] = 0; + out_3160440324773884395[94] = 0; + out_3160440324773884395[95] = 0; + out_3160440324773884395[96] = 1; + out_3160440324773884395[97] = 0; + out_3160440324773884395[98] = 0; + out_3160440324773884395[99] = 0; + out_3160440324773884395[100] = 0; + out_3160440324773884395[101] = 0; + out_3160440324773884395[102] = 0; + out_3160440324773884395[103] = 0; + out_3160440324773884395[104] = 0; + out_3160440324773884395[105] = 0; + out_3160440324773884395[106] = 0; + out_3160440324773884395[107] = 0; + out_3160440324773884395[108] = 1; + out_3160440324773884395[109] = 0; + out_3160440324773884395[110] = 0; + out_3160440324773884395[111] = 0; + out_3160440324773884395[112] = 0; + out_3160440324773884395[113] = 0; + out_3160440324773884395[114] = 0; + out_3160440324773884395[115] = 0; + out_3160440324773884395[116] = 0; + out_3160440324773884395[117] = 0; + out_3160440324773884395[118] = 0; + out_3160440324773884395[119] = 0; + out_3160440324773884395[120] = 1; +} +void h_6(double *state, double *sat_pos, double *out_4164777334676317660) { + out_4164777334676317660[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_8721522254593615931) { + out_8721522254593615931[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_8721522254593615931[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_8721522254593615931[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_8721522254593615931[3] = 0; + out_8721522254593615931[4] = 0; + out_8721522254593615931[5] = 0; + out_8721522254593615931[6] = 1; + out_8721522254593615931[7] = 0; + out_8721522254593615931[8] = 0; + out_8721522254593615931[9] = 0; + out_8721522254593615931[10] = 0; +} +void h_20(double *state, double *sat_pos, double *out_1506605518158843733) { + out_1506605518158843733[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_1146945854611411680) { + out_1146945854611411680[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_1146945854611411680[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_1146945854611411680[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_1146945854611411680[3] = 0; + out_1146945854611411680[4] = 0; + out_1146945854611411680[5] = 0; + out_1146945854611411680[6] = 1; + out_1146945854611411680[7] = 0; + out_1146945854611411680[8] = 0; + out_1146945854611411680[9] = 1; + out_1146945854611411680[10] = sat_pos[3]; +} +void h_7(double *state, double *sat_pos_vel, double *out_2838038345896688742) { + out_2838038345896688742[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_7683900239883798223) { + out_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[6] = 0; + out_7683900239883798223[7] = 1; + out_7683900239883798223[8] = 0; + out_7683900239883798223[9] = 0; + out_7683900239883798223[10] = 0; +} +void h_21(double *state, double *sat_pos_vel, double *out_2838038345896688742) { + out_2838038345896688742[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_7683900239883798223) { + out_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[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_7683900239883798223[6] = 0; + out_7683900239883798223[7] = 1; + out_7683900239883798223[8] = 0; + out_7683900239883798223[9] = 0; + out_7683900239883798223[10] = 0; } #include #include @@ -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_4620912923766298580) { - err_fun(nom_x, delta_x, out_4620912923766298580); +void gnss_err_fun(double *nom_x, double *delta_x, double *out_80014774094776572) { + err_fun(nom_x, delta_x, out_80014774094776572); } -void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9187715529705882522) { - inv_err_fun(nom_x, true_x, out_9187715529705882522); +void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9144507820461143592) { + inv_err_fun(nom_x, true_x, out_9144507820461143592); } -void gnss_H_mod_fun(double *state, double *out_922973610943044438) { - H_mod_fun(state, out_922973610943044438); +void gnss_H_mod_fun(double *state, double *out_3038444346073204801) { + H_mod_fun(state, out_3038444346073204801); } -void gnss_f_fun(double *state, double dt, double *out_1079402724574081004) { - f_fun(state, dt, out_1079402724574081004); +void gnss_f_fun(double *state, double dt, double *out_5331942404729498203) { + f_fun(state, dt, out_5331942404729498203); } -void gnss_F_fun(double *state, double dt, double *out_3039166340276301210) { - F_fun(state, dt, out_3039166340276301210); +void gnss_F_fun(double *state, double dt, double *out_3160440324773884395) { + F_fun(state, dt, out_3160440324773884395); } -void gnss_h_6(double *state, double *sat_pos, double *out_3874365000426709074) { - h_6(state, sat_pos, out_3874365000426709074); +void gnss_h_6(double *state, double *sat_pos, double *out_4164777334676317660) { + h_6(state, sat_pos, out_4164777334676317660); } -void gnss_H_6(double *state, double *sat_pos, double *out_3986922819635287129) { - H_6(state, sat_pos, out_3986922819635287129); +void gnss_H_6(double *state, double *sat_pos, double *out_8721522254593615931) { + H_6(state, sat_pos, out_8721522254593615931); } -void gnss_h_20(double *state, double *sat_pos, double *out_5069168270486129229) { - h_20(state, sat_pos, out_5069168270486129229); +void gnss_h_20(double *state, double *sat_pos, double *out_1506605518158843733) { + h_20(state, sat_pos, out_1506605518158843733); } -void gnss_H_20(double *state, double *sat_pos, double *out_678918665408427203) { - H_20(state, sat_pos, out_678918665408427203); +void gnss_H_20(double *state, double *sat_pos, double *out_1146945854611411680) { + H_20(state, sat_pos, out_1146945854611411680); } -void gnss_h_7(double *state, double *sat_pos_vel, double *out_9105285764076347784) { - h_7(state, sat_pos_vel, out_9105285764076347784); +void gnss_h_7(double *state, double *sat_pos_vel, double *out_2838038345896688742) { + h_7(state, sat_pos_vel, out_2838038345896688742); } -void gnss_H_7(double *state, double *sat_pos_vel, double *out_7685540822919128148) { - H_7(state, sat_pos_vel, out_7685540822919128148); +void gnss_H_7(double *state, double *sat_pos_vel, double *out_7683900239883798223) { + H_7(state, sat_pos_vel, out_7683900239883798223); } -void gnss_h_21(double *state, double *sat_pos_vel, double *out_9105285764076347784) { - h_21(state, sat_pos_vel, out_9105285764076347784); +void gnss_h_21(double *state, double *sat_pos_vel, double *out_2838038345896688742) { + h_21(state, sat_pos_vel, out_2838038345896688742); } -void gnss_H_21(double *state, double *sat_pos_vel, double *out_7685540822919128148) { - H_21(state, sat_pos_vel, out_7685540822919128148); +void gnss_H_21(double *state, double *sat_pos_vel, double *out_7683900239883798223) { + H_21(state, sat_pos_vel, out_7683900239883798223); } void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/gnss.h b/selfdrive/locationd/models/generated/gnss.h index 12d353021..1fa1db58c 100644 --- a/selfdrive/locationd/models/generated/gnss.h +++ b/selfdrive/locationd/models/generated/gnss.h @@ -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_4620912923766298580); -void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9187715529705882522); -void gnss_H_mod_fun(double *state, double *out_922973610943044438); -void gnss_f_fun(double *state, double dt, double *out_1079402724574081004); -void gnss_F_fun(double *state, double dt, double *out_3039166340276301210); -void gnss_h_6(double *state, double *sat_pos, double *out_3874365000426709074); -void gnss_H_6(double *state, double *sat_pos, double *out_3986922819635287129); -void gnss_h_20(double *state, double *sat_pos, double *out_5069168270486129229); -void gnss_H_20(double *state, double *sat_pos, double *out_678918665408427203); -void gnss_h_7(double *state, double *sat_pos_vel, double *out_9105285764076347784); -void gnss_H_7(double *state, double *sat_pos_vel, double *out_7685540822919128148); -void gnss_h_21(double *state, double *sat_pos_vel, double *out_9105285764076347784); -void gnss_H_21(double *state, double *sat_pos_vel, double *out_7685540822919128148); +void gnss_err_fun(double *nom_x, double *delta_x, double *out_80014774094776572); +void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9144507820461143592); +void gnss_H_mod_fun(double *state, double *out_3038444346073204801); +void gnss_f_fun(double *state, double dt, double *out_5331942404729498203); +void gnss_F_fun(double *state, double dt, double *out_3160440324773884395); +void gnss_h_6(double *state, double *sat_pos, double *out_4164777334676317660); +void gnss_H_6(double *state, double *sat_pos, double *out_8721522254593615931); +void gnss_h_20(double *state, double *sat_pos, double *out_1506605518158843733); +void gnss_H_20(double *state, double *sat_pos, double *out_1146945854611411680); +void gnss_h_7(double *state, double *sat_pos_vel, double *out_2838038345896688742); +void gnss_H_7(double *state, double *sat_pos_vel, double *out_7683900239883798223); +void gnss_h_21(double *state, double *sat_pos_vel, double *out_2838038345896688742); +void gnss_H_21(double *state, double *sat_pos_vel, double *out_7683900239883798223); void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/libkf.so b/selfdrive/locationd/models/generated/libkf.so index a4ce65623..b724c05ca 100755 Binary files a/selfdrive/locationd/models/generated/libkf.so and b/selfdrive/locationd/models/generated/libkf.so differ diff --git a/selfdrive/locationd/models/generated/live.cpp b/selfdrive/locationd/models/generated/live.cpp index 89a42f778..910105ae1 100644 --- a/selfdrive/locationd/models/generated/live.cpp +++ b/selfdrive/locationd/models/generated/live.cpp @@ -22,1683 +22,1683 @@ const static double MAHA_THRESH_33 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void H(double *in_vec, double *out_919321204285207253) { - out_919321204285207253[0] = 0; - out_919321204285207253[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; - out_919321204285207253[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; - out_919321204285207253[3] = cos(in_vec[1])*cos(in_vec[2]); - out_919321204285207253[4] = sin(in_vec[2])*cos(in_vec[1]); - out_919321204285207253[5] = -sin(in_vec[1]); - out_919321204285207253[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_919321204285207253[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_919321204285207253[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; - out_919321204285207253[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); - out_919321204285207253[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); - out_919321204285207253[11] = sin(in_vec[0])*cos(in_vec[1]); - out_919321204285207253[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; - out_919321204285207253[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; - out_919321204285207253[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; - out_919321204285207253[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); - out_919321204285207253[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); - out_919321204285207253[17] = cos(in_vec[0])*cos(in_vec[1]); +void H(double *in_vec, double *out_6690968010248080223) { + out_6690968010248080223[0] = 0; + out_6690968010248080223[1] = -sin(in_vec[1])*sin(in_vec[2])*in_vec[4] - sin(in_vec[1])*cos(in_vec[2])*in_vec[3] - cos(in_vec[1])*in_vec[5]; + out_6690968010248080223[2] = -sin(in_vec[2])*cos(in_vec[1])*in_vec[3] + cos(in_vec[1])*cos(in_vec[2])*in_vec[4]; + out_6690968010248080223[3] = cos(in_vec[1])*cos(in_vec[2]); + out_6690968010248080223[4] = sin(in_vec[2])*cos(in_vec[1]); + out_6690968010248080223[5] = -sin(in_vec[1]); + out_6690968010248080223[6] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (-sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*in_vec[5]; + out_6690968010248080223[7] = -sin(in_vec[0])*sin(in_vec[1])*in_vec[5] + sin(in_vec[0])*sin(in_vec[2])*cos(in_vec[1])*in_vec[4] + sin(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; + out_6690968010248080223[8] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[3] + (sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]))*in_vec[4]; + out_6690968010248080223[9] = sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) - sin(in_vec[2])*cos(in_vec[0]); + out_6690968010248080223[10] = sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) + cos(in_vec[0])*cos(in_vec[2]); + out_6690968010248080223[11] = sin(in_vec[0])*cos(in_vec[1]); + out_6690968010248080223[12] = (-sin(in_vec[0])*sin(in_vec[1])*sin(in_vec[2]) - cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (-sin(in_vec[0])*sin(in_vec[1])*cos(in_vec[2]) + sin(in_vec[2])*cos(in_vec[0]))*in_vec[3] - sin(in_vec[0])*cos(in_vec[1])*in_vec[5]; + out_6690968010248080223[13] = -sin(in_vec[1])*cos(in_vec[0])*in_vec[5] + sin(in_vec[2])*cos(in_vec[0])*cos(in_vec[1])*in_vec[4] + cos(in_vec[0])*cos(in_vec[1])*cos(in_vec[2])*in_vec[3]; + out_6690968010248080223[14] = (sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]))*in_vec[4] + (sin(in_vec[0])*cos(in_vec[2]) - sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]))*in_vec[3]; + out_6690968010248080223[15] = sin(in_vec[0])*sin(in_vec[2]) + sin(in_vec[1])*cos(in_vec[0])*cos(in_vec[2]); + out_6690968010248080223[16] = -sin(in_vec[0])*cos(in_vec[2]) + sin(in_vec[1])*sin(in_vec[2])*cos(in_vec[0]); + out_6690968010248080223[17] = cos(in_vec[0])*cos(in_vec[1]); } -void err_fun(double *nom_x, double *delta_x, double *out_1239592434612673434) { - out_1239592434612673434[0] = delta_x[0] + nom_x[0]; - out_1239592434612673434[1] = delta_x[1] + nom_x[1]; - out_1239592434612673434[2] = delta_x[2] + nom_x[2]; - out_1239592434612673434[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; - out_1239592434612673434[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; - out_1239592434612673434[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; - out_1239592434612673434[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; - out_1239592434612673434[7] = delta_x[6] + nom_x[7]; - out_1239592434612673434[8] = delta_x[7] + nom_x[8]; - out_1239592434612673434[9] = delta_x[8] + nom_x[9]; - out_1239592434612673434[10] = delta_x[9] + nom_x[10]; - out_1239592434612673434[11] = delta_x[10] + nom_x[11]; - out_1239592434612673434[12] = delta_x[11] + nom_x[12]; - out_1239592434612673434[13] = delta_x[12] + nom_x[13]; - out_1239592434612673434[14] = delta_x[13] + nom_x[14]; - out_1239592434612673434[15] = delta_x[14] + nom_x[15]; - out_1239592434612673434[16] = delta_x[15] + nom_x[16]; - out_1239592434612673434[17] = delta_x[16] + nom_x[17]; - out_1239592434612673434[18] = delta_x[17] + nom_x[18]; - out_1239592434612673434[19] = delta_x[18] + nom_x[19]; - out_1239592434612673434[20] = delta_x[19] + nom_x[20]; - out_1239592434612673434[21] = delta_x[20] + nom_x[21]; +void err_fun(double *nom_x, double *delta_x, double *out_5128644486154966433) { + out_5128644486154966433[0] = delta_x[0] + nom_x[0]; + out_5128644486154966433[1] = delta_x[1] + nom_x[1]; + out_5128644486154966433[2] = delta_x[2] + nom_x[2]; + out_5128644486154966433[3] = -0.5*delta_x[3]*nom_x[4] - 0.5*delta_x[4]*nom_x[5] - 0.5*delta_x[5]*nom_x[6] + 1.0*nom_x[3]; + out_5128644486154966433[4] = 0.5*delta_x[3]*nom_x[3] + 0.5*delta_x[4]*nom_x[6] - 0.5*delta_x[5]*nom_x[5] + 1.0*nom_x[4]; + out_5128644486154966433[5] = -0.5*delta_x[3]*nom_x[6] + 0.5*delta_x[4]*nom_x[3] + 0.5*delta_x[5]*nom_x[4] + 1.0*nom_x[5]; + out_5128644486154966433[6] = 0.5*delta_x[3]*nom_x[5] - 0.5*delta_x[4]*nom_x[4] + 0.5*delta_x[5]*nom_x[3] + 1.0*nom_x[6]; + out_5128644486154966433[7] = delta_x[6] + nom_x[7]; + out_5128644486154966433[8] = delta_x[7] + nom_x[8]; + out_5128644486154966433[9] = delta_x[8] + nom_x[9]; + out_5128644486154966433[10] = delta_x[9] + nom_x[10]; + out_5128644486154966433[11] = delta_x[10] + nom_x[11]; + out_5128644486154966433[12] = delta_x[11] + nom_x[12]; + out_5128644486154966433[13] = delta_x[12] + nom_x[13]; + out_5128644486154966433[14] = delta_x[13] + nom_x[14]; + out_5128644486154966433[15] = delta_x[14] + nom_x[15]; + out_5128644486154966433[16] = delta_x[15] + nom_x[16]; + out_5128644486154966433[17] = delta_x[16] + nom_x[17]; + out_5128644486154966433[18] = delta_x[17] + nom_x[18]; + out_5128644486154966433[19] = delta_x[18] + nom_x[19]; + out_5128644486154966433[20] = delta_x[19] + nom_x[20]; + out_5128644486154966433[21] = delta_x[20] + nom_x[21]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8424707651892025503) { - out_8424707651892025503[0] = -nom_x[0] + true_x[0]; - out_8424707651892025503[1] = -nom_x[1] + true_x[1]; - out_8424707651892025503[2] = -nom_x[2] + true_x[2]; - out_8424707651892025503[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; - out_8424707651892025503[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; - out_8424707651892025503[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; - out_8424707651892025503[6] = -nom_x[7] + true_x[7]; - out_8424707651892025503[7] = -nom_x[8] + true_x[8]; - out_8424707651892025503[8] = -nom_x[9] + true_x[9]; - out_8424707651892025503[9] = -nom_x[10] + true_x[10]; - out_8424707651892025503[10] = -nom_x[11] + true_x[11]; - out_8424707651892025503[11] = -nom_x[12] + true_x[12]; - out_8424707651892025503[12] = -nom_x[13] + true_x[13]; - out_8424707651892025503[13] = -nom_x[14] + true_x[14]; - out_8424707651892025503[14] = -nom_x[15] + true_x[15]; - out_8424707651892025503[15] = -nom_x[16] + true_x[16]; - out_8424707651892025503[16] = -nom_x[17] + true_x[17]; - out_8424707651892025503[17] = -nom_x[18] + true_x[18]; - out_8424707651892025503[18] = -nom_x[19] + true_x[19]; - out_8424707651892025503[19] = -nom_x[20] + true_x[20]; - out_8424707651892025503[20] = -nom_x[21] + true_x[21]; +void inv_err_fun(double *nom_x, double *true_x, double *out_7600355658971333284) { + out_7600355658971333284[0] = -nom_x[0] + true_x[0]; + out_7600355658971333284[1] = -nom_x[1] + true_x[1]; + out_7600355658971333284[2] = -nom_x[2] + true_x[2]; + out_7600355658971333284[3] = 2*nom_x[3]*true_x[4] - 2*nom_x[4]*true_x[3] + 2*nom_x[5]*true_x[6] - 2*nom_x[6]*true_x[5]; + out_7600355658971333284[4] = 2*nom_x[3]*true_x[5] - 2*nom_x[4]*true_x[6] - 2*nom_x[5]*true_x[3] + 2*nom_x[6]*true_x[4]; + out_7600355658971333284[5] = 2*nom_x[3]*true_x[6] + 2*nom_x[4]*true_x[5] - 2*nom_x[5]*true_x[4] - 2*nom_x[6]*true_x[3]; + out_7600355658971333284[6] = -nom_x[7] + true_x[7]; + out_7600355658971333284[7] = -nom_x[8] + true_x[8]; + out_7600355658971333284[8] = -nom_x[9] + true_x[9]; + out_7600355658971333284[9] = -nom_x[10] + true_x[10]; + out_7600355658971333284[10] = -nom_x[11] + true_x[11]; + out_7600355658971333284[11] = -nom_x[12] + true_x[12]; + out_7600355658971333284[12] = -nom_x[13] + true_x[13]; + out_7600355658971333284[13] = -nom_x[14] + true_x[14]; + out_7600355658971333284[14] = -nom_x[15] + true_x[15]; + out_7600355658971333284[15] = -nom_x[16] + true_x[16]; + out_7600355658971333284[16] = -nom_x[17] + true_x[17]; + out_7600355658971333284[17] = -nom_x[18] + true_x[18]; + out_7600355658971333284[18] = -nom_x[19] + true_x[19]; + out_7600355658971333284[19] = -nom_x[20] + true_x[20]; + out_7600355658971333284[20] = -nom_x[21] + true_x[21]; } -void H_mod_fun(double *state, double *out_3398846321270000840) { - out_3398846321270000840[0] = 1.0; - out_3398846321270000840[1] = 0; - out_3398846321270000840[2] = 0; - out_3398846321270000840[3] = 0; - out_3398846321270000840[4] = 0; - out_3398846321270000840[5] = 0; - out_3398846321270000840[6] = 0; - out_3398846321270000840[7] = 0; - out_3398846321270000840[8] = 0; - out_3398846321270000840[9] = 0; - out_3398846321270000840[10] = 0; - out_3398846321270000840[11] = 0; - out_3398846321270000840[12] = 0; - out_3398846321270000840[13] = 0; - out_3398846321270000840[14] = 0; - out_3398846321270000840[15] = 0; - out_3398846321270000840[16] = 0; - out_3398846321270000840[17] = 0; - out_3398846321270000840[18] = 0; - out_3398846321270000840[19] = 0; - out_3398846321270000840[20] = 0; - out_3398846321270000840[21] = 0; - out_3398846321270000840[22] = 1.0; - out_3398846321270000840[23] = 0; - out_3398846321270000840[24] = 0; - out_3398846321270000840[25] = 0; - out_3398846321270000840[26] = 0; - out_3398846321270000840[27] = 0; - out_3398846321270000840[28] = 0; - out_3398846321270000840[29] = 0; - out_3398846321270000840[30] = 0; - out_3398846321270000840[31] = 0; - out_3398846321270000840[32] = 0; - out_3398846321270000840[33] = 0; - out_3398846321270000840[34] = 0; - out_3398846321270000840[35] = 0; - out_3398846321270000840[36] = 0; - out_3398846321270000840[37] = 0; - out_3398846321270000840[38] = 0; - out_3398846321270000840[39] = 0; - out_3398846321270000840[40] = 0; - out_3398846321270000840[41] = 0; - out_3398846321270000840[42] = 0; - out_3398846321270000840[43] = 0; - out_3398846321270000840[44] = 1.0; - out_3398846321270000840[45] = 0; - out_3398846321270000840[46] = 0; - out_3398846321270000840[47] = 0; - out_3398846321270000840[48] = 0; - out_3398846321270000840[49] = 0; - out_3398846321270000840[50] = 0; - out_3398846321270000840[51] = 0; - out_3398846321270000840[52] = 0; - out_3398846321270000840[53] = 0; - out_3398846321270000840[54] = 0; - out_3398846321270000840[55] = 0; - out_3398846321270000840[56] = 0; - out_3398846321270000840[57] = 0; - out_3398846321270000840[58] = 0; - out_3398846321270000840[59] = 0; - out_3398846321270000840[60] = 0; - out_3398846321270000840[61] = 0; - out_3398846321270000840[62] = 0; - out_3398846321270000840[63] = 0; - out_3398846321270000840[64] = 0; - out_3398846321270000840[65] = 0; - out_3398846321270000840[66] = -0.5*state[4]; - out_3398846321270000840[67] = -0.5*state[5]; - out_3398846321270000840[68] = -0.5*state[6]; - out_3398846321270000840[69] = 0; - out_3398846321270000840[70] = 0; - out_3398846321270000840[71] = 0; - out_3398846321270000840[72] = 0; - out_3398846321270000840[73] = 0; - out_3398846321270000840[74] = 0; - out_3398846321270000840[75] = 0; - out_3398846321270000840[76] = 0; - out_3398846321270000840[77] = 0; - out_3398846321270000840[78] = 0; - out_3398846321270000840[79] = 0; - out_3398846321270000840[80] = 0; - out_3398846321270000840[81] = 0; - out_3398846321270000840[82] = 0; - out_3398846321270000840[83] = 0; - out_3398846321270000840[84] = 0; - out_3398846321270000840[85] = 0; - out_3398846321270000840[86] = 0; - out_3398846321270000840[87] = 0.5*state[3]; - out_3398846321270000840[88] = 0.5*state[6]; - out_3398846321270000840[89] = -0.5*state[5]; - out_3398846321270000840[90] = 0; - out_3398846321270000840[91] = 0; - out_3398846321270000840[92] = 0; - out_3398846321270000840[93] = 0; - out_3398846321270000840[94] = 0; - out_3398846321270000840[95] = 0; - out_3398846321270000840[96] = 0; - out_3398846321270000840[97] = 0; - out_3398846321270000840[98] = 0; - out_3398846321270000840[99] = 0; - out_3398846321270000840[100] = 0; - out_3398846321270000840[101] = 0; - out_3398846321270000840[102] = 0; - out_3398846321270000840[103] = 0; - out_3398846321270000840[104] = 0; - out_3398846321270000840[105] = 0; - out_3398846321270000840[106] = 0; - out_3398846321270000840[107] = 0; - out_3398846321270000840[108] = -0.5*state[6]; - out_3398846321270000840[109] = 0.5*state[3]; - out_3398846321270000840[110] = 0.5*state[4]; - out_3398846321270000840[111] = 0; - out_3398846321270000840[112] = 0; - out_3398846321270000840[113] = 0; - out_3398846321270000840[114] = 0; - out_3398846321270000840[115] = 0; - out_3398846321270000840[116] = 0; - out_3398846321270000840[117] = 0; - out_3398846321270000840[118] = 0; - out_3398846321270000840[119] = 0; - out_3398846321270000840[120] = 0; - out_3398846321270000840[121] = 0; - out_3398846321270000840[122] = 0; - out_3398846321270000840[123] = 0; - out_3398846321270000840[124] = 0; - out_3398846321270000840[125] = 0; - out_3398846321270000840[126] = 0; - out_3398846321270000840[127] = 0; - out_3398846321270000840[128] = 0; - out_3398846321270000840[129] = 0.5*state[5]; - out_3398846321270000840[130] = -0.5*state[4]; - out_3398846321270000840[131] = 0.5*state[3]; - out_3398846321270000840[132] = 0; - out_3398846321270000840[133] = 0; - out_3398846321270000840[134] = 0; - out_3398846321270000840[135] = 0; - out_3398846321270000840[136] = 0; - out_3398846321270000840[137] = 0; - out_3398846321270000840[138] = 0; - out_3398846321270000840[139] = 0; - out_3398846321270000840[140] = 0; - out_3398846321270000840[141] = 0; - out_3398846321270000840[142] = 0; - out_3398846321270000840[143] = 0; - out_3398846321270000840[144] = 0; - out_3398846321270000840[145] = 0; - out_3398846321270000840[146] = 0; - out_3398846321270000840[147] = 0; - out_3398846321270000840[148] = 0; - out_3398846321270000840[149] = 0; - out_3398846321270000840[150] = 0; - out_3398846321270000840[151] = 0; - out_3398846321270000840[152] = 0; - out_3398846321270000840[153] = 1.0; - out_3398846321270000840[154] = 0; - out_3398846321270000840[155] = 0; - out_3398846321270000840[156] = 0; - out_3398846321270000840[157] = 0; - out_3398846321270000840[158] = 0; - out_3398846321270000840[159] = 0; - out_3398846321270000840[160] = 0; - out_3398846321270000840[161] = 0; - out_3398846321270000840[162] = 0; - out_3398846321270000840[163] = 0; - out_3398846321270000840[164] = 0; - out_3398846321270000840[165] = 0; - out_3398846321270000840[166] = 0; - out_3398846321270000840[167] = 0; - out_3398846321270000840[168] = 0; - out_3398846321270000840[169] = 0; - out_3398846321270000840[170] = 0; - out_3398846321270000840[171] = 0; - out_3398846321270000840[172] = 0; - out_3398846321270000840[173] = 0; - out_3398846321270000840[174] = 0; - out_3398846321270000840[175] = 1.0; - out_3398846321270000840[176] = 0; - out_3398846321270000840[177] = 0; - out_3398846321270000840[178] = 0; - out_3398846321270000840[179] = 0; - out_3398846321270000840[180] = 0; - out_3398846321270000840[181] = 0; - out_3398846321270000840[182] = 0; - out_3398846321270000840[183] = 0; - out_3398846321270000840[184] = 0; - out_3398846321270000840[185] = 0; - out_3398846321270000840[186] = 0; - out_3398846321270000840[187] = 0; - out_3398846321270000840[188] = 0; - out_3398846321270000840[189] = 0; - out_3398846321270000840[190] = 0; - out_3398846321270000840[191] = 0; - out_3398846321270000840[192] = 0; - out_3398846321270000840[193] = 0; - out_3398846321270000840[194] = 0; - out_3398846321270000840[195] = 0; - out_3398846321270000840[196] = 0; - out_3398846321270000840[197] = 1.0; - out_3398846321270000840[198] = 0; - out_3398846321270000840[199] = 0; - out_3398846321270000840[200] = 0; - out_3398846321270000840[201] = 0; - out_3398846321270000840[202] = 0; - out_3398846321270000840[203] = 0; - out_3398846321270000840[204] = 0; - out_3398846321270000840[205] = 0; - out_3398846321270000840[206] = 0; - out_3398846321270000840[207] = 0; - out_3398846321270000840[208] = 0; - out_3398846321270000840[209] = 0; - out_3398846321270000840[210] = 0; - out_3398846321270000840[211] = 0; - out_3398846321270000840[212] = 0; - out_3398846321270000840[213] = 0; - out_3398846321270000840[214] = 0; - out_3398846321270000840[215] = 0; - out_3398846321270000840[216] = 0; - out_3398846321270000840[217] = 0; - out_3398846321270000840[218] = 0; - out_3398846321270000840[219] = 1.0; - out_3398846321270000840[220] = 0; - out_3398846321270000840[221] = 0; - out_3398846321270000840[222] = 0; - out_3398846321270000840[223] = 0; - out_3398846321270000840[224] = 0; - out_3398846321270000840[225] = 0; - out_3398846321270000840[226] = 0; - out_3398846321270000840[227] = 0; - out_3398846321270000840[228] = 0; - out_3398846321270000840[229] = 0; - out_3398846321270000840[230] = 0; - out_3398846321270000840[231] = 0; - out_3398846321270000840[232] = 0; - out_3398846321270000840[233] = 0; - out_3398846321270000840[234] = 0; - out_3398846321270000840[235] = 0; - out_3398846321270000840[236] = 0; - out_3398846321270000840[237] = 0; - out_3398846321270000840[238] = 0; - out_3398846321270000840[239] = 0; - out_3398846321270000840[240] = 0; - out_3398846321270000840[241] = 1.0; - out_3398846321270000840[242] = 0; - out_3398846321270000840[243] = 0; - out_3398846321270000840[244] = 0; - out_3398846321270000840[245] = 0; - out_3398846321270000840[246] = 0; - out_3398846321270000840[247] = 0; - out_3398846321270000840[248] = 0; - out_3398846321270000840[249] = 0; - out_3398846321270000840[250] = 0; - out_3398846321270000840[251] = 0; - out_3398846321270000840[252] = 0; - out_3398846321270000840[253] = 0; - out_3398846321270000840[254] = 0; - out_3398846321270000840[255] = 0; - out_3398846321270000840[256] = 0; - out_3398846321270000840[257] = 0; - out_3398846321270000840[258] = 0; - out_3398846321270000840[259] = 0; - out_3398846321270000840[260] = 0; - out_3398846321270000840[261] = 0; - out_3398846321270000840[262] = 0; - out_3398846321270000840[263] = 1.0; - out_3398846321270000840[264] = 0; - out_3398846321270000840[265] = 0; - out_3398846321270000840[266] = 0; - out_3398846321270000840[267] = 0; - out_3398846321270000840[268] = 0; - out_3398846321270000840[269] = 0; - out_3398846321270000840[270] = 0; - out_3398846321270000840[271] = 0; - out_3398846321270000840[272] = 0; - out_3398846321270000840[273] = 0; - out_3398846321270000840[274] = 0; - out_3398846321270000840[275] = 0; - out_3398846321270000840[276] = 0; - out_3398846321270000840[277] = 0; - out_3398846321270000840[278] = 0; - out_3398846321270000840[279] = 0; - out_3398846321270000840[280] = 0; - out_3398846321270000840[281] = 0; - out_3398846321270000840[282] = 0; - out_3398846321270000840[283] = 0; - out_3398846321270000840[284] = 0; - out_3398846321270000840[285] = 1.0; - out_3398846321270000840[286] = 0; - out_3398846321270000840[287] = 0; - out_3398846321270000840[288] = 0; - out_3398846321270000840[289] = 0; - out_3398846321270000840[290] = 0; - out_3398846321270000840[291] = 0; - out_3398846321270000840[292] = 0; - out_3398846321270000840[293] = 0; - out_3398846321270000840[294] = 0; - out_3398846321270000840[295] = 0; - out_3398846321270000840[296] = 0; - out_3398846321270000840[297] = 0; - out_3398846321270000840[298] = 0; - out_3398846321270000840[299] = 0; - out_3398846321270000840[300] = 0; - out_3398846321270000840[301] = 0; - out_3398846321270000840[302] = 0; - out_3398846321270000840[303] = 0; - out_3398846321270000840[304] = 0; - out_3398846321270000840[305] = 0; - out_3398846321270000840[306] = 0; - out_3398846321270000840[307] = 1.0; - out_3398846321270000840[308] = 0; - out_3398846321270000840[309] = 0; - out_3398846321270000840[310] = 0; - out_3398846321270000840[311] = 0; - out_3398846321270000840[312] = 0; - out_3398846321270000840[313] = 0; - out_3398846321270000840[314] = 0; - out_3398846321270000840[315] = 0; - out_3398846321270000840[316] = 0; - out_3398846321270000840[317] = 0; - out_3398846321270000840[318] = 0; - out_3398846321270000840[319] = 0; - out_3398846321270000840[320] = 0; - out_3398846321270000840[321] = 0; - out_3398846321270000840[322] = 0; - out_3398846321270000840[323] = 0; - out_3398846321270000840[324] = 0; - out_3398846321270000840[325] = 0; - out_3398846321270000840[326] = 0; - out_3398846321270000840[327] = 0; - out_3398846321270000840[328] = 0; - out_3398846321270000840[329] = 1.0; - out_3398846321270000840[330] = 0; - out_3398846321270000840[331] = 0; - out_3398846321270000840[332] = 0; - out_3398846321270000840[333] = 0; - out_3398846321270000840[334] = 0; - out_3398846321270000840[335] = 0; - out_3398846321270000840[336] = 0; - out_3398846321270000840[337] = 0; - out_3398846321270000840[338] = 0; - out_3398846321270000840[339] = 0; - out_3398846321270000840[340] = 0; - out_3398846321270000840[341] = 0; - out_3398846321270000840[342] = 0; - out_3398846321270000840[343] = 0; - out_3398846321270000840[344] = 0; - out_3398846321270000840[345] = 0; - out_3398846321270000840[346] = 0; - out_3398846321270000840[347] = 0; - out_3398846321270000840[348] = 0; - out_3398846321270000840[349] = 0; - out_3398846321270000840[350] = 0; - out_3398846321270000840[351] = 1.0; - out_3398846321270000840[352] = 0; - out_3398846321270000840[353] = 0; - out_3398846321270000840[354] = 0; - out_3398846321270000840[355] = 0; - out_3398846321270000840[356] = 0; - out_3398846321270000840[357] = 0; - out_3398846321270000840[358] = 0; - out_3398846321270000840[359] = 0; - out_3398846321270000840[360] = 0; - out_3398846321270000840[361] = 0; - out_3398846321270000840[362] = 0; - out_3398846321270000840[363] = 0; - out_3398846321270000840[364] = 0; - out_3398846321270000840[365] = 0; - out_3398846321270000840[366] = 0; - out_3398846321270000840[367] = 0; - out_3398846321270000840[368] = 0; - out_3398846321270000840[369] = 0; - out_3398846321270000840[370] = 0; - out_3398846321270000840[371] = 0; - out_3398846321270000840[372] = 0; - out_3398846321270000840[373] = 1.0; - out_3398846321270000840[374] = 0; - out_3398846321270000840[375] = 0; - out_3398846321270000840[376] = 0; - out_3398846321270000840[377] = 0; - out_3398846321270000840[378] = 0; - out_3398846321270000840[379] = 0; - out_3398846321270000840[380] = 0; - out_3398846321270000840[381] = 0; - out_3398846321270000840[382] = 0; - out_3398846321270000840[383] = 0; - out_3398846321270000840[384] = 0; - out_3398846321270000840[385] = 0; - out_3398846321270000840[386] = 0; - out_3398846321270000840[387] = 0; - out_3398846321270000840[388] = 0; - out_3398846321270000840[389] = 0; - out_3398846321270000840[390] = 0; - out_3398846321270000840[391] = 0; - out_3398846321270000840[392] = 0; - out_3398846321270000840[393] = 0; - out_3398846321270000840[394] = 0; - out_3398846321270000840[395] = 1.0; - out_3398846321270000840[396] = 0; - out_3398846321270000840[397] = 0; - out_3398846321270000840[398] = 0; - out_3398846321270000840[399] = 0; - out_3398846321270000840[400] = 0; - out_3398846321270000840[401] = 0; - out_3398846321270000840[402] = 0; - out_3398846321270000840[403] = 0; - out_3398846321270000840[404] = 0; - out_3398846321270000840[405] = 0; - out_3398846321270000840[406] = 0; - out_3398846321270000840[407] = 0; - out_3398846321270000840[408] = 0; - out_3398846321270000840[409] = 0; - out_3398846321270000840[410] = 0; - out_3398846321270000840[411] = 0; - out_3398846321270000840[412] = 0; - out_3398846321270000840[413] = 0; - out_3398846321270000840[414] = 0; - out_3398846321270000840[415] = 0; - out_3398846321270000840[416] = 0; - out_3398846321270000840[417] = 1.0; - out_3398846321270000840[418] = 0; - out_3398846321270000840[419] = 0; - out_3398846321270000840[420] = 0; - out_3398846321270000840[421] = 0; - out_3398846321270000840[422] = 0; - out_3398846321270000840[423] = 0; - out_3398846321270000840[424] = 0; - out_3398846321270000840[425] = 0; - out_3398846321270000840[426] = 0; - out_3398846321270000840[427] = 0; - out_3398846321270000840[428] = 0; - out_3398846321270000840[429] = 0; - out_3398846321270000840[430] = 0; - out_3398846321270000840[431] = 0; - out_3398846321270000840[432] = 0; - out_3398846321270000840[433] = 0; - out_3398846321270000840[434] = 0; - out_3398846321270000840[435] = 0; - out_3398846321270000840[436] = 0; - out_3398846321270000840[437] = 0; - out_3398846321270000840[438] = 0; - out_3398846321270000840[439] = 1.0; - out_3398846321270000840[440] = 0; - out_3398846321270000840[441] = 0; - out_3398846321270000840[442] = 0; - out_3398846321270000840[443] = 0; - out_3398846321270000840[444] = 0; - out_3398846321270000840[445] = 0; - out_3398846321270000840[446] = 0; - out_3398846321270000840[447] = 0; - out_3398846321270000840[448] = 0; - out_3398846321270000840[449] = 0; - out_3398846321270000840[450] = 0; - out_3398846321270000840[451] = 0; - out_3398846321270000840[452] = 0; - out_3398846321270000840[453] = 0; - out_3398846321270000840[454] = 0; - out_3398846321270000840[455] = 0; - out_3398846321270000840[456] = 0; - out_3398846321270000840[457] = 0; - out_3398846321270000840[458] = 0; - out_3398846321270000840[459] = 0; - out_3398846321270000840[460] = 0; - out_3398846321270000840[461] = 1.0; +void H_mod_fun(double *state, double *out_1685726956674156760) { + out_1685726956674156760[0] = 1.0; + out_1685726956674156760[1] = 0; + out_1685726956674156760[2] = 0; + out_1685726956674156760[3] = 0; + out_1685726956674156760[4] = 0; + out_1685726956674156760[5] = 0; + out_1685726956674156760[6] = 0; + out_1685726956674156760[7] = 0; + out_1685726956674156760[8] = 0; + out_1685726956674156760[9] = 0; + out_1685726956674156760[10] = 0; + out_1685726956674156760[11] = 0; + out_1685726956674156760[12] = 0; + out_1685726956674156760[13] = 0; + out_1685726956674156760[14] = 0; + out_1685726956674156760[15] = 0; + out_1685726956674156760[16] = 0; + out_1685726956674156760[17] = 0; + out_1685726956674156760[18] = 0; + out_1685726956674156760[19] = 0; + out_1685726956674156760[20] = 0; + out_1685726956674156760[21] = 0; + out_1685726956674156760[22] = 1.0; + out_1685726956674156760[23] = 0; + out_1685726956674156760[24] = 0; + out_1685726956674156760[25] = 0; + out_1685726956674156760[26] = 0; + out_1685726956674156760[27] = 0; + out_1685726956674156760[28] = 0; + out_1685726956674156760[29] = 0; + out_1685726956674156760[30] = 0; + out_1685726956674156760[31] = 0; + out_1685726956674156760[32] = 0; + out_1685726956674156760[33] = 0; + out_1685726956674156760[34] = 0; + out_1685726956674156760[35] = 0; + out_1685726956674156760[36] = 0; + out_1685726956674156760[37] = 0; + out_1685726956674156760[38] = 0; + out_1685726956674156760[39] = 0; + out_1685726956674156760[40] = 0; + out_1685726956674156760[41] = 0; + out_1685726956674156760[42] = 0; + out_1685726956674156760[43] = 0; + out_1685726956674156760[44] = 1.0; + out_1685726956674156760[45] = 0; + out_1685726956674156760[46] = 0; + out_1685726956674156760[47] = 0; + out_1685726956674156760[48] = 0; + out_1685726956674156760[49] = 0; + out_1685726956674156760[50] = 0; + out_1685726956674156760[51] = 0; + out_1685726956674156760[52] = 0; + out_1685726956674156760[53] = 0; + out_1685726956674156760[54] = 0; + out_1685726956674156760[55] = 0; + out_1685726956674156760[56] = 0; + out_1685726956674156760[57] = 0; + out_1685726956674156760[58] = 0; + out_1685726956674156760[59] = 0; + out_1685726956674156760[60] = 0; + out_1685726956674156760[61] = 0; + out_1685726956674156760[62] = 0; + out_1685726956674156760[63] = 0; + out_1685726956674156760[64] = 0; + out_1685726956674156760[65] = 0; + out_1685726956674156760[66] = -0.5*state[4]; + out_1685726956674156760[67] = -0.5*state[5]; + out_1685726956674156760[68] = -0.5*state[6]; + out_1685726956674156760[69] = 0; + out_1685726956674156760[70] = 0; + out_1685726956674156760[71] = 0; + out_1685726956674156760[72] = 0; + out_1685726956674156760[73] = 0; + out_1685726956674156760[74] = 0; + out_1685726956674156760[75] = 0; + out_1685726956674156760[76] = 0; + out_1685726956674156760[77] = 0; + out_1685726956674156760[78] = 0; + out_1685726956674156760[79] = 0; + out_1685726956674156760[80] = 0; + out_1685726956674156760[81] = 0; + out_1685726956674156760[82] = 0; + out_1685726956674156760[83] = 0; + out_1685726956674156760[84] = 0; + out_1685726956674156760[85] = 0; + out_1685726956674156760[86] = 0; + out_1685726956674156760[87] = 0.5*state[3]; + out_1685726956674156760[88] = 0.5*state[6]; + out_1685726956674156760[89] = -0.5*state[5]; + out_1685726956674156760[90] = 0; + out_1685726956674156760[91] = 0; + out_1685726956674156760[92] = 0; + out_1685726956674156760[93] = 0; + out_1685726956674156760[94] = 0; + out_1685726956674156760[95] = 0; + out_1685726956674156760[96] = 0; + out_1685726956674156760[97] = 0; + out_1685726956674156760[98] = 0; + out_1685726956674156760[99] = 0; + out_1685726956674156760[100] = 0; + out_1685726956674156760[101] = 0; + out_1685726956674156760[102] = 0; + out_1685726956674156760[103] = 0; + out_1685726956674156760[104] = 0; + out_1685726956674156760[105] = 0; + out_1685726956674156760[106] = 0; + out_1685726956674156760[107] = 0; + out_1685726956674156760[108] = -0.5*state[6]; + out_1685726956674156760[109] = 0.5*state[3]; + out_1685726956674156760[110] = 0.5*state[4]; + out_1685726956674156760[111] = 0; + out_1685726956674156760[112] = 0; + out_1685726956674156760[113] = 0; + out_1685726956674156760[114] = 0; + out_1685726956674156760[115] = 0; + out_1685726956674156760[116] = 0; + out_1685726956674156760[117] = 0; + out_1685726956674156760[118] = 0; + out_1685726956674156760[119] = 0; + out_1685726956674156760[120] = 0; + out_1685726956674156760[121] = 0; + out_1685726956674156760[122] = 0; + out_1685726956674156760[123] = 0; + out_1685726956674156760[124] = 0; + out_1685726956674156760[125] = 0; + out_1685726956674156760[126] = 0; + out_1685726956674156760[127] = 0; + out_1685726956674156760[128] = 0; + out_1685726956674156760[129] = 0.5*state[5]; + out_1685726956674156760[130] = -0.5*state[4]; + out_1685726956674156760[131] = 0.5*state[3]; + out_1685726956674156760[132] = 0; + out_1685726956674156760[133] = 0; + out_1685726956674156760[134] = 0; + out_1685726956674156760[135] = 0; + out_1685726956674156760[136] = 0; + out_1685726956674156760[137] = 0; + out_1685726956674156760[138] = 0; + out_1685726956674156760[139] = 0; + out_1685726956674156760[140] = 0; + out_1685726956674156760[141] = 0; + out_1685726956674156760[142] = 0; + out_1685726956674156760[143] = 0; + out_1685726956674156760[144] = 0; + out_1685726956674156760[145] = 0; + out_1685726956674156760[146] = 0; + out_1685726956674156760[147] = 0; + out_1685726956674156760[148] = 0; + out_1685726956674156760[149] = 0; + out_1685726956674156760[150] = 0; + out_1685726956674156760[151] = 0; + out_1685726956674156760[152] = 0; + out_1685726956674156760[153] = 1.0; + out_1685726956674156760[154] = 0; + out_1685726956674156760[155] = 0; + out_1685726956674156760[156] = 0; + out_1685726956674156760[157] = 0; + out_1685726956674156760[158] = 0; + out_1685726956674156760[159] = 0; + out_1685726956674156760[160] = 0; + out_1685726956674156760[161] = 0; + out_1685726956674156760[162] = 0; + out_1685726956674156760[163] = 0; + out_1685726956674156760[164] = 0; + out_1685726956674156760[165] = 0; + out_1685726956674156760[166] = 0; + out_1685726956674156760[167] = 0; + out_1685726956674156760[168] = 0; + out_1685726956674156760[169] = 0; + out_1685726956674156760[170] = 0; + out_1685726956674156760[171] = 0; + out_1685726956674156760[172] = 0; + out_1685726956674156760[173] = 0; + out_1685726956674156760[174] = 0; + out_1685726956674156760[175] = 1.0; + out_1685726956674156760[176] = 0; + out_1685726956674156760[177] = 0; + out_1685726956674156760[178] = 0; + out_1685726956674156760[179] = 0; + out_1685726956674156760[180] = 0; + out_1685726956674156760[181] = 0; + out_1685726956674156760[182] = 0; + out_1685726956674156760[183] = 0; + out_1685726956674156760[184] = 0; + out_1685726956674156760[185] = 0; + out_1685726956674156760[186] = 0; + out_1685726956674156760[187] = 0; + out_1685726956674156760[188] = 0; + out_1685726956674156760[189] = 0; + out_1685726956674156760[190] = 0; + out_1685726956674156760[191] = 0; + out_1685726956674156760[192] = 0; + out_1685726956674156760[193] = 0; + out_1685726956674156760[194] = 0; + out_1685726956674156760[195] = 0; + out_1685726956674156760[196] = 0; + out_1685726956674156760[197] = 1.0; + out_1685726956674156760[198] = 0; + out_1685726956674156760[199] = 0; + out_1685726956674156760[200] = 0; + out_1685726956674156760[201] = 0; + out_1685726956674156760[202] = 0; + out_1685726956674156760[203] = 0; + out_1685726956674156760[204] = 0; + out_1685726956674156760[205] = 0; + out_1685726956674156760[206] = 0; + out_1685726956674156760[207] = 0; + out_1685726956674156760[208] = 0; + out_1685726956674156760[209] = 0; + out_1685726956674156760[210] = 0; + out_1685726956674156760[211] = 0; + out_1685726956674156760[212] = 0; + out_1685726956674156760[213] = 0; + out_1685726956674156760[214] = 0; + out_1685726956674156760[215] = 0; + out_1685726956674156760[216] = 0; + out_1685726956674156760[217] = 0; + out_1685726956674156760[218] = 0; + out_1685726956674156760[219] = 1.0; + out_1685726956674156760[220] = 0; + out_1685726956674156760[221] = 0; + out_1685726956674156760[222] = 0; + out_1685726956674156760[223] = 0; + out_1685726956674156760[224] = 0; + out_1685726956674156760[225] = 0; + out_1685726956674156760[226] = 0; + out_1685726956674156760[227] = 0; + out_1685726956674156760[228] = 0; + out_1685726956674156760[229] = 0; + out_1685726956674156760[230] = 0; + out_1685726956674156760[231] = 0; + out_1685726956674156760[232] = 0; + out_1685726956674156760[233] = 0; + out_1685726956674156760[234] = 0; + out_1685726956674156760[235] = 0; + out_1685726956674156760[236] = 0; + out_1685726956674156760[237] = 0; + out_1685726956674156760[238] = 0; + out_1685726956674156760[239] = 0; + out_1685726956674156760[240] = 0; + out_1685726956674156760[241] = 1.0; + out_1685726956674156760[242] = 0; + out_1685726956674156760[243] = 0; + out_1685726956674156760[244] = 0; + out_1685726956674156760[245] = 0; + out_1685726956674156760[246] = 0; + out_1685726956674156760[247] = 0; + out_1685726956674156760[248] = 0; + out_1685726956674156760[249] = 0; + out_1685726956674156760[250] = 0; + out_1685726956674156760[251] = 0; + out_1685726956674156760[252] = 0; + out_1685726956674156760[253] = 0; + out_1685726956674156760[254] = 0; + out_1685726956674156760[255] = 0; + out_1685726956674156760[256] = 0; + out_1685726956674156760[257] = 0; + out_1685726956674156760[258] = 0; + out_1685726956674156760[259] = 0; + out_1685726956674156760[260] = 0; + out_1685726956674156760[261] = 0; + out_1685726956674156760[262] = 0; + out_1685726956674156760[263] = 1.0; + out_1685726956674156760[264] = 0; + out_1685726956674156760[265] = 0; + out_1685726956674156760[266] = 0; + out_1685726956674156760[267] = 0; + out_1685726956674156760[268] = 0; + out_1685726956674156760[269] = 0; + out_1685726956674156760[270] = 0; + out_1685726956674156760[271] = 0; + out_1685726956674156760[272] = 0; + out_1685726956674156760[273] = 0; + out_1685726956674156760[274] = 0; + out_1685726956674156760[275] = 0; + out_1685726956674156760[276] = 0; + out_1685726956674156760[277] = 0; + out_1685726956674156760[278] = 0; + out_1685726956674156760[279] = 0; + out_1685726956674156760[280] = 0; + out_1685726956674156760[281] = 0; + out_1685726956674156760[282] = 0; + out_1685726956674156760[283] = 0; + out_1685726956674156760[284] = 0; + out_1685726956674156760[285] = 1.0; + out_1685726956674156760[286] = 0; + out_1685726956674156760[287] = 0; + out_1685726956674156760[288] = 0; + out_1685726956674156760[289] = 0; + out_1685726956674156760[290] = 0; + out_1685726956674156760[291] = 0; + out_1685726956674156760[292] = 0; + out_1685726956674156760[293] = 0; + out_1685726956674156760[294] = 0; + out_1685726956674156760[295] = 0; + out_1685726956674156760[296] = 0; + out_1685726956674156760[297] = 0; + out_1685726956674156760[298] = 0; + out_1685726956674156760[299] = 0; + out_1685726956674156760[300] = 0; + out_1685726956674156760[301] = 0; + out_1685726956674156760[302] = 0; + out_1685726956674156760[303] = 0; + out_1685726956674156760[304] = 0; + out_1685726956674156760[305] = 0; + out_1685726956674156760[306] = 0; + out_1685726956674156760[307] = 1.0; + out_1685726956674156760[308] = 0; + out_1685726956674156760[309] = 0; + out_1685726956674156760[310] = 0; + out_1685726956674156760[311] = 0; + out_1685726956674156760[312] = 0; + out_1685726956674156760[313] = 0; + out_1685726956674156760[314] = 0; + out_1685726956674156760[315] = 0; + out_1685726956674156760[316] = 0; + out_1685726956674156760[317] = 0; + out_1685726956674156760[318] = 0; + out_1685726956674156760[319] = 0; + out_1685726956674156760[320] = 0; + out_1685726956674156760[321] = 0; + out_1685726956674156760[322] = 0; + out_1685726956674156760[323] = 0; + out_1685726956674156760[324] = 0; + out_1685726956674156760[325] = 0; + out_1685726956674156760[326] = 0; + out_1685726956674156760[327] = 0; + out_1685726956674156760[328] = 0; + out_1685726956674156760[329] = 1.0; + out_1685726956674156760[330] = 0; + out_1685726956674156760[331] = 0; + out_1685726956674156760[332] = 0; + out_1685726956674156760[333] = 0; + out_1685726956674156760[334] = 0; + out_1685726956674156760[335] = 0; + out_1685726956674156760[336] = 0; + out_1685726956674156760[337] = 0; + out_1685726956674156760[338] = 0; + out_1685726956674156760[339] = 0; + out_1685726956674156760[340] = 0; + out_1685726956674156760[341] = 0; + out_1685726956674156760[342] = 0; + out_1685726956674156760[343] = 0; + out_1685726956674156760[344] = 0; + out_1685726956674156760[345] = 0; + out_1685726956674156760[346] = 0; + out_1685726956674156760[347] = 0; + out_1685726956674156760[348] = 0; + out_1685726956674156760[349] = 0; + out_1685726956674156760[350] = 0; + out_1685726956674156760[351] = 1.0; + out_1685726956674156760[352] = 0; + out_1685726956674156760[353] = 0; + out_1685726956674156760[354] = 0; + out_1685726956674156760[355] = 0; + out_1685726956674156760[356] = 0; + out_1685726956674156760[357] = 0; + out_1685726956674156760[358] = 0; + out_1685726956674156760[359] = 0; + out_1685726956674156760[360] = 0; + out_1685726956674156760[361] = 0; + out_1685726956674156760[362] = 0; + out_1685726956674156760[363] = 0; + out_1685726956674156760[364] = 0; + out_1685726956674156760[365] = 0; + out_1685726956674156760[366] = 0; + out_1685726956674156760[367] = 0; + out_1685726956674156760[368] = 0; + out_1685726956674156760[369] = 0; + out_1685726956674156760[370] = 0; + out_1685726956674156760[371] = 0; + out_1685726956674156760[372] = 0; + out_1685726956674156760[373] = 1.0; + out_1685726956674156760[374] = 0; + out_1685726956674156760[375] = 0; + out_1685726956674156760[376] = 0; + out_1685726956674156760[377] = 0; + out_1685726956674156760[378] = 0; + out_1685726956674156760[379] = 0; + out_1685726956674156760[380] = 0; + out_1685726956674156760[381] = 0; + out_1685726956674156760[382] = 0; + out_1685726956674156760[383] = 0; + out_1685726956674156760[384] = 0; + out_1685726956674156760[385] = 0; + out_1685726956674156760[386] = 0; + out_1685726956674156760[387] = 0; + out_1685726956674156760[388] = 0; + out_1685726956674156760[389] = 0; + out_1685726956674156760[390] = 0; + out_1685726956674156760[391] = 0; + out_1685726956674156760[392] = 0; + out_1685726956674156760[393] = 0; + out_1685726956674156760[394] = 0; + out_1685726956674156760[395] = 1.0; + out_1685726956674156760[396] = 0; + out_1685726956674156760[397] = 0; + out_1685726956674156760[398] = 0; + out_1685726956674156760[399] = 0; + out_1685726956674156760[400] = 0; + out_1685726956674156760[401] = 0; + out_1685726956674156760[402] = 0; + out_1685726956674156760[403] = 0; + out_1685726956674156760[404] = 0; + out_1685726956674156760[405] = 0; + out_1685726956674156760[406] = 0; + out_1685726956674156760[407] = 0; + out_1685726956674156760[408] = 0; + out_1685726956674156760[409] = 0; + out_1685726956674156760[410] = 0; + out_1685726956674156760[411] = 0; + out_1685726956674156760[412] = 0; + out_1685726956674156760[413] = 0; + out_1685726956674156760[414] = 0; + out_1685726956674156760[415] = 0; + out_1685726956674156760[416] = 0; + out_1685726956674156760[417] = 1.0; + out_1685726956674156760[418] = 0; + out_1685726956674156760[419] = 0; + out_1685726956674156760[420] = 0; + out_1685726956674156760[421] = 0; + out_1685726956674156760[422] = 0; + out_1685726956674156760[423] = 0; + out_1685726956674156760[424] = 0; + out_1685726956674156760[425] = 0; + out_1685726956674156760[426] = 0; + out_1685726956674156760[427] = 0; + out_1685726956674156760[428] = 0; + out_1685726956674156760[429] = 0; + out_1685726956674156760[430] = 0; + out_1685726956674156760[431] = 0; + out_1685726956674156760[432] = 0; + out_1685726956674156760[433] = 0; + out_1685726956674156760[434] = 0; + out_1685726956674156760[435] = 0; + out_1685726956674156760[436] = 0; + out_1685726956674156760[437] = 0; + out_1685726956674156760[438] = 0; + out_1685726956674156760[439] = 1.0; + out_1685726956674156760[440] = 0; + out_1685726956674156760[441] = 0; + out_1685726956674156760[442] = 0; + out_1685726956674156760[443] = 0; + out_1685726956674156760[444] = 0; + out_1685726956674156760[445] = 0; + out_1685726956674156760[446] = 0; + out_1685726956674156760[447] = 0; + out_1685726956674156760[448] = 0; + out_1685726956674156760[449] = 0; + out_1685726956674156760[450] = 0; + out_1685726956674156760[451] = 0; + out_1685726956674156760[452] = 0; + out_1685726956674156760[453] = 0; + out_1685726956674156760[454] = 0; + out_1685726956674156760[455] = 0; + out_1685726956674156760[456] = 0; + out_1685726956674156760[457] = 0; + out_1685726956674156760[458] = 0; + out_1685726956674156760[459] = 0; + out_1685726956674156760[460] = 0; + out_1685726956674156760[461] = 1.0; } -void f_fun(double *state, double dt, double *out_5945410084485745517) { - out_5945410084485745517[0] = dt*state[7] + state[0]; - out_5945410084485745517[1] = dt*state[8] + state[1]; - out_5945410084485745517[2] = dt*state[9] + state[2]; - out_5945410084485745517[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; - out_5945410084485745517[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; - out_5945410084485745517[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; - out_5945410084485745517[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; - out_5945410084485745517[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; - out_5945410084485745517[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; - out_5945410084485745517[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; - out_5945410084485745517[10] = state[10]; - out_5945410084485745517[11] = state[11]; - out_5945410084485745517[12] = state[12]; - out_5945410084485745517[13] = state[13]; - out_5945410084485745517[14] = state[14]; - out_5945410084485745517[15] = state[15]; - out_5945410084485745517[16] = state[16]; - out_5945410084485745517[17] = state[17]; - out_5945410084485745517[18] = state[18]; - out_5945410084485745517[19] = state[19]; - out_5945410084485745517[20] = state[20]; - out_5945410084485745517[21] = state[21]; +void f_fun(double *state, double dt, double *out_774966178326963321) { + out_774966178326963321[0] = dt*state[7] + state[0]; + out_774966178326963321[1] = dt*state[8] + state[1]; + out_774966178326963321[2] = dt*state[9] + state[2]; + out_774966178326963321[3] = dt*(-0.5*state[4]*state[10] - 0.5*state[5]*state[11] - 0.5*state[6]*state[12]) + state[3]; + out_774966178326963321[4] = dt*(0.5*state[3]*state[10] + 0.5*state[5]*state[12] - 0.5*state[6]*state[11]) + state[4]; + out_774966178326963321[5] = dt*(0.5*state[3]*state[11] - 0.5*state[4]*state[12] + 0.5*state[6]*state[10]) + state[5]; + out_774966178326963321[6] = dt*(0.5*state[3]*state[12] + 0.5*state[4]*state[11] - 0.5*state[5]*state[10]) + state[6]; + out_774966178326963321[7] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]) + state[7]; + out_774966178326963321[8] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]) + state[8]; + out_774966178326963321[9] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]) + state[9]; + out_774966178326963321[10] = state[10]; + out_774966178326963321[11] = state[11]; + out_774966178326963321[12] = state[12]; + out_774966178326963321[13] = state[13]; + out_774966178326963321[14] = state[14]; + out_774966178326963321[15] = state[15]; + out_774966178326963321[16] = state[16]; + out_774966178326963321[17] = state[17]; + out_774966178326963321[18] = state[18]; + out_774966178326963321[19] = state[19]; + out_774966178326963321[20] = state[20]; + out_774966178326963321[21] = state[21]; } -void F_fun(double *state, double dt, double *out_1406202444158773973) { - out_1406202444158773973[0] = 1; - out_1406202444158773973[1] = 0; - out_1406202444158773973[2] = 0; - out_1406202444158773973[3] = 0; - out_1406202444158773973[4] = 0; - out_1406202444158773973[5] = 0; - out_1406202444158773973[6] = dt; - out_1406202444158773973[7] = 0; - out_1406202444158773973[8] = 0; - out_1406202444158773973[9] = 0; - out_1406202444158773973[10] = 0; - out_1406202444158773973[11] = 0; - out_1406202444158773973[12] = 0; - out_1406202444158773973[13] = 0; - out_1406202444158773973[14] = 0; - out_1406202444158773973[15] = 0; - out_1406202444158773973[16] = 0; - out_1406202444158773973[17] = 0; - out_1406202444158773973[18] = 0; - out_1406202444158773973[19] = 0; - out_1406202444158773973[20] = 0; - out_1406202444158773973[21] = 0; - out_1406202444158773973[22] = 1; - out_1406202444158773973[23] = 0; - out_1406202444158773973[24] = 0; - out_1406202444158773973[25] = 0; - out_1406202444158773973[26] = 0; - out_1406202444158773973[27] = 0; - out_1406202444158773973[28] = dt; - out_1406202444158773973[29] = 0; - out_1406202444158773973[30] = 0; - out_1406202444158773973[31] = 0; - out_1406202444158773973[32] = 0; - out_1406202444158773973[33] = 0; - out_1406202444158773973[34] = 0; - out_1406202444158773973[35] = 0; - out_1406202444158773973[36] = 0; - out_1406202444158773973[37] = 0; - out_1406202444158773973[38] = 0; - out_1406202444158773973[39] = 0; - out_1406202444158773973[40] = 0; - out_1406202444158773973[41] = 0; - out_1406202444158773973[42] = 0; - out_1406202444158773973[43] = 0; - out_1406202444158773973[44] = 1; - out_1406202444158773973[45] = 0; - out_1406202444158773973[46] = 0; - out_1406202444158773973[47] = 0; - out_1406202444158773973[48] = 0; - out_1406202444158773973[49] = 0; - out_1406202444158773973[50] = dt; - out_1406202444158773973[51] = 0; - out_1406202444158773973[52] = 0; - out_1406202444158773973[53] = 0; - out_1406202444158773973[54] = 0; - out_1406202444158773973[55] = 0; - out_1406202444158773973[56] = 0; - out_1406202444158773973[57] = 0; - out_1406202444158773973[58] = 0; - out_1406202444158773973[59] = 0; - out_1406202444158773973[60] = 0; - out_1406202444158773973[61] = 0; - out_1406202444158773973[62] = 0; - out_1406202444158773973[63] = 0; - out_1406202444158773973[64] = 0; - out_1406202444158773973[65] = 0; - out_1406202444158773973[66] = 1; - out_1406202444158773973[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_1406202444158773973[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); - out_1406202444158773973[69] = 0; - out_1406202444158773973[70] = 0; - out_1406202444158773973[71] = 0; - out_1406202444158773973[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_1406202444158773973[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_1406202444158773973[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_1406202444158773973[75] = 0; - out_1406202444158773973[76] = 0; - out_1406202444158773973[77] = 0; - out_1406202444158773973[78] = 0; - out_1406202444158773973[79] = 0; - out_1406202444158773973[80] = 0; - out_1406202444158773973[81] = 0; - out_1406202444158773973[82] = 0; - out_1406202444158773973[83] = 0; - out_1406202444158773973[84] = 0; - out_1406202444158773973[85] = 0; - out_1406202444158773973[86] = 0; - out_1406202444158773973[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); - out_1406202444158773973[88] = 1; - out_1406202444158773973[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); - out_1406202444158773973[90] = 0; - out_1406202444158773973[91] = 0; - out_1406202444158773973[92] = 0; - out_1406202444158773973[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_1406202444158773973[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_1406202444158773973[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_1406202444158773973[96] = 0; - out_1406202444158773973[97] = 0; - out_1406202444158773973[98] = 0; - out_1406202444158773973[99] = 0; - out_1406202444158773973[100] = 0; - out_1406202444158773973[101] = 0; - out_1406202444158773973[102] = 0; - out_1406202444158773973[103] = 0; - out_1406202444158773973[104] = 0; - out_1406202444158773973[105] = 0; - out_1406202444158773973[106] = 0; - out_1406202444158773973[107] = 0; - out_1406202444158773973[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); - out_1406202444158773973[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); - out_1406202444158773973[110] = 1; - out_1406202444158773973[111] = 0; - out_1406202444158773973[112] = 0; - out_1406202444158773973[113] = 0; - out_1406202444158773973[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_1406202444158773973[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_1406202444158773973[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_1406202444158773973[117] = 0; - out_1406202444158773973[118] = 0; - out_1406202444158773973[119] = 0; - out_1406202444158773973[120] = 0; - out_1406202444158773973[121] = 0; - out_1406202444158773973[122] = 0; - out_1406202444158773973[123] = 0; - out_1406202444158773973[124] = 0; - out_1406202444158773973[125] = 0; - out_1406202444158773973[126] = 0; - out_1406202444158773973[127] = 0; - out_1406202444158773973[128] = 0; - out_1406202444158773973[129] = 0; - out_1406202444158773973[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_1406202444158773973[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); - out_1406202444158773973[132] = 1; - out_1406202444158773973[133] = 0; - out_1406202444158773973[134] = 0; - out_1406202444158773973[135] = 0; - out_1406202444158773973[136] = 0; - out_1406202444158773973[137] = 0; - out_1406202444158773973[138] = 0; - out_1406202444158773973[139] = 0; - out_1406202444158773973[140] = 0; - out_1406202444158773973[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_1406202444158773973[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); - out_1406202444158773973[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); - out_1406202444158773973[144] = 0; - out_1406202444158773973[145] = 0; - out_1406202444158773973[146] = 0; - out_1406202444158773973[147] = 0; - out_1406202444158773973[148] = 0; - out_1406202444158773973[149] = 0; - out_1406202444158773973[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); - out_1406202444158773973[151] = 0; - out_1406202444158773973[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); - out_1406202444158773973[153] = 0; - out_1406202444158773973[154] = 1; - out_1406202444158773973[155] = 0; - out_1406202444158773973[156] = 0; - out_1406202444158773973[157] = 0; - out_1406202444158773973[158] = 0; - out_1406202444158773973[159] = 0; - out_1406202444158773973[160] = 0; - out_1406202444158773973[161] = 0; - out_1406202444158773973[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); - out_1406202444158773973[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_1406202444158773973[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); - out_1406202444158773973[165] = 0; - out_1406202444158773973[166] = 0; - out_1406202444158773973[167] = 0; - out_1406202444158773973[168] = 0; - out_1406202444158773973[169] = 0; - out_1406202444158773973[170] = 0; - out_1406202444158773973[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); - out_1406202444158773973[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); - out_1406202444158773973[173] = 0; - out_1406202444158773973[174] = 0; - out_1406202444158773973[175] = 0; - out_1406202444158773973[176] = 1; - out_1406202444158773973[177] = 0; - out_1406202444158773973[178] = 0; - out_1406202444158773973[179] = 0; - out_1406202444158773973[180] = 0; - out_1406202444158773973[181] = 0; - out_1406202444158773973[182] = 0; - out_1406202444158773973[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); - out_1406202444158773973[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); - out_1406202444158773973[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_1406202444158773973[186] = 0; - out_1406202444158773973[187] = 0; - out_1406202444158773973[188] = 0; - out_1406202444158773973[189] = 0; - out_1406202444158773973[190] = 0; - out_1406202444158773973[191] = 0; - out_1406202444158773973[192] = 0; - out_1406202444158773973[193] = 0; - out_1406202444158773973[194] = 0; - out_1406202444158773973[195] = 0; - out_1406202444158773973[196] = 0; - out_1406202444158773973[197] = 0; - out_1406202444158773973[198] = 1; - out_1406202444158773973[199] = 0; - out_1406202444158773973[200] = 0; - out_1406202444158773973[201] = 0; - out_1406202444158773973[202] = 0; - out_1406202444158773973[203] = 0; - out_1406202444158773973[204] = 0; - out_1406202444158773973[205] = 0; - out_1406202444158773973[206] = 0; - out_1406202444158773973[207] = 0; - out_1406202444158773973[208] = 0; - out_1406202444158773973[209] = 0; - out_1406202444158773973[210] = 0; - out_1406202444158773973[211] = 0; - out_1406202444158773973[212] = 0; - out_1406202444158773973[213] = 0; - out_1406202444158773973[214] = 0; - out_1406202444158773973[215] = 0; - out_1406202444158773973[216] = 0; - out_1406202444158773973[217] = 0; - out_1406202444158773973[218] = 0; - out_1406202444158773973[219] = 0; - out_1406202444158773973[220] = 1; - out_1406202444158773973[221] = 0; - out_1406202444158773973[222] = 0; - out_1406202444158773973[223] = 0; - out_1406202444158773973[224] = 0; - out_1406202444158773973[225] = 0; - out_1406202444158773973[226] = 0; - out_1406202444158773973[227] = 0; - out_1406202444158773973[228] = 0; - out_1406202444158773973[229] = 0; - out_1406202444158773973[230] = 0; - out_1406202444158773973[231] = 0; - out_1406202444158773973[232] = 0; - out_1406202444158773973[233] = 0; - out_1406202444158773973[234] = 0; - out_1406202444158773973[235] = 0; - out_1406202444158773973[236] = 0; - out_1406202444158773973[237] = 0; - out_1406202444158773973[238] = 0; - out_1406202444158773973[239] = 0; - out_1406202444158773973[240] = 0; - out_1406202444158773973[241] = 0; - out_1406202444158773973[242] = 1; - out_1406202444158773973[243] = 0; - out_1406202444158773973[244] = 0; - out_1406202444158773973[245] = 0; - out_1406202444158773973[246] = 0; - out_1406202444158773973[247] = 0; - out_1406202444158773973[248] = 0; - out_1406202444158773973[249] = 0; - out_1406202444158773973[250] = 0; - out_1406202444158773973[251] = 0; - out_1406202444158773973[252] = 0; - out_1406202444158773973[253] = 0; - out_1406202444158773973[254] = 0; - out_1406202444158773973[255] = 0; - out_1406202444158773973[256] = 0; - out_1406202444158773973[257] = 0; - out_1406202444158773973[258] = 0; - out_1406202444158773973[259] = 0; - out_1406202444158773973[260] = 0; - out_1406202444158773973[261] = 0; - out_1406202444158773973[262] = 0; - out_1406202444158773973[263] = 0; - out_1406202444158773973[264] = 1; - out_1406202444158773973[265] = 0; - out_1406202444158773973[266] = 0; - out_1406202444158773973[267] = 0; - out_1406202444158773973[268] = 0; - out_1406202444158773973[269] = 0; - out_1406202444158773973[270] = 0; - out_1406202444158773973[271] = 0; - out_1406202444158773973[272] = 0; - out_1406202444158773973[273] = 0; - out_1406202444158773973[274] = 0; - out_1406202444158773973[275] = 0; - out_1406202444158773973[276] = 0; - out_1406202444158773973[277] = 0; - out_1406202444158773973[278] = 0; - out_1406202444158773973[279] = 0; - out_1406202444158773973[280] = 0; - out_1406202444158773973[281] = 0; - out_1406202444158773973[282] = 0; - out_1406202444158773973[283] = 0; - out_1406202444158773973[284] = 0; - out_1406202444158773973[285] = 0; - out_1406202444158773973[286] = 1; - out_1406202444158773973[287] = 0; - out_1406202444158773973[288] = 0; - out_1406202444158773973[289] = 0; - out_1406202444158773973[290] = 0; - out_1406202444158773973[291] = 0; - out_1406202444158773973[292] = 0; - out_1406202444158773973[293] = 0; - out_1406202444158773973[294] = 0; - out_1406202444158773973[295] = 0; - out_1406202444158773973[296] = 0; - out_1406202444158773973[297] = 0; - out_1406202444158773973[298] = 0; - out_1406202444158773973[299] = 0; - out_1406202444158773973[300] = 0; - out_1406202444158773973[301] = 0; - out_1406202444158773973[302] = 0; - out_1406202444158773973[303] = 0; - out_1406202444158773973[304] = 0; - out_1406202444158773973[305] = 0; - out_1406202444158773973[306] = 0; - out_1406202444158773973[307] = 0; - out_1406202444158773973[308] = 1; - out_1406202444158773973[309] = 0; - out_1406202444158773973[310] = 0; - out_1406202444158773973[311] = 0; - out_1406202444158773973[312] = 0; - out_1406202444158773973[313] = 0; - out_1406202444158773973[314] = 0; - out_1406202444158773973[315] = 0; - out_1406202444158773973[316] = 0; - out_1406202444158773973[317] = 0; - out_1406202444158773973[318] = 0; - out_1406202444158773973[319] = 0; - out_1406202444158773973[320] = 0; - out_1406202444158773973[321] = 0; - out_1406202444158773973[322] = 0; - out_1406202444158773973[323] = 0; - out_1406202444158773973[324] = 0; - out_1406202444158773973[325] = 0; - out_1406202444158773973[326] = 0; - out_1406202444158773973[327] = 0; - out_1406202444158773973[328] = 0; - out_1406202444158773973[329] = 0; - out_1406202444158773973[330] = 1; - out_1406202444158773973[331] = 0; - out_1406202444158773973[332] = 0; - out_1406202444158773973[333] = 0; - out_1406202444158773973[334] = 0; - out_1406202444158773973[335] = 0; - out_1406202444158773973[336] = 0; - out_1406202444158773973[337] = 0; - out_1406202444158773973[338] = 0; - out_1406202444158773973[339] = 0; - out_1406202444158773973[340] = 0; - out_1406202444158773973[341] = 0; - out_1406202444158773973[342] = 0; - out_1406202444158773973[343] = 0; - out_1406202444158773973[344] = 0; - out_1406202444158773973[345] = 0; - out_1406202444158773973[346] = 0; - out_1406202444158773973[347] = 0; - out_1406202444158773973[348] = 0; - out_1406202444158773973[349] = 0; - out_1406202444158773973[350] = 0; - out_1406202444158773973[351] = 0; - out_1406202444158773973[352] = 1; - out_1406202444158773973[353] = 0; - out_1406202444158773973[354] = 0; - out_1406202444158773973[355] = 0; - out_1406202444158773973[356] = 0; - out_1406202444158773973[357] = 0; - out_1406202444158773973[358] = 0; - out_1406202444158773973[359] = 0; - out_1406202444158773973[360] = 0; - out_1406202444158773973[361] = 0; - out_1406202444158773973[362] = 0; - out_1406202444158773973[363] = 0; - out_1406202444158773973[364] = 0; - out_1406202444158773973[365] = 0; - out_1406202444158773973[366] = 0; - out_1406202444158773973[367] = 0; - out_1406202444158773973[368] = 0; - out_1406202444158773973[369] = 0; - out_1406202444158773973[370] = 0; - out_1406202444158773973[371] = 0; - out_1406202444158773973[372] = 0; - out_1406202444158773973[373] = 0; - out_1406202444158773973[374] = 1; - out_1406202444158773973[375] = 0; - out_1406202444158773973[376] = 0; - out_1406202444158773973[377] = 0; - out_1406202444158773973[378] = 0; - out_1406202444158773973[379] = 0; - out_1406202444158773973[380] = 0; - out_1406202444158773973[381] = 0; - out_1406202444158773973[382] = 0; - out_1406202444158773973[383] = 0; - out_1406202444158773973[384] = 0; - out_1406202444158773973[385] = 0; - out_1406202444158773973[386] = 0; - out_1406202444158773973[387] = 0; - out_1406202444158773973[388] = 0; - out_1406202444158773973[389] = 0; - out_1406202444158773973[390] = 0; - out_1406202444158773973[391] = 0; - out_1406202444158773973[392] = 0; - out_1406202444158773973[393] = 0; - out_1406202444158773973[394] = 0; - out_1406202444158773973[395] = 0; - out_1406202444158773973[396] = 1; - out_1406202444158773973[397] = 0; - out_1406202444158773973[398] = 0; - out_1406202444158773973[399] = 0; - out_1406202444158773973[400] = 0; - out_1406202444158773973[401] = 0; - out_1406202444158773973[402] = 0; - out_1406202444158773973[403] = 0; - out_1406202444158773973[404] = 0; - out_1406202444158773973[405] = 0; - out_1406202444158773973[406] = 0; - out_1406202444158773973[407] = 0; - out_1406202444158773973[408] = 0; - out_1406202444158773973[409] = 0; - out_1406202444158773973[410] = 0; - out_1406202444158773973[411] = 0; - out_1406202444158773973[412] = 0; - out_1406202444158773973[413] = 0; - out_1406202444158773973[414] = 0; - out_1406202444158773973[415] = 0; - out_1406202444158773973[416] = 0; - out_1406202444158773973[417] = 0; - out_1406202444158773973[418] = 1; - out_1406202444158773973[419] = 0; - out_1406202444158773973[420] = 0; - out_1406202444158773973[421] = 0; - out_1406202444158773973[422] = 0; - out_1406202444158773973[423] = 0; - out_1406202444158773973[424] = 0; - out_1406202444158773973[425] = 0; - out_1406202444158773973[426] = 0; - out_1406202444158773973[427] = 0; - out_1406202444158773973[428] = 0; - out_1406202444158773973[429] = 0; - out_1406202444158773973[430] = 0; - out_1406202444158773973[431] = 0; - out_1406202444158773973[432] = 0; - out_1406202444158773973[433] = 0; - out_1406202444158773973[434] = 0; - out_1406202444158773973[435] = 0; - out_1406202444158773973[436] = 0; - out_1406202444158773973[437] = 0; - out_1406202444158773973[438] = 0; - out_1406202444158773973[439] = 0; - out_1406202444158773973[440] = 1; +void F_fun(double *state, double dt, double *out_7282145086155387076) { + out_7282145086155387076[0] = 1; + out_7282145086155387076[1] = 0; + out_7282145086155387076[2] = 0; + out_7282145086155387076[3] = 0; + out_7282145086155387076[4] = 0; + out_7282145086155387076[5] = 0; + out_7282145086155387076[6] = dt; + out_7282145086155387076[7] = 0; + out_7282145086155387076[8] = 0; + out_7282145086155387076[9] = 0; + out_7282145086155387076[10] = 0; + out_7282145086155387076[11] = 0; + out_7282145086155387076[12] = 0; + out_7282145086155387076[13] = 0; + out_7282145086155387076[14] = 0; + out_7282145086155387076[15] = 0; + out_7282145086155387076[16] = 0; + out_7282145086155387076[17] = 0; + out_7282145086155387076[18] = 0; + out_7282145086155387076[19] = 0; + out_7282145086155387076[20] = 0; + out_7282145086155387076[21] = 0; + out_7282145086155387076[22] = 1; + out_7282145086155387076[23] = 0; + out_7282145086155387076[24] = 0; + out_7282145086155387076[25] = 0; + out_7282145086155387076[26] = 0; + out_7282145086155387076[27] = 0; + out_7282145086155387076[28] = dt; + out_7282145086155387076[29] = 0; + out_7282145086155387076[30] = 0; + out_7282145086155387076[31] = 0; + out_7282145086155387076[32] = 0; + out_7282145086155387076[33] = 0; + out_7282145086155387076[34] = 0; + out_7282145086155387076[35] = 0; + out_7282145086155387076[36] = 0; + out_7282145086155387076[37] = 0; + out_7282145086155387076[38] = 0; + out_7282145086155387076[39] = 0; + out_7282145086155387076[40] = 0; + out_7282145086155387076[41] = 0; + out_7282145086155387076[42] = 0; + out_7282145086155387076[43] = 0; + out_7282145086155387076[44] = 1; + out_7282145086155387076[45] = 0; + out_7282145086155387076[46] = 0; + out_7282145086155387076[47] = 0; + out_7282145086155387076[48] = 0; + out_7282145086155387076[49] = 0; + out_7282145086155387076[50] = dt; + out_7282145086155387076[51] = 0; + out_7282145086155387076[52] = 0; + out_7282145086155387076[53] = 0; + out_7282145086155387076[54] = 0; + out_7282145086155387076[55] = 0; + out_7282145086155387076[56] = 0; + out_7282145086155387076[57] = 0; + out_7282145086155387076[58] = 0; + out_7282145086155387076[59] = 0; + out_7282145086155387076[60] = 0; + out_7282145086155387076[61] = 0; + out_7282145086155387076[62] = 0; + out_7282145086155387076[63] = 0; + out_7282145086155387076[64] = 0; + out_7282145086155387076[65] = 0; + out_7282145086155387076[66] = 1; + out_7282145086155387076[67] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[11] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); + out_7282145086155387076[68] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[12] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[10] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[11]); + out_7282145086155387076[69] = 0; + out_7282145086155387076[70] = 0; + out_7282145086155387076[71] = 0; + out_7282145086155387076[72] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_7282145086155387076[73] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); + out_7282145086155387076[74] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); + out_7282145086155387076[75] = 0; + out_7282145086155387076[76] = 0; + out_7282145086155387076[77] = 0; + out_7282145086155387076[78] = 0; + out_7282145086155387076[79] = 0; + out_7282145086155387076[80] = 0; + out_7282145086155387076[81] = 0; + out_7282145086155387076[82] = 0; + out_7282145086155387076[83] = 0; + out_7282145086155387076[84] = 0; + out_7282145086155387076[85] = 0; + out_7282145086155387076[86] = 0; + out_7282145086155387076[87] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[11] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[10] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[12]); + out_7282145086155387076[88] = 1; + out_7282145086155387076[89] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[12] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[11] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[10]); + out_7282145086155387076[90] = 0; + out_7282145086155387076[91] = 0; + out_7282145086155387076[92] = 0; + out_7282145086155387076[93] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); + out_7282145086155387076[94] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_7282145086155387076[95] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); + out_7282145086155387076[96] = 0; + out_7282145086155387076[97] = 0; + out_7282145086155387076[98] = 0; + out_7282145086155387076[99] = 0; + out_7282145086155387076[100] = 0; + out_7282145086155387076[101] = 0; + out_7282145086155387076[102] = 0; + out_7282145086155387076[103] = 0; + out_7282145086155387076[104] = 0; + out_7282145086155387076[105] = 0; + out_7282145086155387076[106] = 0; + out_7282145086155387076[107] = 0; + out_7282145086155387076[108] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[12] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[10] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[11]); + out_7282145086155387076[109] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[12] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[11] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[10]); + out_7282145086155387076[110] = 1; + out_7282145086155387076[111] = 0; + out_7282145086155387076[112] = 0; + out_7282145086155387076[113] = 0; + out_7282145086155387076[114] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); + out_7282145086155387076[115] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); + out_7282145086155387076[116] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_7282145086155387076[117] = 0; + out_7282145086155387076[118] = 0; + out_7282145086155387076[119] = 0; + out_7282145086155387076[120] = 0; + out_7282145086155387076[121] = 0; + out_7282145086155387076[122] = 0; + out_7282145086155387076[123] = 0; + out_7282145086155387076[124] = 0; + out_7282145086155387076[125] = 0; + out_7282145086155387076[126] = 0; + out_7282145086155387076[127] = 0; + out_7282145086155387076[128] = 0; + out_7282145086155387076[129] = 0; + out_7282145086155387076[130] = dt*((2*state[3]*state[4] + 2*state[5]*state[6])*state[17] + (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); + out_7282145086155387076[131] = dt*((2*state[3]*state[4] - 2*state[5]*state[6])*state[18] + (-2*state[3]*state[6] - 2*state[4]*state[5])*state[16] + (-pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[17]); + out_7282145086155387076[132] = 1; + out_7282145086155387076[133] = 0; + out_7282145086155387076[134] = 0; + out_7282145086155387076[135] = 0; + out_7282145086155387076[136] = 0; + out_7282145086155387076[137] = 0; + out_7282145086155387076[138] = 0; + out_7282145086155387076[139] = 0; + out_7282145086155387076[140] = 0; + out_7282145086155387076[141] = dt*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_7282145086155387076[142] = dt*(-2*state[3]*state[6] + 2*state[4]*state[5]); + out_7282145086155387076[143] = dt*(2*state[3]*state[5] + 2*state[4]*state[6]); + out_7282145086155387076[144] = 0; + out_7282145086155387076[145] = 0; + out_7282145086155387076[146] = 0; + out_7282145086155387076[147] = 0; + out_7282145086155387076[148] = 0; + out_7282145086155387076[149] = 0; + out_7282145086155387076[150] = dt*(-(2*state[3]*state[4] + 2*state[5]*state[6])*state[17] - (-2*state[3]*state[5] + 2*state[4]*state[6])*state[16] - (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[18]); + out_7282145086155387076[151] = 0; + out_7282145086155387076[152] = dt*((2*state[3]*state[5] + 2*state[4]*state[6])*state[18] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[17] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[16]); + out_7282145086155387076[153] = 0; + out_7282145086155387076[154] = 1; + out_7282145086155387076[155] = 0; + out_7282145086155387076[156] = 0; + out_7282145086155387076[157] = 0; + out_7282145086155387076[158] = 0; + out_7282145086155387076[159] = 0; + out_7282145086155387076[160] = 0; + out_7282145086155387076[161] = 0; + out_7282145086155387076[162] = dt*(2*state[3]*state[6] + 2*state[4]*state[5]); + out_7282145086155387076[163] = dt*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_7282145086155387076[164] = dt*(-2*state[3]*state[4] + 2*state[5]*state[6]); + out_7282145086155387076[165] = 0; + out_7282145086155387076[166] = 0; + out_7282145086155387076[167] = 0; + out_7282145086155387076[168] = 0; + out_7282145086155387076[169] = 0; + out_7282145086155387076[170] = 0; + out_7282145086155387076[171] = dt*((-2*state[3]*state[4] + 2*state[5]*state[6])*state[18] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[16] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[17]); + out_7282145086155387076[172] = dt*((-2*state[3]*state[5] - 2*state[4]*state[6])*state[18] + (2*state[3]*state[6] - 2*state[4]*state[5])*state[17] + (-pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) + pow(state[6], 2))*state[16]); + out_7282145086155387076[173] = 0; + out_7282145086155387076[174] = 0; + out_7282145086155387076[175] = 0; + out_7282145086155387076[176] = 1; + out_7282145086155387076[177] = 0; + out_7282145086155387076[178] = 0; + out_7282145086155387076[179] = 0; + out_7282145086155387076[180] = 0; + out_7282145086155387076[181] = 0; + out_7282145086155387076[182] = 0; + out_7282145086155387076[183] = dt*(-2*state[3]*state[5] + 2*state[4]*state[6]); + out_7282145086155387076[184] = dt*(2*state[3]*state[4] + 2*state[5]*state[6]); + out_7282145086155387076[185] = dt*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_7282145086155387076[186] = 0; + out_7282145086155387076[187] = 0; + out_7282145086155387076[188] = 0; + out_7282145086155387076[189] = 0; + out_7282145086155387076[190] = 0; + out_7282145086155387076[191] = 0; + out_7282145086155387076[192] = 0; + out_7282145086155387076[193] = 0; + out_7282145086155387076[194] = 0; + out_7282145086155387076[195] = 0; + out_7282145086155387076[196] = 0; + out_7282145086155387076[197] = 0; + out_7282145086155387076[198] = 1; + out_7282145086155387076[199] = 0; + out_7282145086155387076[200] = 0; + out_7282145086155387076[201] = 0; + out_7282145086155387076[202] = 0; + out_7282145086155387076[203] = 0; + out_7282145086155387076[204] = 0; + out_7282145086155387076[205] = 0; + out_7282145086155387076[206] = 0; + out_7282145086155387076[207] = 0; + out_7282145086155387076[208] = 0; + out_7282145086155387076[209] = 0; + out_7282145086155387076[210] = 0; + out_7282145086155387076[211] = 0; + out_7282145086155387076[212] = 0; + out_7282145086155387076[213] = 0; + out_7282145086155387076[214] = 0; + out_7282145086155387076[215] = 0; + out_7282145086155387076[216] = 0; + out_7282145086155387076[217] = 0; + out_7282145086155387076[218] = 0; + out_7282145086155387076[219] = 0; + out_7282145086155387076[220] = 1; + out_7282145086155387076[221] = 0; + out_7282145086155387076[222] = 0; + out_7282145086155387076[223] = 0; + out_7282145086155387076[224] = 0; + out_7282145086155387076[225] = 0; + out_7282145086155387076[226] = 0; + out_7282145086155387076[227] = 0; + out_7282145086155387076[228] = 0; + out_7282145086155387076[229] = 0; + out_7282145086155387076[230] = 0; + out_7282145086155387076[231] = 0; + out_7282145086155387076[232] = 0; + out_7282145086155387076[233] = 0; + out_7282145086155387076[234] = 0; + out_7282145086155387076[235] = 0; + out_7282145086155387076[236] = 0; + out_7282145086155387076[237] = 0; + out_7282145086155387076[238] = 0; + out_7282145086155387076[239] = 0; + out_7282145086155387076[240] = 0; + out_7282145086155387076[241] = 0; + out_7282145086155387076[242] = 1; + out_7282145086155387076[243] = 0; + out_7282145086155387076[244] = 0; + out_7282145086155387076[245] = 0; + out_7282145086155387076[246] = 0; + out_7282145086155387076[247] = 0; + out_7282145086155387076[248] = 0; + out_7282145086155387076[249] = 0; + out_7282145086155387076[250] = 0; + out_7282145086155387076[251] = 0; + out_7282145086155387076[252] = 0; + out_7282145086155387076[253] = 0; + out_7282145086155387076[254] = 0; + out_7282145086155387076[255] = 0; + out_7282145086155387076[256] = 0; + out_7282145086155387076[257] = 0; + out_7282145086155387076[258] = 0; + out_7282145086155387076[259] = 0; + out_7282145086155387076[260] = 0; + out_7282145086155387076[261] = 0; + out_7282145086155387076[262] = 0; + out_7282145086155387076[263] = 0; + out_7282145086155387076[264] = 1; + out_7282145086155387076[265] = 0; + out_7282145086155387076[266] = 0; + out_7282145086155387076[267] = 0; + out_7282145086155387076[268] = 0; + out_7282145086155387076[269] = 0; + out_7282145086155387076[270] = 0; + out_7282145086155387076[271] = 0; + out_7282145086155387076[272] = 0; + out_7282145086155387076[273] = 0; + out_7282145086155387076[274] = 0; + out_7282145086155387076[275] = 0; + out_7282145086155387076[276] = 0; + out_7282145086155387076[277] = 0; + out_7282145086155387076[278] = 0; + out_7282145086155387076[279] = 0; + out_7282145086155387076[280] = 0; + out_7282145086155387076[281] = 0; + out_7282145086155387076[282] = 0; + out_7282145086155387076[283] = 0; + out_7282145086155387076[284] = 0; + out_7282145086155387076[285] = 0; + out_7282145086155387076[286] = 1; + out_7282145086155387076[287] = 0; + out_7282145086155387076[288] = 0; + out_7282145086155387076[289] = 0; + out_7282145086155387076[290] = 0; + out_7282145086155387076[291] = 0; + out_7282145086155387076[292] = 0; + out_7282145086155387076[293] = 0; + out_7282145086155387076[294] = 0; + out_7282145086155387076[295] = 0; + out_7282145086155387076[296] = 0; + out_7282145086155387076[297] = 0; + out_7282145086155387076[298] = 0; + out_7282145086155387076[299] = 0; + out_7282145086155387076[300] = 0; + out_7282145086155387076[301] = 0; + out_7282145086155387076[302] = 0; + out_7282145086155387076[303] = 0; + out_7282145086155387076[304] = 0; + out_7282145086155387076[305] = 0; + out_7282145086155387076[306] = 0; + out_7282145086155387076[307] = 0; + out_7282145086155387076[308] = 1; + out_7282145086155387076[309] = 0; + out_7282145086155387076[310] = 0; + out_7282145086155387076[311] = 0; + out_7282145086155387076[312] = 0; + out_7282145086155387076[313] = 0; + out_7282145086155387076[314] = 0; + out_7282145086155387076[315] = 0; + out_7282145086155387076[316] = 0; + out_7282145086155387076[317] = 0; + out_7282145086155387076[318] = 0; + out_7282145086155387076[319] = 0; + out_7282145086155387076[320] = 0; + out_7282145086155387076[321] = 0; + out_7282145086155387076[322] = 0; + out_7282145086155387076[323] = 0; + out_7282145086155387076[324] = 0; + out_7282145086155387076[325] = 0; + out_7282145086155387076[326] = 0; + out_7282145086155387076[327] = 0; + out_7282145086155387076[328] = 0; + out_7282145086155387076[329] = 0; + out_7282145086155387076[330] = 1; + out_7282145086155387076[331] = 0; + out_7282145086155387076[332] = 0; + out_7282145086155387076[333] = 0; + out_7282145086155387076[334] = 0; + out_7282145086155387076[335] = 0; + out_7282145086155387076[336] = 0; + out_7282145086155387076[337] = 0; + out_7282145086155387076[338] = 0; + out_7282145086155387076[339] = 0; + out_7282145086155387076[340] = 0; + out_7282145086155387076[341] = 0; + out_7282145086155387076[342] = 0; + out_7282145086155387076[343] = 0; + out_7282145086155387076[344] = 0; + out_7282145086155387076[345] = 0; + out_7282145086155387076[346] = 0; + out_7282145086155387076[347] = 0; + out_7282145086155387076[348] = 0; + out_7282145086155387076[349] = 0; + out_7282145086155387076[350] = 0; + out_7282145086155387076[351] = 0; + out_7282145086155387076[352] = 1; + out_7282145086155387076[353] = 0; + out_7282145086155387076[354] = 0; + out_7282145086155387076[355] = 0; + out_7282145086155387076[356] = 0; + out_7282145086155387076[357] = 0; + out_7282145086155387076[358] = 0; + out_7282145086155387076[359] = 0; + out_7282145086155387076[360] = 0; + out_7282145086155387076[361] = 0; + out_7282145086155387076[362] = 0; + out_7282145086155387076[363] = 0; + out_7282145086155387076[364] = 0; + out_7282145086155387076[365] = 0; + out_7282145086155387076[366] = 0; + out_7282145086155387076[367] = 0; + out_7282145086155387076[368] = 0; + out_7282145086155387076[369] = 0; + out_7282145086155387076[370] = 0; + out_7282145086155387076[371] = 0; + out_7282145086155387076[372] = 0; + out_7282145086155387076[373] = 0; + out_7282145086155387076[374] = 1; + out_7282145086155387076[375] = 0; + out_7282145086155387076[376] = 0; + out_7282145086155387076[377] = 0; + out_7282145086155387076[378] = 0; + out_7282145086155387076[379] = 0; + out_7282145086155387076[380] = 0; + out_7282145086155387076[381] = 0; + out_7282145086155387076[382] = 0; + out_7282145086155387076[383] = 0; + out_7282145086155387076[384] = 0; + out_7282145086155387076[385] = 0; + out_7282145086155387076[386] = 0; + out_7282145086155387076[387] = 0; + out_7282145086155387076[388] = 0; + out_7282145086155387076[389] = 0; + out_7282145086155387076[390] = 0; + out_7282145086155387076[391] = 0; + out_7282145086155387076[392] = 0; + out_7282145086155387076[393] = 0; + out_7282145086155387076[394] = 0; + out_7282145086155387076[395] = 0; + out_7282145086155387076[396] = 1; + out_7282145086155387076[397] = 0; + out_7282145086155387076[398] = 0; + out_7282145086155387076[399] = 0; + out_7282145086155387076[400] = 0; + out_7282145086155387076[401] = 0; + out_7282145086155387076[402] = 0; + out_7282145086155387076[403] = 0; + out_7282145086155387076[404] = 0; + out_7282145086155387076[405] = 0; + out_7282145086155387076[406] = 0; + out_7282145086155387076[407] = 0; + out_7282145086155387076[408] = 0; + out_7282145086155387076[409] = 0; + out_7282145086155387076[410] = 0; + out_7282145086155387076[411] = 0; + out_7282145086155387076[412] = 0; + out_7282145086155387076[413] = 0; + out_7282145086155387076[414] = 0; + out_7282145086155387076[415] = 0; + out_7282145086155387076[416] = 0; + out_7282145086155387076[417] = 0; + out_7282145086155387076[418] = 1; + out_7282145086155387076[419] = 0; + out_7282145086155387076[420] = 0; + out_7282145086155387076[421] = 0; + out_7282145086155387076[422] = 0; + out_7282145086155387076[423] = 0; + out_7282145086155387076[424] = 0; + out_7282145086155387076[425] = 0; + out_7282145086155387076[426] = 0; + out_7282145086155387076[427] = 0; + out_7282145086155387076[428] = 0; + out_7282145086155387076[429] = 0; + out_7282145086155387076[430] = 0; + out_7282145086155387076[431] = 0; + out_7282145086155387076[432] = 0; + out_7282145086155387076[433] = 0; + out_7282145086155387076[434] = 0; + out_7282145086155387076[435] = 0; + out_7282145086155387076[436] = 0; + out_7282145086155387076[437] = 0; + out_7282145086155387076[438] = 0; + out_7282145086155387076[439] = 0; + out_7282145086155387076[440] = 1; } -void h_4(double *state, double *unused, double *out_855318047403330363) { - out_855318047403330363[0] = state[10] + state[13]; - out_855318047403330363[1] = state[11] + state[14]; - out_855318047403330363[2] = state[12] + state[15]; +void h_4(double *state, double *unused, double *out_3936704980047894152) { + out_3936704980047894152[0] = state[10] + state[13]; + out_3936704980047894152[1] = state[11] + state[14]; + out_3936704980047894152[2] = state[12] + state[15]; } -void H_4(double *state, double *unused, double *out_7229846732012737030) { - out_7229846732012737030[0] = 0; - out_7229846732012737030[1] = 0; - out_7229846732012737030[2] = 0; - out_7229846732012737030[3] = 0; - out_7229846732012737030[4] = 0; - out_7229846732012737030[5] = 0; - out_7229846732012737030[6] = 0; - out_7229846732012737030[7] = 0; - out_7229846732012737030[8] = 0; - out_7229846732012737030[9] = 0; - out_7229846732012737030[10] = 1; - out_7229846732012737030[11] = 0; - out_7229846732012737030[12] = 0; - out_7229846732012737030[13] = 1; - out_7229846732012737030[14] = 0; - out_7229846732012737030[15] = 0; - out_7229846732012737030[16] = 0; - out_7229846732012737030[17] = 0; - out_7229846732012737030[18] = 0; - out_7229846732012737030[19] = 0; - out_7229846732012737030[20] = 0; - out_7229846732012737030[21] = 0; - out_7229846732012737030[22] = 0; - out_7229846732012737030[23] = 0; - out_7229846732012737030[24] = 0; - out_7229846732012737030[25] = 0; - out_7229846732012737030[26] = 0; - out_7229846732012737030[27] = 0; - out_7229846732012737030[28] = 0; - out_7229846732012737030[29] = 0; - out_7229846732012737030[30] = 0; - out_7229846732012737030[31] = 0; - out_7229846732012737030[32] = 0; - out_7229846732012737030[33] = 1; - out_7229846732012737030[34] = 0; - out_7229846732012737030[35] = 0; - out_7229846732012737030[36] = 1; - out_7229846732012737030[37] = 0; - out_7229846732012737030[38] = 0; - out_7229846732012737030[39] = 0; - out_7229846732012737030[40] = 0; - out_7229846732012737030[41] = 0; - out_7229846732012737030[42] = 0; - out_7229846732012737030[43] = 0; - out_7229846732012737030[44] = 0; - out_7229846732012737030[45] = 0; - out_7229846732012737030[46] = 0; - out_7229846732012737030[47] = 0; - out_7229846732012737030[48] = 0; - out_7229846732012737030[49] = 0; - out_7229846732012737030[50] = 0; - out_7229846732012737030[51] = 0; - out_7229846732012737030[52] = 0; - out_7229846732012737030[53] = 0; - out_7229846732012737030[54] = 0; - out_7229846732012737030[55] = 0; - out_7229846732012737030[56] = 1; - out_7229846732012737030[57] = 0; - out_7229846732012737030[58] = 0; - out_7229846732012737030[59] = 1; - out_7229846732012737030[60] = 0; - out_7229846732012737030[61] = 0; - out_7229846732012737030[62] = 0; - out_7229846732012737030[63] = 0; - out_7229846732012737030[64] = 0; - out_7229846732012737030[65] = 0; +void H_4(double *state, double *unused, double *out_9052514604414786102) { + out_9052514604414786102[0] = 0; + out_9052514604414786102[1] = 0; + out_9052514604414786102[2] = 0; + out_9052514604414786102[3] = 0; + out_9052514604414786102[4] = 0; + out_9052514604414786102[5] = 0; + out_9052514604414786102[6] = 0; + out_9052514604414786102[7] = 0; + out_9052514604414786102[8] = 0; + out_9052514604414786102[9] = 0; + out_9052514604414786102[10] = 1; + out_9052514604414786102[11] = 0; + out_9052514604414786102[12] = 0; + out_9052514604414786102[13] = 1; + out_9052514604414786102[14] = 0; + out_9052514604414786102[15] = 0; + out_9052514604414786102[16] = 0; + out_9052514604414786102[17] = 0; + out_9052514604414786102[18] = 0; + out_9052514604414786102[19] = 0; + out_9052514604414786102[20] = 0; + out_9052514604414786102[21] = 0; + out_9052514604414786102[22] = 0; + out_9052514604414786102[23] = 0; + out_9052514604414786102[24] = 0; + out_9052514604414786102[25] = 0; + out_9052514604414786102[26] = 0; + out_9052514604414786102[27] = 0; + out_9052514604414786102[28] = 0; + out_9052514604414786102[29] = 0; + out_9052514604414786102[30] = 0; + out_9052514604414786102[31] = 0; + out_9052514604414786102[32] = 0; + out_9052514604414786102[33] = 1; + out_9052514604414786102[34] = 0; + out_9052514604414786102[35] = 0; + out_9052514604414786102[36] = 1; + out_9052514604414786102[37] = 0; + out_9052514604414786102[38] = 0; + out_9052514604414786102[39] = 0; + out_9052514604414786102[40] = 0; + out_9052514604414786102[41] = 0; + out_9052514604414786102[42] = 0; + out_9052514604414786102[43] = 0; + out_9052514604414786102[44] = 0; + out_9052514604414786102[45] = 0; + out_9052514604414786102[46] = 0; + out_9052514604414786102[47] = 0; + out_9052514604414786102[48] = 0; + out_9052514604414786102[49] = 0; + out_9052514604414786102[50] = 0; + out_9052514604414786102[51] = 0; + out_9052514604414786102[52] = 0; + out_9052514604414786102[53] = 0; + out_9052514604414786102[54] = 0; + out_9052514604414786102[55] = 0; + out_9052514604414786102[56] = 1; + out_9052514604414786102[57] = 0; + out_9052514604414786102[58] = 0; + out_9052514604414786102[59] = 1; + out_9052514604414786102[60] = 0; + out_9052514604414786102[61] = 0; + out_9052514604414786102[62] = 0; + out_9052514604414786102[63] = 0; + out_9052514604414786102[64] = 0; + out_9052514604414786102[65] = 0; } -void h_9(double *state, double *unused, double *out_2075713419116157705) { - out_2075713419116157705[0] = state[10]; - out_2075713419116157705[1] = state[11]; - out_2075713419116157705[2] = state[12]; +void h_9(double *state, double *unused, double *out_4670376777294811307) { + out_4670376777294811307[0] = state[10]; + out_4670376777294811307[1] = state[11]; + out_4670376777294811307[2] = state[12]; } -void H_9(double *state, double *unused, double *out_4340985179732657688) { - out_4340985179732657688[0] = 0; - out_4340985179732657688[1] = 0; - out_4340985179732657688[2] = 0; - out_4340985179732657688[3] = 0; - out_4340985179732657688[4] = 0; - out_4340985179732657688[5] = 0; - out_4340985179732657688[6] = 0; - out_4340985179732657688[7] = 0; - out_4340985179732657688[8] = 0; - out_4340985179732657688[9] = 0; - out_4340985179732657688[10] = 1; - out_4340985179732657688[11] = 0; - out_4340985179732657688[12] = 0; - out_4340985179732657688[13] = 0; - out_4340985179732657688[14] = 0; - out_4340985179732657688[15] = 0; - out_4340985179732657688[16] = 0; - out_4340985179732657688[17] = 0; - out_4340985179732657688[18] = 0; - out_4340985179732657688[19] = 0; - out_4340985179732657688[20] = 0; - out_4340985179732657688[21] = 0; - out_4340985179732657688[22] = 0; - out_4340985179732657688[23] = 0; - out_4340985179732657688[24] = 0; - out_4340985179732657688[25] = 0; - out_4340985179732657688[26] = 0; - out_4340985179732657688[27] = 0; - out_4340985179732657688[28] = 0; - out_4340985179732657688[29] = 0; - out_4340985179732657688[30] = 0; - out_4340985179732657688[31] = 0; - out_4340985179732657688[32] = 0; - out_4340985179732657688[33] = 1; - out_4340985179732657688[34] = 0; - out_4340985179732657688[35] = 0; - out_4340985179732657688[36] = 0; - out_4340985179732657688[37] = 0; - out_4340985179732657688[38] = 0; - out_4340985179732657688[39] = 0; - out_4340985179732657688[40] = 0; - out_4340985179732657688[41] = 0; - out_4340985179732657688[42] = 0; - out_4340985179732657688[43] = 0; - out_4340985179732657688[44] = 0; - out_4340985179732657688[45] = 0; - out_4340985179732657688[46] = 0; - out_4340985179732657688[47] = 0; - out_4340985179732657688[48] = 0; - out_4340985179732657688[49] = 0; - out_4340985179732657688[50] = 0; - out_4340985179732657688[51] = 0; - out_4340985179732657688[52] = 0; - out_4340985179732657688[53] = 0; - out_4340985179732657688[54] = 0; - out_4340985179732657688[55] = 0; - out_4340985179732657688[56] = 1; - out_4340985179732657688[57] = 0; - out_4340985179732657688[58] = 0; - out_4340985179732657688[59] = 0; - out_4340985179732657688[60] = 0; - out_4340985179732657688[61] = 0; - out_4340985179732657688[62] = 0; - out_4340985179732657688[63] = 0; - out_4340985179732657688[64] = 0; - out_4340985179732657688[65] = 0; +void H_9(double *state, double *unused, double *out_9153039822665174869) { + out_9153039822665174869[0] = 0; + out_9153039822665174869[1] = 0; + out_9153039822665174869[2] = 0; + out_9153039822665174869[3] = 0; + out_9153039822665174869[4] = 0; + out_9153039822665174869[5] = 0; + out_9153039822665174869[6] = 0; + out_9153039822665174869[7] = 0; + out_9153039822665174869[8] = 0; + out_9153039822665174869[9] = 0; + out_9153039822665174869[10] = 1; + out_9153039822665174869[11] = 0; + out_9153039822665174869[12] = 0; + out_9153039822665174869[13] = 0; + out_9153039822665174869[14] = 0; + out_9153039822665174869[15] = 0; + out_9153039822665174869[16] = 0; + out_9153039822665174869[17] = 0; + out_9153039822665174869[18] = 0; + out_9153039822665174869[19] = 0; + out_9153039822665174869[20] = 0; + out_9153039822665174869[21] = 0; + out_9153039822665174869[22] = 0; + out_9153039822665174869[23] = 0; + out_9153039822665174869[24] = 0; + out_9153039822665174869[25] = 0; + out_9153039822665174869[26] = 0; + out_9153039822665174869[27] = 0; + out_9153039822665174869[28] = 0; + out_9153039822665174869[29] = 0; + out_9153039822665174869[30] = 0; + out_9153039822665174869[31] = 0; + out_9153039822665174869[32] = 0; + out_9153039822665174869[33] = 1; + out_9153039822665174869[34] = 0; + out_9153039822665174869[35] = 0; + out_9153039822665174869[36] = 0; + out_9153039822665174869[37] = 0; + out_9153039822665174869[38] = 0; + out_9153039822665174869[39] = 0; + out_9153039822665174869[40] = 0; + out_9153039822665174869[41] = 0; + out_9153039822665174869[42] = 0; + out_9153039822665174869[43] = 0; + out_9153039822665174869[44] = 0; + out_9153039822665174869[45] = 0; + out_9153039822665174869[46] = 0; + out_9153039822665174869[47] = 0; + out_9153039822665174869[48] = 0; + out_9153039822665174869[49] = 0; + out_9153039822665174869[50] = 0; + out_9153039822665174869[51] = 0; + out_9153039822665174869[52] = 0; + out_9153039822665174869[53] = 0; + out_9153039822665174869[54] = 0; + out_9153039822665174869[55] = 0; + out_9153039822665174869[56] = 1; + out_9153039822665174869[57] = 0; + out_9153039822665174869[58] = 0; + out_9153039822665174869[59] = 0; + out_9153039822665174869[60] = 0; + out_9153039822665174869[61] = 0; + out_9153039822665174869[62] = 0; + out_9153039822665174869[63] = 0; + out_9153039822665174869[64] = 0; + out_9153039822665174869[65] = 0; } -void h_10(double *state, double *unused, double *out_4871234396170298071) { - out_4871234396170298071[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; - out_4871234396170298071[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; - out_4871234396170298071[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; +void h_10(double *state, double *unused, double *out_5114531167885874097) { + out_5114531167885874097[0] = 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0] + state[16] + state[19]; + out_5114531167885874097[1] = 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2] + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1] + state[17] + state[20]; + out_5114531167885874097[2] = 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1] + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0] + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[2] + state[18] + state[21]; } -void H_10(double *state, double *unused, double *out_6664980085443225062) { - out_6664980085443225062[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); - out_6664980085443225062[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_6664980085443225062[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; - out_6664980085443225062[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_6664980085443225062[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_6664980085443225062[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_6664980085443225062[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_6664980085443225062[7] = 0; - out_6664980085443225062[8] = 0; - out_6664980085443225062[9] = 0; - out_6664980085443225062[10] = 0; - out_6664980085443225062[11] = 0; - out_6664980085443225062[12] = 0; - out_6664980085443225062[13] = 0; - out_6664980085443225062[14] = 0; - out_6664980085443225062[15] = 0; - out_6664980085443225062[16] = 1; - out_6664980085443225062[17] = 0; - out_6664980085443225062[18] = 0; - out_6664980085443225062[19] = 1; - out_6664980085443225062[20] = 0; - out_6664980085443225062[21] = 0; - out_6664980085443225062[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; - out_6664980085443225062[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); - out_6664980085443225062[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; - out_6664980085443225062[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_6664980085443225062[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_6664980085443225062[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_6664980085443225062[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_6664980085443225062[29] = 0; - out_6664980085443225062[30] = 0; - out_6664980085443225062[31] = 0; - out_6664980085443225062[32] = 0; - out_6664980085443225062[33] = 0; - out_6664980085443225062[34] = 0; - out_6664980085443225062[35] = 0; - out_6664980085443225062[36] = 0; - out_6664980085443225062[37] = 0; - out_6664980085443225062[38] = 0; - out_6664980085443225062[39] = 1; - out_6664980085443225062[40] = 0; - out_6664980085443225062[41] = 0; - out_6664980085443225062[42] = 1; - out_6664980085443225062[43] = 0; - out_6664980085443225062[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; - out_6664980085443225062[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; - out_6664980085443225062[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); - out_6664980085443225062[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; - out_6664980085443225062[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; - out_6664980085443225062[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; - out_6664980085443225062[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; - out_6664980085443225062[51] = 0; - out_6664980085443225062[52] = 0; - out_6664980085443225062[53] = 0; - out_6664980085443225062[54] = 0; - out_6664980085443225062[55] = 0; - out_6664980085443225062[56] = 0; - out_6664980085443225062[57] = 0; - out_6664980085443225062[58] = 0; - out_6664980085443225062[59] = 0; - out_6664980085443225062[60] = 0; - out_6664980085443225062[61] = 0; - out_6664980085443225062[62] = 1; - out_6664980085443225062[63] = 0; - out_6664980085443225062[64] = 0; - out_6664980085443225062[65] = 1; +void H_10(double *state, double *unused, double *out_8901083693322163334) { + out_8901083693322163334[0] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*pow(state[0], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2)); + out_8901083693322163334[1] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; + out_8901083693322163334[2] = -1195801500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(-2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[0]*state[2]; + out_8901083693322163334[3] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_8901083693322163334[4] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_8901083693322163334[5] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_8901083693322163334[6] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_8901083693322163334[7] = 0; + out_8901083693322163334[8] = 0; + out_8901083693322163334[9] = 0; + out_8901083693322163334[10] = 0; + out_8901083693322163334[11] = 0; + out_8901083693322163334[12] = 0; + out_8901083693322163334[13] = 0; + out_8901083693322163334[14] = 0; + out_8901083693322163334[15] = 0; + out_8901083693322163334[16] = 1; + out_8901083693322163334[17] = 0; + out_8901083693322163334[18] = 0; + out_8901083693322163334[19] = 1; + out_8901083693322163334[20] = 0; + out_8901083693322163334[21] = 0; + out_8901083693322163334[22] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[0]*state[1]; + out_8901083693322163334[23] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*pow(state[1], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2)); + out_8901083693322163334[24] = -1195801500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[2], 2) + 398600500000000.0*(2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(-2*state[3]*state[6] + 2*state[4]*state[5])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[1]*state[2]; + out_8901083693322163334[25] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_8901083693322163334[26] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_8901083693322163334[27] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_8901083693322163334[28] = -797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_8901083693322163334[29] = 0; + out_8901083693322163334[30] = 0; + out_8901083693322163334[31] = 0; + out_8901083693322163334[32] = 0; + out_8901083693322163334[33] = 0; + out_8901083693322163334[34] = 0; + out_8901083693322163334[35] = 0; + out_8901083693322163334[36] = 0; + out_8901083693322163334[37] = 0; + out_8901083693322163334[38] = 0; + out_8901083693322163334[39] = 1; + out_8901083693322163334[40] = 0; + out_8901083693322163334[41] = 0; + out_8901083693322163334[42] = 1; + out_8901083693322163334[43] = 0; + out_8901083693322163334[44] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[0], 2) + 398600500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[0]*state[2]; + out_8901083693322163334[45] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*pow(state[1], 2) + 398600500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5) - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[1] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[1]*state[2]; + out_8901083693322163334[46] = -1195801500000000.0*(-2*state[3]*state[4] + 2*state[5]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[1]*state[2] - 1195801500000000.0*(2*state[3]*state[5] + 2*state[4]*state[6])*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*state[0]*state[2] - 1195801500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -2.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*pow(state[2], 2) + 398600500000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*(pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2)); + out_8901083693322163334[47] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[5] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[3]; + out_8901083693322163334[48] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[3] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[4]; + out_8901083693322163334[49] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[3] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[6] - 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[5]; + out_8901083693322163334[50] = 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[0]*state[4] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[1]*state[5] + 797201000000000.0*pow(pow(state[0], 2) + pow(state[1], 2) + pow(state[2], 2), -1.5)*state[2]*state[6]; + out_8901083693322163334[51] = 0; + out_8901083693322163334[52] = 0; + out_8901083693322163334[53] = 0; + out_8901083693322163334[54] = 0; + out_8901083693322163334[55] = 0; + out_8901083693322163334[56] = 0; + out_8901083693322163334[57] = 0; + out_8901083693322163334[58] = 0; + out_8901083693322163334[59] = 0; + out_8901083693322163334[60] = 0; + out_8901083693322163334[61] = 0; + out_8901083693322163334[62] = 1; + out_8901083693322163334[63] = 0; + out_8901083693322163334[64] = 0; + out_8901083693322163334[65] = 1; } -void h_12(double *state, double *unused, double *out_59445924731046944) { - out_59445924731046944[0] = state[0]; - out_59445924731046944[1] = state[1]; - out_59445924731046944[2] = state[2]; +void h_12(double *state, double *unused, double *out_6249225679190641860) { + out_6249225679190641860[0] = state[0]; + out_6249225679190641860[1] = state[1]; + out_6249225679190641860[2] = state[2]; } -void H_12(double *state, double *unused, double *out_437281581669713462) { - out_437281581669713462[0] = 1; - out_437281581669713462[1] = 0; - out_437281581669713462[2] = 0; - out_437281581669713462[3] = 0; - out_437281581669713462[4] = 0; - out_437281581669713462[5] = 0; - out_437281581669713462[6] = 0; - out_437281581669713462[7] = 0; - out_437281581669713462[8] = 0; - out_437281581669713462[9] = 0; - out_437281581669713462[10] = 0; - out_437281581669713462[11] = 0; - out_437281581669713462[12] = 0; - out_437281581669713462[13] = 0; - out_437281581669713462[14] = 0; - out_437281581669713462[15] = 0; - out_437281581669713462[16] = 0; - out_437281581669713462[17] = 0; - out_437281581669713462[18] = 0; - out_437281581669713462[19] = 0; - out_437281581669713462[20] = 0; - out_437281581669713462[21] = 0; - out_437281581669713462[22] = 0; - out_437281581669713462[23] = 1; - out_437281581669713462[24] = 0; - out_437281581669713462[25] = 0; - out_437281581669713462[26] = 0; - out_437281581669713462[27] = 0; - out_437281581669713462[28] = 0; - out_437281581669713462[29] = 0; - out_437281581669713462[30] = 0; - out_437281581669713462[31] = 0; - out_437281581669713462[32] = 0; - out_437281581669713462[33] = 0; - out_437281581669713462[34] = 0; - out_437281581669713462[35] = 0; - out_437281581669713462[36] = 0; - out_437281581669713462[37] = 0; - out_437281581669713462[38] = 0; - out_437281581669713462[39] = 0; - out_437281581669713462[40] = 0; - out_437281581669713462[41] = 0; - out_437281581669713462[42] = 0; - out_437281581669713462[43] = 0; - out_437281581669713462[44] = 0; - out_437281581669713462[45] = 0; - out_437281581669713462[46] = 1; - out_437281581669713462[47] = 0; - out_437281581669713462[48] = 0; - out_437281581669713462[49] = 0; - out_437281581669713462[50] = 0; - out_437281581669713462[51] = 0; - out_437281581669713462[52] = 0; - out_437281581669713462[53] = 0; - out_437281581669713462[54] = 0; - out_437281581669713462[55] = 0; - out_437281581669713462[56] = 0; - out_437281581669713462[57] = 0; - out_437281581669713462[58] = 0; - out_437281581669713462[59] = 0; - out_437281581669713462[60] = 0; - out_437281581669713462[61] = 0; - out_437281581669713462[62] = 0; - out_437281581669713462[63] = 0; - out_437281581669713462[64] = 0; - out_437281581669713462[65] = 0; +void H_12(double *state, double *unused, double *out_4374773061262803719) { + out_4374773061262803719[0] = 1; + out_4374773061262803719[1] = 0; + out_4374773061262803719[2] = 0; + out_4374773061262803719[3] = 0; + out_4374773061262803719[4] = 0; + out_4374773061262803719[5] = 0; + out_4374773061262803719[6] = 0; + out_4374773061262803719[7] = 0; + out_4374773061262803719[8] = 0; + out_4374773061262803719[9] = 0; + out_4374773061262803719[10] = 0; + out_4374773061262803719[11] = 0; + out_4374773061262803719[12] = 0; + out_4374773061262803719[13] = 0; + out_4374773061262803719[14] = 0; + out_4374773061262803719[15] = 0; + out_4374773061262803719[16] = 0; + out_4374773061262803719[17] = 0; + out_4374773061262803719[18] = 0; + out_4374773061262803719[19] = 0; + out_4374773061262803719[20] = 0; + out_4374773061262803719[21] = 0; + out_4374773061262803719[22] = 0; + out_4374773061262803719[23] = 1; + out_4374773061262803719[24] = 0; + out_4374773061262803719[25] = 0; + out_4374773061262803719[26] = 0; + out_4374773061262803719[27] = 0; + out_4374773061262803719[28] = 0; + out_4374773061262803719[29] = 0; + out_4374773061262803719[30] = 0; + out_4374773061262803719[31] = 0; + out_4374773061262803719[32] = 0; + out_4374773061262803719[33] = 0; + out_4374773061262803719[34] = 0; + out_4374773061262803719[35] = 0; + out_4374773061262803719[36] = 0; + out_4374773061262803719[37] = 0; + out_4374773061262803719[38] = 0; + out_4374773061262803719[39] = 0; + out_4374773061262803719[40] = 0; + out_4374773061262803719[41] = 0; + out_4374773061262803719[42] = 0; + out_4374773061262803719[43] = 0; + out_4374773061262803719[44] = 0; + out_4374773061262803719[45] = 0; + out_4374773061262803719[46] = 1; + out_4374773061262803719[47] = 0; + out_4374773061262803719[48] = 0; + out_4374773061262803719[49] = 0; + out_4374773061262803719[50] = 0; + out_4374773061262803719[51] = 0; + out_4374773061262803719[52] = 0; + out_4374773061262803719[53] = 0; + out_4374773061262803719[54] = 0; + out_4374773061262803719[55] = 0; + out_4374773061262803719[56] = 0; + out_4374773061262803719[57] = 0; + out_4374773061262803719[58] = 0; + out_4374773061262803719[59] = 0; + out_4374773061262803719[60] = 0; + out_4374773061262803719[61] = 0; + out_4374773061262803719[62] = 0; + out_4374773061262803719[63] = 0; + out_4374773061262803719[64] = 0; + out_4374773061262803719[65] = 0; } -void h_35(double *state, double *unused, double *out_5470579135920011702) { - out_5470579135920011702[0] = state[7]; - out_5470579135920011702[1] = state[8]; - out_5470579135920011702[2] = state[9]; +void h_35(double *state, double *unused, double *out_5751710703471722702) { + out_5751710703471722702[0] = state[7]; + out_5751710703471722702[1] = state[8]; + out_5751710703471722702[2] = state[9]; } -void H_35(double *state, double *unused, double *out_3863184674640129654) { - out_3863184674640129654[0] = 0; - out_3863184674640129654[1] = 0; - out_3863184674640129654[2] = 0; - out_3863184674640129654[3] = 0; - out_3863184674640129654[4] = 0; - out_3863184674640129654[5] = 0; - out_3863184674640129654[6] = 0; - out_3863184674640129654[7] = 1; - out_3863184674640129654[8] = 0; - out_3863184674640129654[9] = 0; - out_3863184674640129654[10] = 0; - out_3863184674640129654[11] = 0; - out_3863184674640129654[12] = 0; - out_3863184674640129654[13] = 0; - out_3863184674640129654[14] = 0; - out_3863184674640129654[15] = 0; - out_3863184674640129654[16] = 0; - out_3863184674640129654[17] = 0; - out_3863184674640129654[18] = 0; - out_3863184674640129654[19] = 0; - out_3863184674640129654[20] = 0; - out_3863184674640129654[21] = 0; - out_3863184674640129654[22] = 0; - out_3863184674640129654[23] = 0; - out_3863184674640129654[24] = 0; - out_3863184674640129654[25] = 0; - out_3863184674640129654[26] = 0; - out_3863184674640129654[27] = 0; - out_3863184674640129654[28] = 0; - out_3863184674640129654[29] = 0; - out_3863184674640129654[30] = 1; - out_3863184674640129654[31] = 0; - out_3863184674640129654[32] = 0; - out_3863184674640129654[33] = 0; - out_3863184674640129654[34] = 0; - out_3863184674640129654[35] = 0; - out_3863184674640129654[36] = 0; - out_3863184674640129654[37] = 0; - out_3863184674640129654[38] = 0; - out_3863184674640129654[39] = 0; - out_3863184674640129654[40] = 0; - out_3863184674640129654[41] = 0; - out_3863184674640129654[42] = 0; - out_3863184674640129654[43] = 0; - out_3863184674640129654[44] = 0; - out_3863184674640129654[45] = 0; - out_3863184674640129654[46] = 0; - out_3863184674640129654[47] = 0; - out_3863184674640129654[48] = 0; - out_3863184674640129654[49] = 0; - out_3863184674640129654[50] = 0; - out_3863184674640129654[51] = 0; - out_3863184674640129654[52] = 0; - out_3863184674640129654[53] = 1; - out_3863184674640129654[54] = 0; - out_3863184674640129654[55] = 0; - out_3863184674640129654[56] = 0; - out_3863184674640129654[57] = 0; - out_3863184674640129654[58] = 0; - out_3863184674640129654[59] = 0; - out_3863184674640129654[60] = 0; - out_3863184674640129654[61] = 0; - out_3863184674640129654[62] = 0; - out_3863184674640129654[63] = 0; - out_3863184674640129654[64] = 0; - out_3863184674640129654[65] = 0; +void H_35(double *state, double *unused, double *out_1629210028937790010) { + out_1629210028937790010[0] = 0; + out_1629210028937790010[1] = 0; + out_1629210028937790010[2] = 0; + out_1629210028937790010[3] = 0; + out_1629210028937790010[4] = 0; + out_1629210028937790010[5] = 0; + out_1629210028937790010[6] = 0; + out_1629210028937790010[7] = 1; + out_1629210028937790010[8] = 0; + out_1629210028937790010[9] = 0; + out_1629210028937790010[10] = 0; + out_1629210028937790010[11] = 0; + out_1629210028937790010[12] = 0; + out_1629210028937790010[13] = 0; + out_1629210028937790010[14] = 0; + out_1629210028937790010[15] = 0; + out_1629210028937790010[16] = 0; + out_1629210028937790010[17] = 0; + out_1629210028937790010[18] = 0; + out_1629210028937790010[19] = 0; + out_1629210028937790010[20] = 0; + out_1629210028937790010[21] = 0; + out_1629210028937790010[22] = 0; + out_1629210028937790010[23] = 0; + out_1629210028937790010[24] = 0; + out_1629210028937790010[25] = 0; + out_1629210028937790010[26] = 0; + out_1629210028937790010[27] = 0; + out_1629210028937790010[28] = 0; + out_1629210028937790010[29] = 0; + out_1629210028937790010[30] = 1; + out_1629210028937790010[31] = 0; + out_1629210028937790010[32] = 0; + out_1629210028937790010[33] = 0; + out_1629210028937790010[34] = 0; + out_1629210028937790010[35] = 0; + out_1629210028937790010[36] = 0; + out_1629210028937790010[37] = 0; + out_1629210028937790010[38] = 0; + out_1629210028937790010[39] = 0; + out_1629210028937790010[40] = 0; + out_1629210028937790010[41] = 0; + out_1629210028937790010[42] = 0; + out_1629210028937790010[43] = 0; + out_1629210028937790010[44] = 0; + out_1629210028937790010[45] = 0; + out_1629210028937790010[46] = 0; + out_1629210028937790010[47] = 0; + out_1629210028937790010[48] = 0; + out_1629210028937790010[49] = 0; + out_1629210028937790010[50] = 0; + out_1629210028937790010[51] = 0; + out_1629210028937790010[52] = 0; + out_1629210028937790010[53] = 1; + out_1629210028937790010[54] = 0; + out_1629210028937790010[55] = 0; + out_1629210028937790010[56] = 0; + out_1629210028937790010[57] = 0; + out_1629210028937790010[58] = 0; + out_1629210028937790010[59] = 0; + out_1629210028937790010[60] = 0; + out_1629210028937790010[61] = 0; + out_1629210028937790010[62] = 0; + out_1629210028937790010[63] = 0; + out_1629210028937790010[64] = 0; + out_1629210028937790010[65] = 0; } -void h_32(double *state, double *unused, double *out_61541883826926771) { - out_61541883826926771[0] = state[3]; - out_61541883826926771[1] = state[4]; - out_61541883826926771[2] = state[5]; - out_61541883826926771[3] = state[6]; +void h_32(double *state, double *unused, double *out_7960850640984836266) { + out_7960850640984836266[0] = state[3]; + out_7960850640984836266[1] = state[4]; + out_7960850640984836266[2] = state[5]; + out_7960850640984836266[3] = state[6]; } -void H_32(double *state, double *unused, double *out_4156829226020876432) { - out_4156829226020876432[0] = 0; - out_4156829226020876432[1] = 0; - out_4156829226020876432[2] = 0; - out_4156829226020876432[3] = 1; - out_4156829226020876432[4] = 0; - out_4156829226020876432[5] = 0; - out_4156829226020876432[6] = 0; - out_4156829226020876432[7] = 0; - out_4156829226020876432[8] = 0; - out_4156829226020876432[9] = 0; - out_4156829226020876432[10] = 0; - out_4156829226020876432[11] = 0; - out_4156829226020876432[12] = 0; - out_4156829226020876432[13] = 0; - out_4156829226020876432[14] = 0; - out_4156829226020876432[15] = 0; - out_4156829226020876432[16] = 0; - out_4156829226020876432[17] = 0; - out_4156829226020876432[18] = 0; - out_4156829226020876432[19] = 0; - out_4156829226020876432[20] = 0; - out_4156829226020876432[21] = 0; - out_4156829226020876432[22] = 0; - out_4156829226020876432[23] = 0; - out_4156829226020876432[24] = 0; - out_4156829226020876432[25] = 0; - out_4156829226020876432[26] = 1; - out_4156829226020876432[27] = 0; - out_4156829226020876432[28] = 0; - out_4156829226020876432[29] = 0; - out_4156829226020876432[30] = 0; - out_4156829226020876432[31] = 0; - out_4156829226020876432[32] = 0; - out_4156829226020876432[33] = 0; - out_4156829226020876432[34] = 0; - out_4156829226020876432[35] = 0; - out_4156829226020876432[36] = 0; - out_4156829226020876432[37] = 0; - out_4156829226020876432[38] = 0; - out_4156829226020876432[39] = 0; - out_4156829226020876432[40] = 0; - out_4156829226020876432[41] = 0; - out_4156829226020876432[42] = 0; - out_4156829226020876432[43] = 0; - out_4156829226020876432[44] = 0; - out_4156829226020876432[45] = 0; - out_4156829226020876432[46] = 0; - out_4156829226020876432[47] = 0; - out_4156829226020876432[48] = 0; - out_4156829226020876432[49] = 1; - out_4156829226020876432[50] = 0; - out_4156829226020876432[51] = 0; - out_4156829226020876432[52] = 0; - out_4156829226020876432[53] = 0; - out_4156829226020876432[54] = 0; - out_4156829226020876432[55] = 0; - out_4156829226020876432[56] = 0; - out_4156829226020876432[57] = 0; - out_4156829226020876432[58] = 0; - out_4156829226020876432[59] = 0; - out_4156829226020876432[60] = 0; - out_4156829226020876432[61] = 0; - out_4156829226020876432[62] = 0; - out_4156829226020876432[63] = 0; - out_4156829226020876432[64] = 0; - out_4156829226020876432[65] = 0; - out_4156829226020876432[66] = 0; - out_4156829226020876432[67] = 0; - out_4156829226020876432[68] = 0; - out_4156829226020876432[69] = 0; - out_4156829226020876432[70] = 0; - out_4156829226020876432[71] = 0; - out_4156829226020876432[72] = 1; - out_4156829226020876432[73] = 0; - out_4156829226020876432[74] = 0; - out_4156829226020876432[75] = 0; - out_4156829226020876432[76] = 0; - out_4156829226020876432[77] = 0; - out_4156829226020876432[78] = 0; - out_4156829226020876432[79] = 0; - out_4156829226020876432[80] = 0; - out_4156829226020876432[81] = 0; - out_4156829226020876432[82] = 0; - out_4156829226020876432[83] = 0; - out_4156829226020876432[84] = 0; - out_4156829226020876432[85] = 0; - out_4156829226020876432[86] = 0; - out_4156829226020876432[87] = 0; +void H_32(double *state, double *unused, double *out_8200477424115484408) { + out_8200477424115484408[0] = 0; + out_8200477424115484408[1] = 0; + out_8200477424115484408[2] = 0; + out_8200477424115484408[3] = 1; + out_8200477424115484408[4] = 0; + out_8200477424115484408[5] = 0; + out_8200477424115484408[6] = 0; + out_8200477424115484408[7] = 0; + out_8200477424115484408[8] = 0; + out_8200477424115484408[9] = 0; + out_8200477424115484408[10] = 0; + out_8200477424115484408[11] = 0; + out_8200477424115484408[12] = 0; + out_8200477424115484408[13] = 0; + out_8200477424115484408[14] = 0; + out_8200477424115484408[15] = 0; + out_8200477424115484408[16] = 0; + out_8200477424115484408[17] = 0; + out_8200477424115484408[18] = 0; + out_8200477424115484408[19] = 0; + out_8200477424115484408[20] = 0; + out_8200477424115484408[21] = 0; + out_8200477424115484408[22] = 0; + out_8200477424115484408[23] = 0; + out_8200477424115484408[24] = 0; + out_8200477424115484408[25] = 0; + out_8200477424115484408[26] = 1; + out_8200477424115484408[27] = 0; + out_8200477424115484408[28] = 0; + out_8200477424115484408[29] = 0; + out_8200477424115484408[30] = 0; + out_8200477424115484408[31] = 0; + out_8200477424115484408[32] = 0; + out_8200477424115484408[33] = 0; + out_8200477424115484408[34] = 0; + out_8200477424115484408[35] = 0; + out_8200477424115484408[36] = 0; + out_8200477424115484408[37] = 0; + out_8200477424115484408[38] = 0; + out_8200477424115484408[39] = 0; + out_8200477424115484408[40] = 0; + out_8200477424115484408[41] = 0; + out_8200477424115484408[42] = 0; + out_8200477424115484408[43] = 0; + out_8200477424115484408[44] = 0; + out_8200477424115484408[45] = 0; + out_8200477424115484408[46] = 0; + out_8200477424115484408[47] = 0; + out_8200477424115484408[48] = 0; + out_8200477424115484408[49] = 1; + out_8200477424115484408[50] = 0; + out_8200477424115484408[51] = 0; + out_8200477424115484408[52] = 0; + out_8200477424115484408[53] = 0; + out_8200477424115484408[54] = 0; + out_8200477424115484408[55] = 0; + out_8200477424115484408[56] = 0; + out_8200477424115484408[57] = 0; + out_8200477424115484408[58] = 0; + out_8200477424115484408[59] = 0; + out_8200477424115484408[60] = 0; + out_8200477424115484408[61] = 0; + out_8200477424115484408[62] = 0; + out_8200477424115484408[63] = 0; + out_8200477424115484408[64] = 0; + out_8200477424115484408[65] = 0; + out_8200477424115484408[66] = 0; + out_8200477424115484408[67] = 0; + out_8200477424115484408[68] = 0; + out_8200477424115484408[69] = 0; + out_8200477424115484408[70] = 0; + out_8200477424115484408[71] = 0; + out_8200477424115484408[72] = 1; + out_8200477424115484408[73] = 0; + out_8200477424115484408[74] = 0; + out_8200477424115484408[75] = 0; + out_8200477424115484408[76] = 0; + out_8200477424115484408[77] = 0; + out_8200477424115484408[78] = 0; + out_8200477424115484408[79] = 0; + out_8200477424115484408[80] = 0; + out_8200477424115484408[81] = 0; + out_8200477424115484408[82] = 0; + out_8200477424115484408[83] = 0; + out_8200477424115484408[84] = 0; + out_8200477424115484408[85] = 0; + out_8200477424115484408[86] = 0; + out_8200477424115484408[87] = 0; } -void h_13(double *state, double *unused, double *out_4436606334341082472) { - out_4436606334341082472[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; - out_4436606334341082472[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; - out_4436606334341082472[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; +void h_13(double *state, double *unused, double *out_2913105661532148231) { + out_2913105661532148231[0] = (-2*state[3]*state[5] + 2*state[4]*state[6])*state[9] + (2*state[3]*state[6] + 2*state[4]*state[5])*state[8] + (pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2))*state[7]; + out_2913105661532148231[1] = (2*state[3]*state[4] + 2*state[5]*state[6])*state[9] + (-2*state[3]*state[6] + 2*state[4]*state[5])*state[7] + (pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2))*state[8]; + out_2913105661532148231[2] = (-2*state[3]*state[4] + 2*state[5]*state[6])*state[8] + (2*state[3]*state[5] + 2*state[4]*state[6])*state[7] + (pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2))*state[9]; } -void H_13(double *state, double *unused, double *out_7475084592984867636) { - out_7475084592984867636[0] = 0; - out_7475084592984867636[1] = 0; - out_7475084592984867636[2] = 0; - out_7475084592984867636[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_7475084592984867636[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_7475084592984867636[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; - out_7475084592984867636[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_7475084592984867636[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); - out_7475084592984867636[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; - out_7475084592984867636[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; - out_7475084592984867636[10] = 0; - out_7475084592984867636[11] = 0; - out_7475084592984867636[12] = 0; - out_7475084592984867636[13] = 0; - out_7475084592984867636[14] = 0; - out_7475084592984867636[15] = 0; - out_7475084592984867636[16] = 0; - out_7475084592984867636[17] = 0; - out_7475084592984867636[18] = 0; - out_7475084592984867636[19] = 0; - out_7475084592984867636[20] = 0; - out_7475084592984867636[21] = 0; - out_7475084592984867636[22] = 0; - out_7475084592984867636[23] = 0; - out_7475084592984867636[24] = 0; - out_7475084592984867636[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; - out_7475084592984867636[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_7475084592984867636[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_7475084592984867636[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; - out_7475084592984867636[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; - out_7475084592984867636[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); - out_7475084592984867636[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; - out_7475084592984867636[32] = 0; - out_7475084592984867636[33] = 0; - out_7475084592984867636[34] = 0; - out_7475084592984867636[35] = 0; - out_7475084592984867636[36] = 0; - out_7475084592984867636[37] = 0; - out_7475084592984867636[38] = 0; - out_7475084592984867636[39] = 0; - out_7475084592984867636[40] = 0; - out_7475084592984867636[41] = 0; - out_7475084592984867636[42] = 0; - out_7475084592984867636[43] = 0; - out_7475084592984867636[44] = 0; - out_7475084592984867636[45] = 0; - out_7475084592984867636[46] = 0; - out_7475084592984867636[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; - out_7475084592984867636[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; - out_7475084592984867636[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; - out_7475084592984867636[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; - out_7475084592984867636[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; - out_7475084592984867636[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; - out_7475084592984867636[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); - out_7475084592984867636[54] = 0; - out_7475084592984867636[55] = 0; - out_7475084592984867636[56] = 0; - out_7475084592984867636[57] = 0; - out_7475084592984867636[58] = 0; - out_7475084592984867636[59] = 0; - out_7475084592984867636[60] = 0; - out_7475084592984867636[61] = 0; - out_7475084592984867636[62] = 0; - out_7475084592984867636[63] = 0; - out_7475084592984867636[64] = 0; - out_7475084592984867636[65] = 0; +void H_13(double *state, double *unused, double *out_6442117529364745675) { + out_6442117529364745675[0] = 0; + out_6442117529364745675[1] = 0; + out_6442117529364745675[2] = 0; + out_6442117529364745675[3] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; + out_6442117529364745675[4] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6442117529364745675[5] = -2*state[3]*state[9] + 2*state[4]*state[8] - 2*state[5]*state[7]; + out_6442117529364745675[6] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; + out_6442117529364745675[7] = pow(state[3], 2) + pow(state[4], 2) - pow(state[5], 2) - pow(state[6], 2); + out_6442117529364745675[8] = 2*state[3]*state[6] + 2*state[4]*state[5]; + out_6442117529364745675[9] = -2*state[3]*state[5] + 2*state[4]*state[6]; + out_6442117529364745675[10] = 0; + out_6442117529364745675[11] = 0; + out_6442117529364745675[12] = 0; + out_6442117529364745675[13] = 0; + out_6442117529364745675[14] = 0; + out_6442117529364745675[15] = 0; + out_6442117529364745675[16] = 0; + out_6442117529364745675[17] = 0; + out_6442117529364745675[18] = 0; + out_6442117529364745675[19] = 0; + out_6442117529364745675[20] = 0; + out_6442117529364745675[21] = 0; + out_6442117529364745675[22] = 0; + out_6442117529364745675[23] = 0; + out_6442117529364745675[24] = 0; + out_6442117529364745675[25] = 2*state[3]*state[8] + 2*state[4]*state[9] - 2*state[6]*state[7]; + out_6442117529364745675[26] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; + out_6442117529364745675[27] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6442117529364745675[28] = -2*state[3]*state[7] + 2*state[5]*state[9] - 2*state[6]*state[8]; + out_6442117529364745675[29] = -2*state[3]*state[6] + 2*state[4]*state[5]; + out_6442117529364745675[30] = pow(state[3], 2) - pow(state[4], 2) + pow(state[5], 2) - pow(state[6], 2); + out_6442117529364745675[31] = 2*state[3]*state[4] + 2*state[5]*state[6]; + out_6442117529364745675[32] = 0; + out_6442117529364745675[33] = 0; + out_6442117529364745675[34] = 0; + out_6442117529364745675[35] = 0; + out_6442117529364745675[36] = 0; + out_6442117529364745675[37] = 0; + out_6442117529364745675[38] = 0; + out_6442117529364745675[39] = 0; + out_6442117529364745675[40] = 0; + out_6442117529364745675[41] = 0; + out_6442117529364745675[42] = 0; + out_6442117529364745675[43] = 0; + out_6442117529364745675[44] = 0; + out_6442117529364745675[45] = 0; + out_6442117529364745675[46] = 0; + out_6442117529364745675[47] = 2*state[3]*state[9] - 2*state[4]*state[8] + 2*state[5]*state[7]; + out_6442117529364745675[48] = -2*state[3]*state[8] - 2*state[4]*state[9] + 2*state[6]*state[7]; + out_6442117529364745675[49] = 2*state[3]*state[7] - 2*state[5]*state[9] + 2*state[6]*state[8]; + out_6442117529364745675[50] = 2*state[4]*state[7] + 2*state[5]*state[8] + 2*state[6]*state[9]; + out_6442117529364745675[51] = 2*state[3]*state[5] + 2*state[4]*state[6]; + out_6442117529364745675[52] = -2*state[3]*state[4] + 2*state[5]*state[6]; + out_6442117529364745675[53] = pow(state[3], 2) - pow(state[4], 2) - pow(state[5], 2) + pow(state[6], 2); + out_6442117529364745675[54] = 0; + out_6442117529364745675[55] = 0; + out_6442117529364745675[56] = 0; + out_6442117529364745675[57] = 0; + out_6442117529364745675[58] = 0; + out_6442117529364745675[59] = 0; + out_6442117529364745675[60] = 0; + out_6442117529364745675[61] = 0; + out_6442117529364745675[62] = 0; + out_6442117529364745675[63] = 0; + out_6442117529364745675[64] = 0; + out_6442117529364745675[65] = 0; } -void h_14(double *state, double *unused, double *out_2075713419116157705) { - out_2075713419116157705[0] = state[10]; - out_2075713419116157705[1] = state[11]; - out_2075713419116157705[2] = state[12]; +void h_14(double *state, double *unused, double *out_4670376777294811307) { + out_4670376777294811307[0] = state[10]; + out_4670376777294811307[1] = state[11]; + out_4670376777294811307[2] = state[12]; } -void H_14(double *state, double *unused, double *out_4340985179732657688) { - out_4340985179732657688[0] = 0; - out_4340985179732657688[1] = 0; - out_4340985179732657688[2] = 0; - out_4340985179732657688[3] = 0; - out_4340985179732657688[4] = 0; - out_4340985179732657688[5] = 0; - out_4340985179732657688[6] = 0; - out_4340985179732657688[7] = 0; - out_4340985179732657688[8] = 0; - out_4340985179732657688[9] = 0; - out_4340985179732657688[10] = 1; - out_4340985179732657688[11] = 0; - out_4340985179732657688[12] = 0; - out_4340985179732657688[13] = 0; - out_4340985179732657688[14] = 0; - out_4340985179732657688[15] = 0; - out_4340985179732657688[16] = 0; - out_4340985179732657688[17] = 0; - out_4340985179732657688[18] = 0; - out_4340985179732657688[19] = 0; - out_4340985179732657688[20] = 0; - out_4340985179732657688[21] = 0; - out_4340985179732657688[22] = 0; - out_4340985179732657688[23] = 0; - out_4340985179732657688[24] = 0; - out_4340985179732657688[25] = 0; - out_4340985179732657688[26] = 0; - out_4340985179732657688[27] = 0; - out_4340985179732657688[28] = 0; - out_4340985179732657688[29] = 0; - out_4340985179732657688[30] = 0; - out_4340985179732657688[31] = 0; - out_4340985179732657688[32] = 0; - out_4340985179732657688[33] = 1; - out_4340985179732657688[34] = 0; - out_4340985179732657688[35] = 0; - out_4340985179732657688[36] = 0; - out_4340985179732657688[37] = 0; - out_4340985179732657688[38] = 0; - out_4340985179732657688[39] = 0; - out_4340985179732657688[40] = 0; - out_4340985179732657688[41] = 0; - out_4340985179732657688[42] = 0; - out_4340985179732657688[43] = 0; - out_4340985179732657688[44] = 0; - out_4340985179732657688[45] = 0; - out_4340985179732657688[46] = 0; - out_4340985179732657688[47] = 0; - out_4340985179732657688[48] = 0; - out_4340985179732657688[49] = 0; - out_4340985179732657688[50] = 0; - out_4340985179732657688[51] = 0; - out_4340985179732657688[52] = 0; - out_4340985179732657688[53] = 0; - out_4340985179732657688[54] = 0; - out_4340985179732657688[55] = 0; - out_4340985179732657688[56] = 1; - out_4340985179732657688[57] = 0; - out_4340985179732657688[58] = 0; - out_4340985179732657688[59] = 0; - out_4340985179732657688[60] = 0; - out_4340985179732657688[61] = 0; - out_4340985179732657688[62] = 0; - out_4340985179732657688[63] = 0; - out_4340985179732657688[64] = 0; - out_4340985179732657688[65] = 0; +void H_14(double *state, double *unused, double *out_9153039822665174869) { + out_9153039822665174869[0] = 0; + out_9153039822665174869[1] = 0; + out_9153039822665174869[2] = 0; + out_9153039822665174869[3] = 0; + out_9153039822665174869[4] = 0; + out_9153039822665174869[5] = 0; + out_9153039822665174869[6] = 0; + out_9153039822665174869[7] = 0; + out_9153039822665174869[8] = 0; + out_9153039822665174869[9] = 0; + out_9153039822665174869[10] = 1; + out_9153039822665174869[11] = 0; + out_9153039822665174869[12] = 0; + out_9153039822665174869[13] = 0; + out_9153039822665174869[14] = 0; + out_9153039822665174869[15] = 0; + out_9153039822665174869[16] = 0; + out_9153039822665174869[17] = 0; + out_9153039822665174869[18] = 0; + out_9153039822665174869[19] = 0; + out_9153039822665174869[20] = 0; + out_9153039822665174869[21] = 0; + out_9153039822665174869[22] = 0; + out_9153039822665174869[23] = 0; + out_9153039822665174869[24] = 0; + out_9153039822665174869[25] = 0; + out_9153039822665174869[26] = 0; + out_9153039822665174869[27] = 0; + out_9153039822665174869[28] = 0; + out_9153039822665174869[29] = 0; + out_9153039822665174869[30] = 0; + out_9153039822665174869[31] = 0; + out_9153039822665174869[32] = 0; + out_9153039822665174869[33] = 1; + out_9153039822665174869[34] = 0; + out_9153039822665174869[35] = 0; + out_9153039822665174869[36] = 0; + out_9153039822665174869[37] = 0; + out_9153039822665174869[38] = 0; + out_9153039822665174869[39] = 0; + out_9153039822665174869[40] = 0; + out_9153039822665174869[41] = 0; + out_9153039822665174869[42] = 0; + out_9153039822665174869[43] = 0; + out_9153039822665174869[44] = 0; + out_9153039822665174869[45] = 0; + out_9153039822665174869[46] = 0; + out_9153039822665174869[47] = 0; + out_9153039822665174869[48] = 0; + out_9153039822665174869[49] = 0; + out_9153039822665174869[50] = 0; + out_9153039822665174869[51] = 0; + out_9153039822665174869[52] = 0; + out_9153039822665174869[53] = 0; + out_9153039822665174869[54] = 0; + out_9153039822665174869[55] = 0; + out_9153039822665174869[56] = 1; + out_9153039822665174869[57] = 0; + out_9153039822665174869[58] = 0; + out_9153039822665174869[59] = 0; + out_9153039822665174869[60] = 0; + out_9153039822665174869[61] = 0; + out_9153039822665174869[62] = 0; + out_9153039822665174869[63] = 0; + out_9153039822665174869[64] = 0; + out_9153039822665174869[65] = 0; } -void h_33(double *state, double *unused, double *out_7363698010792310478) { - out_7363698010792310478[0] = state[16]; - out_7363698010792310478[1] = state[17]; - out_7363698010792310478[2] = state[18]; +void h_33(double *state, double *unused, double *out_3733937587285700568) { + out_3733937587285700568[0] = state[16]; + out_3733937587285700568[1] = state[17]; + out_3733937587285700568[2] = state[18]; } -void H_33(double *state, double *unused, double *out_712627670001272050) { - out_712627670001272050[0] = 0; - out_712627670001272050[1] = 0; - out_712627670001272050[2] = 0; - out_712627670001272050[3] = 0; - out_712627670001272050[4] = 0; - out_712627670001272050[5] = 0; - out_712627670001272050[6] = 0; - out_712627670001272050[7] = 0; - out_712627670001272050[8] = 0; - out_712627670001272050[9] = 0; - out_712627670001272050[10] = 0; - out_712627670001272050[11] = 0; - out_712627670001272050[12] = 0; - out_712627670001272050[13] = 0; - out_712627670001272050[14] = 0; - out_712627670001272050[15] = 0; - out_712627670001272050[16] = 1; - out_712627670001272050[17] = 0; - out_712627670001272050[18] = 0; - out_712627670001272050[19] = 0; - out_712627670001272050[20] = 0; - out_712627670001272050[21] = 0; - out_712627670001272050[22] = 0; - out_712627670001272050[23] = 0; - out_712627670001272050[24] = 0; - out_712627670001272050[25] = 0; - out_712627670001272050[26] = 0; - out_712627670001272050[27] = 0; - out_712627670001272050[28] = 0; - out_712627670001272050[29] = 0; - out_712627670001272050[30] = 0; - out_712627670001272050[31] = 0; - out_712627670001272050[32] = 0; - out_712627670001272050[33] = 0; - out_712627670001272050[34] = 0; - out_712627670001272050[35] = 0; - out_712627670001272050[36] = 0; - out_712627670001272050[37] = 0; - out_712627670001272050[38] = 0; - out_712627670001272050[39] = 1; - out_712627670001272050[40] = 0; - out_712627670001272050[41] = 0; - out_712627670001272050[42] = 0; - out_712627670001272050[43] = 0; - out_712627670001272050[44] = 0; - out_712627670001272050[45] = 0; - out_712627670001272050[46] = 0; - out_712627670001272050[47] = 0; - out_712627670001272050[48] = 0; - out_712627670001272050[49] = 0; - out_712627670001272050[50] = 0; - out_712627670001272050[51] = 0; - out_712627670001272050[52] = 0; - out_712627670001272050[53] = 0; - out_712627670001272050[54] = 0; - out_712627670001272050[55] = 0; - out_712627670001272050[56] = 0; - out_712627670001272050[57] = 0; - out_712627670001272050[58] = 0; - out_712627670001272050[59] = 0; - out_712627670001272050[60] = 0; - out_712627670001272050[61] = 0; - out_712627670001272050[62] = 1; - out_712627670001272050[63] = 0; - out_712627670001272050[64] = 0; - out_712627670001272050[65] = 0; +void H_33(double *state, double *unused, double *out_1521346975701067594) { + out_1521346975701067594[0] = 0; + out_1521346975701067594[1] = 0; + out_1521346975701067594[2] = 0; + out_1521346975701067594[3] = 0; + out_1521346975701067594[4] = 0; + out_1521346975701067594[5] = 0; + out_1521346975701067594[6] = 0; + out_1521346975701067594[7] = 0; + out_1521346975701067594[8] = 0; + out_1521346975701067594[9] = 0; + out_1521346975701067594[10] = 0; + out_1521346975701067594[11] = 0; + out_1521346975701067594[12] = 0; + out_1521346975701067594[13] = 0; + out_1521346975701067594[14] = 0; + out_1521346975701067594[15] = 0; + out_1521346975701067594[16] = 1; + out_1521346975701067594[17] = 0; + out_1521346975701067594[18] = 0; + out_1521346975701067594[19] = 0; + out_1521346975701067594[20] = 0; + out_1521346975701067594[21] = 0; + out_1521346975701067594[22] = 0; + out_1521346975701067594[23] = 0; + out_1521346975701067594[24] = 0; + out_1521346975701067594[25] = 0; + out_1521346975701067594[26] = 0; + out_1521346975701067594[27] = 0; + out_1521346975701067594[28] = 0; + out_1521346975701067594[29] = 0; + out_1521346975701067594[30] = 0; + out_1521346975701067594[31] = 0; + out_1521346975701067594[32] = 0; + out_1521346975701067594[33] = 0; + out_1521346975701067594[34] = 0; + out_1521346975701067594[35] = 0; + out_1521346975701067594[36] = 0; + out_1521346975701067594[37] = 0; + out_1521346975701067594[38] = 0; + out_1521346975701067594[39] = 1; + out_1521346975701067594[40] = 0; + out_1521346975701067594[41] = 0; + out_1521346975701067594[42] = 0; + out_1521346975701067594[43] = 0; + out_1521346975701067594[44] = 0; + out_1521346975701067594[45] = 0; + out_1521346975701067594[46] = 0; + out_1521346975701067594[47] = 0; + out_1521346975701067594[48] = 0; + out_1521346975701067594[49] = 0; + out_1521346975701067594[50] = 0; + out_1521346975701067594[51] = 0; + out_1521346975701067594[52] = 0; + out_1521346975701067594[53] = 0; + out_1521346975701067594[54] = 0; + out_1521346975701067594[55] = 0; + out_1521346975701067594[56] = 0; + out_1521346975701067594[57] = 0; + out_1521346975701067594[58] = 0; + out_1521346975701067594[59] = 0; + out_1521346975701067594[60] = 0; + out_1521346975701067594[61] = 0; + out_1521346975701067594[62] = 1; + out_1521346975701067594[63] = 0; + out_1521346975701067594[64] = 0; + out_1521346975701067594[65] = 0; } #include #include @@ -1855,77 +1855,77 @@ void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, doub void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_33, H_33, NULL, in_z, in_R, in_ea, MAHA_THRESH_33); } -void live_H(double *in_vec, double *out_919321204285207253) { - H(in_vec, out_919321204285207253); +void live_H(double *in_vec, double *out_6690968010248080223) { + H(in_vec, out_6690968010248080223); } -void live_err_fun(double *nom_x, double *delta_x, double *out_1239592434612673434) { - err_fun(nom_x, delta_x, out_1239592434612673434); +void live_err_fun(double *nom_x, double *delta_x, double *out_5128644486154966433) { + err_fun(nom_x, delta_x, out_5128644486154966433); } -void live_inv_err_fun(double *nom_x, double *true_x, double *out_8424707651892025503) { - inv_err_fun(nom_x, true_x, out_8424707651892025503); +void live_inv_err_fun(double *nom_x, double *true_x, double *out_7600355658971333284) { + inv_err_fun(nom_x, true_x, out_7600355658971333284); } -void live_H_mod_fun(double *state, double *out_3398846321270000840) { - H_mod_fun(state, out_3398846321270000840); +void live_H_mod_fun(double *state, double *out_1685726956674156760) { + H_mod_fun(state, out_1685726956674156760); } -void live_f_fun(double *state, double dt, double *out_5945410084485745517) { - f_fun(state, dt, out_5945410084485745517); +void live_f_fun(double *state, double dt, double *out_774966178326963321) { + f_fun(state, dt, out_774966178326963321); } -void live_F_fun(double *state, double dt, double *out_1406202444158773973) { - F_fun(state, dt, out_1406202444158773973); +void live_F_fun(double *state, double dt, double *out_7282145086155387076) { + F_fun(state, dt, out_7282145086155387076); } -void live_h_4(double *state, double *unused, double *out_855318047403330363) { - h_4(state, unused, out_855318047403330363); +void live_h_4(double *state, double *unused, double *out_3936704980047894152) { + h_4(state, unused, out_3936704980047894152); } -void live_H_4(double *state, double *unused, double *out_7229846732012737030) { - H_4(state, unused, out_7229846732012737030); +void live_H_4(double *state, double *unused, double *out_9052514604414786102) { + H_4(state, unused, out_9052514604414786102); } -void live_h_9(double *state, double *unused, double *out_2075713419116157705) { - h_9(state, unused, out_2075713419116157705); +void live_h_9(double *state, double *unused, double *out_4670376777294811307) { + h_9(state, unused, out_4670376777294811307); } -void live_H_9(double *state, double *unused, double *out_4340985179732657688) { - H_9(state, unused, out_4340985179732657688); +void live_H_9(double *state, double *unused, double *out_9153039822665174869) { + H_9(state, unused, out_9153039822665174869); } -void live_h_10(double *state, double *unused, double *out_4871234396170298071) { - h_10(state, unused, out_4871234396170298071); +void live_h_10(double *state, double *unused, double *out_5114531167885874097) { + h_10(state, unused, out_5114531167885874097); } -void live_H_10(double *state, double *unused, double *out_6664980085443225062) { - H_10(state, unused, out_6664980085443225062); +void live_H_10(double *state, double *unused, double *out_8901083693322163334) { + H_10(state, unused, out_8901083693322163334); } -void live_h_12(double *state, double *unused, double *out_59445924731046944) { - h_12(state, unused, out_59445924731046944); +void live_h_12(double *state, double *unused, double *out_6249225679190641860) { + h_12(state, unused, out_6249225679190641860); } -void live_H_12(double *state, double *unused, double *out_437281581669713462) { - H_12(state, unused, out_437281581669713462); +void live_H_12(double *state, double *unused, double *out_4374773061262803719) { + H_12(state, unused, out_4374773061262803719); } -void live_h_35(double *state, double *unused, double *out_5470579135920011702) { - h_35(state, unused, out_5470579135920011702); +void live_h_35(double *state, double *unused, double *out_5751710703471722702) { + h_35(state, unused, out_5751710703471722702); } -void live_H_35(double *state, double *unused, double *out_3863184674640129654) { - H_35(state, unused, out_3863184674640129654); +void live_H_35(double *state, double *unused, double *out_1629210028937790010) { + H_35(state, unused, out_1629210028937790010); } -void live_h_32(double *state, double *unused, double *out_61541883826926771) { - h_32(state, unused, out_61541883826926771); +void live_h_32(double *state, double *unused, double *out_7960850640984836266) { + h_32(state, unused, out_7960850640984836266); } -void live_H_32(double *state, double *unused, double *out_4156829226020876432) { - H_32(state, unused, out_4156829226020876432); +void live_H_32(double *state, double *unused, double *out_8200477424115484408) { + H_32(state, unused, out_8200477424115484408); } -void live_h_13(double *state, double *unused, double *out_4436606334341082472) { - h_13(state, unused, out_4436606334341082472); +void live_h_13(double *state, double *unused, double *out_2913105661532148231) { + h_13(state, unused, out_2913105661532148231); } -void live_H_13(double *state, double *unused, double *out_7475084592984867636) { - H_13(state, unused, out_7475084592984867636); +void live_H_13(double *state, double *unused, double *out_6442117529364745675) { + H_13(state, unused, out_6442117529364745675); } -void live_h_14(double *state, double *unused, double *out_2075713419116157705) { - h_14(state, unused, out_2075713419116157705); +void live_h_14(double *state, double *unused, double *out_4670376777294811307) { + h_14(state, unused, out_4670376777294811307); } -void live_H_14(double *state, double *unused, double *out_4340985179732657688) { - H_14(state, unused, out_4340985179732657688); +void live_H_14(double *state, double *unused, double *out_9153039822665174869) { + H_14(state, unused, out_9153039822665174869); } -void live_h_33(double *state, double *unused, double *out_7363698010792310478) { - h_33(state, unused, out_7363698010792310478); +void live_h_33(double *state, double *unused, double *out_3733937587285700568) { + h_33(state, unused, out_3733937587285700568); } -void live_H_33(double *state, double *unused, double *out_712627670001272050) { - H_33(state, unused, out_712627670001272050); +void live_H_33(double *state, double *unused, double *out_1521346975701067594) { + H_33(state, unused, out_1521346975701067594); } void live_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/live.h b/selfdrive/locationd/models/generated/live.h index b228cc0d9..817aa083e 100644 --- a/selfdrive/locationd/models/generated/live.h +++ b/selfdrive/locationd/models/generated/live.h @@ -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_919321204285207253); -void live_err_fun(double *nom_x, double *delta_x, double *out_1239592434612673434); -void live_inv_err_fun(double *nom_x, double *true_x, double *out_8424707651892025503); -void live_H_mod_fun(double *state, double *out_3398846321270000840); -void live_f_fun(double *state, double dt, double *out_5945410084485745517); -void live_F_fun(double *state, double dt, double *out_1406202444158773973); -void live_h_4(double *state, double *unused, double *out_855318047403330363); -void live_H_4(double *state, double *unused, double *out_7229846732012737030); -void live_h_9(double *state, double *unused, double *out_2075713419116157705); -void live_H_9(double *state, double *unused, double *out_4340985179732657688); -void live_h_10(double *state, double *unused, double *out_4871234396170298071); -void live_H_10(double *state, double *unused, double *out_6664980085443225062); -void live_h_12(double *state, double *unused, double *out_59445924731046944); -void live_H_12(double *state, double *unused, double *out_437281581669713462); -void live_h_35(double *state, double *unused, double *out_5470579135920011702); -void live_H_35(double *state, double *unused, double *out_3863184674640129654); -void live_h_32(double *state, double *unused, double *out_61541883826926771); -void live_H_32(double *state, double *unused, double *out_4156829226020876432); -void live_h_13(double *state, double *unused, double *out_4436606334341082472); -void live_H_13(double *state, double *unused, double *out_7475084592984867636); -void live_h_14(double *state, double *unused, double *out_2075713419116157705); -void live_H_14(double *state, double *unused, double *out_4340985179732657688); -void live_h_33(double *state, double *unused, double *out_7363698010792310478); -void live_H_33(double *state, double *unused, double *out_712627670001272050); +void live_H(double *in_vec, double *out_6690968010248080223); +void live_err_fun(double *nom_x, double *delta_x, double *out_5128644486154966433); +void live_inv_err_fun(double *nom_x, double *true_x, double *out_7600355658971333284); +void live_H_mod_fun(double *state, double *out_1685726956674156760); +void live_f_fun(double *state, double dt, double *out_774966178326963321); +void live_F_fun(double *state, double dt, double *out_7282145086155387076); +void live_h_4(double *state, double *unused, double *out_3936704980047894152); +void live_H_4(double *state, double *unused, double *out_9052514604414786102); +void live_h_9(double *state, double *unused, double *out_4670376777294811307); +void live_H_9(double *state, double *unused, double *out_9153039822665174869); +void live_h_10(double *state, double *unused, double *out_5114531167885874097); +void live_H_10(double *state, double *unused, double *out_8901083693322163334); +void live_h_12(double *state, double *unused, double *out_6249225679190641860); +void live_H_12(double *state, double *unused, double *out_4374773061262803719); +void live_h_35(double *state, double *unused, double *out_5751710703471722702); +void live_H_35(double *state, double *unused, double *out_1629210028937790010); +void live_h_32(double *state, double *unused, double *out_7960850640984836266); +void live_H_32(double *state, double *unused, double *out_8200477424115484408); +void live_h_13(double *state, double *unused, double *out_2913105661532148231); +void live_H_13(double *state, double *unused, double *out_6442117529364745675); +void live_h_14(double *state, double *unused, double *out_4670376777294811307); +void live_H_14(double *state, double *unused, double *out_9153039822665174869); +void live_h_33(double *state, double *unused, double *out_3733937587285700568); +void live_H_33(double *state, double *unused, double *out_1521346975701067594); void live_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/locationd/ubloxd b/selfdrive/locationd/ubloxd index 6e29bab11..01979994d 100755 Binary files a/selfdrive/locationd/ubloxd and b/selfdrive/locationd/ubloxd differ diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py index 92dbea9d0..8718744ca 100644 --- a/selfdrive/mapd/mapd.py +++ b/selfdrive/mapd/mapd.py @@ -11,7 +11,11 @@ from selfdrive.mapd.lib.WayCollection import WayCollection from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME from system.swaglog import cloudlog -ROAD_NAME_TIMEOUT = 10 # secs +# dp +from common.params import Params +import json + +ROAD_NAME_TIMEOUT = 30 # secs _DEBUG = False _CLOUDLOG_DEBUG = True @@ -54,6 +58,18 @@ class MapD(): self._road_name_last = "" self._road_name_last_timed_out = 0. + # dp - use LastGPSPosition as init position (if we are in a undercover car park?) + # this way we can prefetch osm data before we get a fix. + last_pos = Params().get("LastGPSPosition") + if last_pos is not None and last_pos != "": + l = json.loads(last_pos) + lat = float(l["latitude"]) + lon = float(l["longitude"]) + self.location_rad = np.radians(np.array([lat, lon], dtype=float)) + self.location_deg = (lat, lon) + self.bearing_rad = np.radians(0, dtype=float) + _debug("Use LastGPSPosition position - lat: %s, lon: %s" % (lat, lon)) + def udpate_state(self, sm): sock = 'controlsState' if not sm.updated[sock] or not sm.valid[sock]: @@ -201,7 +217,7 @@ class MapD(): turn_speed_limit_section = self.route.current_curvature_speed_limit_section horizon_mts = self.gps_speed * LOOK_AHEAD_HORIZON_TIME next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts) - current_road_name = self.route.current_road_name + current_road_name = "" if self.route.current_road_name is None else str(self.route.current_road_name).strip() map_data_msg = messaging.new_message('liveMapData') map_data_msg.valid = sm.all_alive(service_list=['gpsLocationExternal']) and \ @@ -234,18 +250,19 @@ class MapD(): map_data_msg.liveMapData.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections] map_data_msg.liveMapData.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections] - road_name = str(current_road_name if current_road_name is not None else "").strip() - if road_name == "" and self._road_name_last != "": + # dp - cache road name to avoid name display blinking + if current_road_name == "": + sec = sec_since_boot() if self._road_name_last_timed_out == 0.: - self._road_name_last_timed_out = sec_since_boot() + ROAD_NAME_TIMEOUT - else: - if sec_since_boot() <= self._road_name_last_timed_out: - road_name = self._road_name_last + self._road_name_last_timed_out = sec + ROAD_NAME_TIMEOUT + + if sec < self._road_name_last_timed_out: + current_road_name = self._road_name_last else: self._road_name_last_timed_out = 0. - self._road_name_last = road_name + self._road_name_last = current_road_name - map_data_msg.liveMapData.currentRoadName = road_name + map_data_msg.liveMapData.currentRoadName = current_road_name pm.send('liveMapData', map_data_msg) _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False) diff --git a/selfdrive/modeld/_modeld b/selfdrive/modeld/_modeld index f715bf6dc..19b9e00b0 100755 Binary files a/selfdrive/modeld/_modeld and b/selfdrive/modeld/_modeld differ diff --git a/selfdrive/modeld/models/supercombo.thneed b/selfdrive/modeld/models/supercombo.thneed index affb25ba3..77f4cb8de 100644 Binary files a/selfdrive/modeld/models/supercombo.thneed and b/selfdrive/modeld/models/supercombo.thneed differ diff --git a/selfdrive/sensord/_sensord b/selfdrive/sensord/_sensord index 8d18b2b1e..d85ec0874 100755 Binary files a/selfdrive/sensord/_sensord and b/selfdrive/sensord/_sensord differ diff --git a/selfdrive/ui/_ui b/selfdrive/ui/_ui index b121773d0..e604bd14a 100755 Binary files a/selfdrive/ui/_ui and b/selfdrive/ui/_ui differ diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner index d06d7f061..97d5b64fb 100755 Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ diff --git a/selfdrive/ui/qt/text b/selfdrive/ui/qt/text index 8bd58985a..710bd1b5b 100755 Binary files a/selfdrive/ui/qt/text and b/selfdrive/ui/qt/text differ diff --git a/selfdrive/ui/soundd/_soundd b/selfdrive/ui/soundd/_soundd index 6a7e017f6..bb2660ab6 100755 Binary files a/selfdrive/ui/soundd/_soundd and b/selfdrive/ui/soundd/_soundd differ diff --git a/selfdrive/ui/translations/main_en.qm b/selfdrive/ui/translations/main_en.qm new file mode 100644 index 000000000..dab9f0ee1 Binary files /dev/null and b/selfdrive/ui/translations/main_en.qm differ diff --git a/selfdrive/ui/translations/main_ja.qm b/selfdrive/ui/translations/main_ja.qm new file mode 100644 index 000000000..365980142 Binary files /dev/null and b/selfdrive/ui/translations/main_ja.qm differ diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 20f96b8f4..36ad618bd 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -73,6 +73,30 @@ 空白のままにして、自動設定にします + + C2NetworkPanel + + + Wi-Fi Settings + + + + + + OPEN + + + + + Tethering Settings + + + + + IP Address + IP アドレス + + ConfirmationDialog @@ -87,6 +111,524 @@ キャンセル + + DPCarPanel + + + Override Speed When Below + + + + + Override feature will be enabled when set cruise speed is lower than this value. +1 km/h = 0.62 mph. + + + + + km/h + + + + + Turn On Cruise Speed Override + + + + + This feature will let you set your cruise speed below vehicle standard. (usually at 26~40 km/h) + + + + + Toyota / Lexus + + + + + Enable SnG Mod + + + + + Enable this to fix stop and go (SnG) issue on some models. +Reboot required. + + + + + Enable AM Physical Button Ctrl + + + + + Enable this to link Accel Mode (AM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES. +Reboot required. + + + + + Mazda + + + + + Display Below Steer Speed Alert + + + + + Enable this will show below steer speed alert. +Thanks to @TheCrowd + + + + + DPControlPanel + + + ALCA Delay + + + + + Once the vehicle meets all ALCA criteria, it will wait for the seconds set here before peforming lane change automatically. + + + + + secs + + + + + ALCA Min Speed + + + + + ALCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + + mph + + + + + LCA Min Speed + + + + + LCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + Lateral Ctrl Mode + + + + + 1 = Lane Change Assist (LCA) +2 = Auto Lane Change Assist (ALCA) + + + + + Longitudinal + + + + + Manually Control Accel Mode + + + + + Enable this if you wish to adjust openpilot's acceleration control. + + + + + Enable vision based turn control + + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + + Lateral + + + + + Use Lane Lines + + + + + Enable this if you wish to use lane lines mode. + + + + + Use LQR Controller + + + + + Enable this if you wish to use LQR instead of PID or INDI controller. +WORKS WELL ONLY ON SOME VEHICLES. +More linear steering experience. + + + + + Always On Lateral + + + + + Use at your own risk! +1 = Stock Long +2 = OP Long +Run Panda Recovery if Panda is not detected after reboot. +Reboot required. + + + + + DPGeneralPanel + + + Auto Shutdown In + + + + + Adjust your shutdown waiting period. +0 = shutdown immediately. + + + + + mins + + + + + Enable Auto Shutdown + + + + + Enable this if you wish to shutdown your device automatically. + + + + + Use Custom API Server + + + + + Enable this if you wish to connect to a custom API server. +Default to "https://api.retropilot.org/", change "dp_api_custom" if you want to change API server URL. +Reboot required. + + + + + Services + + + + + Enable On-Road Dashcam + + + + + Enable this if you wish to record screen, just like a dashcam. +Reboot required. + + + + + Enable GPS Logger + + + + + This will store your track in /data/media/0/gpx_logs/. +Reboot required. + + + + + Hardware - General + + + + + Enable Hotspot On Boot + + + + + Reboot required. + + + + + Miscellaneous + + + + + Enable Jetson Support + + + + + Enable this option if you intend to run dp on Nvidia Jetson. +Reboot required. + + + + + Reboot recommended. +Reboot? + + + + + Flashing Panda Firmware + + + + + FLASH + + + + + Tap the button to update your panda firmware. +The device should reboot once if it finish updating. + + + + + + + Are you sure? + + + + + Pandas Firmware Recovery + + + + + RECOVER + + + + + Tap the button ONLY if your panda ran into issue. + + + + + Delete All Driving Log + + + + + DELETE + + + + + Tap the button to delete ALL your driving logs (including dashcam / gpx / driving logs). + + + + + DPMapsPanel + + + Show Full Screen Nav. + + + + + This will show navigation in full screen. +Please tap green boarder if you wish to switch back drive view. + + + + + Search Destination using Google Map + + + + + This will allow you to search destination in google map api. +You will need to apply your own google map api key.Enter your key detail in web interface once it's enabled. + + + + + Enable Nav. + + + + + This will let use the build in Navigation. +Use web interface to control it: *http://<device_ip>:8082*. +You will need to apply your own mapbox token at https://www.mapbox.com/. +Internet access from mobile phone (tethering) is required. +Reboot required. + + + + + Enable Speed Limit Control + + + + + Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits. + + + + + Enable Speed Limit Offset + + + + + Set speed limit slightly higher than actual speed limit for a more natural drive. + + + + + Enable Map Data Turn Control + + + + + Use curvature info from map data to define speed limits to take turns ahead + + + + + Show debug UI elements + + + + + Show UI elements that aid debugging. + + + + + Enable MapD + + + + + Use OSM to assist lateral/longitudinal control. +Please note: +1. This feature will works only when your car support OP longitudinal. +2. MapD will contribute your route to OSM for future improvement automatically. +3. You can add your own offset for mapd just follow the readme under /selfdrive/mapd. +4. Not connecting to the internet for while might feel up device storage from all the gps traces. + + + + + DPUIPanel + + + Display Mode + + + + + 0 = Default +1 = Screen Off While Driving + + + + + Screen Brightness + + + + + Adjust your screen brightness. + + + + + + AUTO + + + + + Alert Volume + + + + + Adjust your alert volume. + + + + + Quiet Drive + + + + + Display alert and play important warning sound. Thanks @sunnyhaibin. + + + + + Display Side Info + + + + + Enable this to display steering angle / lead car distance / engine RPM. + + + + + Display Top Info + + + + + Enable this to display time / system temp / battery level. + + + DeclinePage @@ -108,152 +650,152 @@ DevicePanel - + Dongle ID ドングル番号 (Dongle ID) - + N/A N/A - + Serial シリアル番号 - + Driver Camera 車内カメラ - + PREVIEW 見る - + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 車内カメラをプレビューして、ドライバー監視システムの視界を確認ができます。(車両の電源を切る必要があります) - + Reset Calibration ｷｬﾘﾌﾞﾚｰｼｮﾝをﾘｾｯﾄ - + RESET ﾘｾｯﾄ - + Are you sure you want to reset calibration? ｷｬﾘﾌﾞﾚｰｼｮﾝをリセットしてもよろしいですか？ - + Review Training Guide 入門書を見る - + REVIEW 見る - + Review the rules, features, and limitations of openpilot openpilot の特徴を見る - + Are you sure you want to review the training guide? 入門書を見てもよろしいですか？ - + Regulatory 認証情報 - + VIEW 見る - + Change Language 言語を変更 - + CHANGE 変更 - + Select a language 言語を選択 - + Reboot 再起動 - + Power Off 電源を切る - + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. openpilot は、左または右の4°以内、上の5°または下の8°以内にデバイスを取付ける必要があります。キャリブレーションを引き続きます、リセットはほとんど必要ありません。 - + Your device is pointed %1° %2 and %3° %4. このデバイスは%2の%1°、%4の%3°に向けます。 - + down 下 - + up 上 - + left 左 - + right 右 - + Are you sure you want to reboot? 再起動してもよろしいですか？ - + Disengage to Reboot openpilot をキャンセルして再起動ができます - + Are you sure you want to power off? シャットダウンしてもよろしいですか？ - + Disengage to Power Off openpilot をキャンセルしてシャットダウンができます @@ -294,7 +836,7 @@ DriverViewScene - + camera starting ｶﾒﾗを起動しています @@ -340,27 +882,27 @@ MapETA - + eta 予定到着時間 - + min 分 - + hr 時間 - + km ｷﾛﾒｰﾄﾙ - + mi ﾏｲﾙ @@ -368,22 +910,22 @@ MapInstructions - + km ｷﾛﾒｰﾄﾙ - + m ﾒｰﾄﾙ - + mi ﾏｲﾙ - + ft ﾌｨｰﾄ @@ -418,21 +960,21 @@ prime subscription. Sign up now: https://connect.comma.ai 詳しくはこちら：https://connect.comma.ai - + No home location set 自宅の住所はまだ 設定されていません - + No work location set 職場の住所はまだ 設定されていません - + no recent destinations 最近の目的地履歴がありません @@ -490,48 +1032,88 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX 最高速度 - - + + SPEED 速度 - - + + LIMIT 制限速度 + + + REAL STEER + + + + + RAM PER + + + + + OFF + + + + + ENG RPM + + + + + NOR + + + + + SPT + + + + + ECO + + + + + ACCEL + + OffroadHome - + UPDATE 更新 - + ALERTS 警告 - + ALERT 警告 @@ -539,22 +1121,22 @@ location set PairingPopup - + Pair your device to your comma account デバイスと comma アカウントを連携する - + Go to https://connect.comma.ai on your phone モバイルデバイスで「connect.comma.ai」にアクセスして - + Click "add new device" and scan the QR code on the right 「新しいデバイスを追加」を押すと、右側のQRコードをスキャンしてください - + Bookmark connect.comma.ai to your home screen to use it like an app 「connect.comma.ai」をホーム画面に追加して、アプリのように使うことができます @@ -562,32 +1144,32 @@ location set PrimeAdWidget - + Upgrade Now 今すぐｱｯﾌﾟｸﾞﾚｰﾄ - + Become a comma prime member at connect.comma.ai connect.comma.ai でﾌﾟﾗｲﾑ会員に登録できます - + PRIME FEATURES: 特典： - + Remote access リモートアクセス - + 1 year of storage 一年間の保存期間 - + Developer perks 開発者向け特典 @@ -595,22 +1177,22 @@ location set PrimeUserWidget - + ✓ SUBSCRIBED ✓ 入会しました - + comma prime comma prime - + CONNECT.COMMA.AI CONNECT.COMMA.AI - + COMMA POINTS COMMA POINTS @@ -628,14 +1210,12 @@ location set 閉じる - dashcam - ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ + ﾄﾞﾗｲﾌﾞﾚｺｰﾀﾞｰ - openpilot - openpilot + openpilot @@ -718,36 +1298,65 @@ location set SettingsWindow - × - × + × - + + BACK + + + + Device デバイス - - + + Network ﾈｯﾄﾜｰｸ - + Toggles 切り替え - + Software ｿﾌﾄｳｪｱ - + Navigation ﾅﾋﾞｹﾞｰｼｮﾝ + + + DP - General + + + + + DP - Controls + + + + + DP - UI + + + + + DP - Cars + + + + + DP - Maps + + Setup @@ -858,17 +1467,17 @@ location set SetupWidget - + Finish Setup セットアップ完了 - + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. デバイスを comma connect (connect.comma.ai)でペアリングし comma prime 特典を申請してください。 - + Pair device デバイスをペアリング @@ -888,7 +1497,7 @@ location set - + ONLINE オンライン @@ -898,84 +1507,81 @@ location set エラー - - + + TEMP 温度 - HIGH - 高温 + 高温 - GOOD - 最適 + 最適 - OK - OK + OK - + VEHICLE 車両 - + NO NO - + PANDA PANDA - + GPS GPS - + SEARCH 検索 - + -- -- - + Wi-Fi Wi-Fi - + ETH ETH - + 2G 2G - + 3G 3G - + LTE LTE - + 5G 5G @@ -983,89 +1589,89 @@ location set SoftwarePanel - + Git Branch Git ブランチ - + Git Commit Git コミット - + OS Version OS バージョン - + Version バージョン - + Last Update Check 最終更新確認 - + The last time openpilot successfully checked for an update. The updater only runs while the car is off. openpilotが最後にアップデートの確認に成功してからの時間です。アップデート処理は、車の電源が切れているときのみ実行されます。 - + Check for Update 更新プログラムをチェック - + CHECKING 確認中 - + Switch Branch - + ENTER - - + + The new branch will be pulled the next time the updater runs. - + Enter branch name - + UNINSTALL アンインストール - + Uninstall %1 %1をアンインストール - + Are you sure you want to uninstall? アンインストールしてもよろしいですか？ - + failed to fetch update 更新のﾀﾞｳﾝﾛｰﾄﾞにｴﾗｰが発生しました - - + + CHECK 確認 @@ -1153,82 +1759,92 @@ location set TogglesPanel - + Enable openpilot openpilot を有効化 - + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. アダプティブクルーズコントロールとレーンキーピングドライバーアシスト（openpilotシステム）。この機能を使用するには、常に注意が必要です。この設定を変更すると、車の電源が切れたときに有効になります。 - + Enable Lane Departure Warnings 車線逸脱警報機能を有効化 - + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 時速31マイル（50km）を超えるスピードで走行中、方向指示器を作動させずに検出された車線ライン上に車両が触れた場合、車線に戻るアラートを受信します。 - + + Enable Right-Hand Drive + + + + + Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat. + + + + Use Metric System メートル法を有効化 - + Display speed in km/h instead of mph. 速度は mph ではなく km/h で表示されます。 - + Record and Upload Driver Camera 車内カメラの録画とアップロード - + Upload data from the driver facing camera and help improve the driver monitoring algorithm. 車内カメラの映像をアップロードし、ドライバー監視システムのアルゴリズムの向上に役立てます。 - + Disengage On Accelerator Pedal ｱｸｾﾙ踏むと openpilot をｷｬﾝｾﾙ - + When enabled, pressing the accelerator pedal will disengage openpilot. 有効な場合は、アクセルを踏むと openpilot をキャンセルします。 - + Show ETA in 24h format 24時間表示 - + Use 24h format instead of am/pm AM/PM の代わりに24時間形式を使用します - + Show Map on Left Side of UI ﾃﾞｨｽﾌﾟﾚｲの左側にﾏｯﾌﾟを表示 - + Show map on left side when in split screen view. 分割画面表示の場合、ディスプレイの左側にマップを表示します。 - + openpilot Longitudinal Control openpilot 縦方向制御 - + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! openpilot は、車のレーダーを無効化し、アクセルとブレーキの制御を引き継ぎます。注意：AEB を無効化にします！ diff --git a/selfdrive/ui/translations/main_ko.qm b/selfdrive/ui/translations/main_ko.qm index 29236f76f..585e0c3c9 100644 Binary files a/selfdrive/ui/translations/main_ko.qm and b/selfdrive/ui/translations/main_ko.qm differ diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index cd9c73c34..5c52f38c7 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -73,6 +73,30 @@ 자동설정을 하려면 공백으로 두세요 + + C2NetworkPanel + + + Wi-Fi Settings + + + + + + OPEN + + + + + Tethering Settings + + + + + IP Address + IP 주소 + + ConfirmationDialog @@ -87,6 +111,524 @@ 취소 + + DPCarPanel + + + Override Speed When Below + + + + + Override feature will be enabled when set cruise speed is lower than this value. +1 km/h = 0.62 mph. + + + + + km/h + + + + + Turn On Cruise Speed Override + + + + + This feature will let you set your cruise speed below vehicle standard. (usually at 26~40 km/h) + + + + + Toyota / Lexus + + + + + Enable SnG Mod + + + + + Enable this to fix stop and go (SnG) issue on some models. +Reboot required. + + + + + Enable AM Physical Button Ctrl + + + + + Enable this to link Accel Mode (AM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES. +Reboot required. + + + + + Mazda + + + + + Display Below Steer Speed Alert + + + + + Enable this will show below steer speed alert. +Thanks to @TheCrowd + + + + + DPControlPanel + + + ALCA Delay + + + + + Once the vehicle meets all ALCA criteria, it will wait for the seconds set here before peforming lane change automatically. + + + + + secs + + + + + ALCA Min Speed + + + + + ALCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + + mph + + + + + LCA Min Speed + + + + + LCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + Lateral Ctrl Mode + + + + + 1 = Lane Change Assist (LCA) +2 = Auto Lane Change Assist (ALCA) + + + + + Longitudinal + + + + + Manually Control Accel Mode + + + + + Enable this if you wish to adjust openpilot's acceleration control. + + + + + Enable vision based turn control + + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + + Lateral + + + + + Use Lane Lines + + + + + Enable this if you wish to use lane lines mode. + + + + + Use LQR Controller + + + + + Enable this if you wish to use LQR instead of PID or INDI controller. +WORKS WELL ONLY ON SOME VEHICLES. +More linear steering experience. + + + + + Always On Lateral + + + + + Use at your own risk! +1 = Stock Long +2 = OP Long +Run Panda Recovery if Panda is not detected after reboot. +Reboot required. + + + + + DPGeneralPanel + + + Auto Shutdown In + + + + + Adjust your shutdown waiting period. +0 = shutdown immediately. + + + + + mins + + + + + Enable Auto Shutdown + + + + + Enable this if you wish to shutdown your device automatically. + + + + + Use Custom API Server + + + + + Enable this if you wish to connect to a custom API server. +Default to "https://api.retropilot.org/", change "dp_api_custom" if you want to change API server URL. +Reboot required. + + + + + Services + + + + + Enable On-Road Dashcam + + + + + Enable this if you wish to record screen, just like a dashcam. +Reboot required. + + + + + Enable GPS Logger + + + + + This will store your track in /data/media/0/gpx_logs/. +Reboot required. + + + + + Hardware - General + + + + + Enable Hotspot On Boot + + + + + Reboot required. + + + + + Miscellaneous + + + + + Enable Jetson Support + + + + + Enable this option if you intend to run dp on Nvidia Jetson. +Reboot required. + + + + + Reboot recommended. +Reboot? + + + + + Flashing Panda Firmware + + + + + FLASH + + + + + Tap the button to update your panda firmware. +The device should reboot once if it finish updating. + + + + + + + Are you sure? + + + + + Pandas Firmware Recovery + + + + + RECOVER + + + + + Tap the button ONLY if your panda ran into issue. + + + + + Delete All Driving Log + + + + + DELETE + + + + + Tap the button to delete ALL your driving logs (including dashcam / gpx / driving logs). + + + + + DPMapsPanel + + + Show Full Screen Nav. + + + + + This will show navigation in full screen. +Please tap green boarder if you wish to switch back drive view. + + + + + Search Destination using Google Map + + + + + This will allow you to search destination in google map api. +You will need to apply your own google map api key.Enter your key detail in web interface once it's enabled. + + + + + Enable Nav. + + + + + This will let use the build in Navigation. +Use web interface to control it: *http://<device_ip>:8082*. +You will need to apply your own mapbox token at https://www.mapbox.com/. +Internet access from mobile phone (tethering) is required. +Reboot required. + + + + + Enable Speed Limit Control + + + + + Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits. + + + + + Enable Speed Limit Offset + + + + + Set speed limit slightly higher than actual speed limit for a more natural drive. + + + + + Enable Map Data Turn Control + + + + + Use curvature info from map data to define speed limits to take turns ahead + + + + + Show debug UI elements + + + + + Show UI elements that aid debugging. + + + + + Enable MapD + + + + + Use OSM to assist lateral/longitudinal control. +Please note: +1. This feature will works only when your car support OP longitudinal. +2. MapD will contribute your route to OSM for future improvement automatically. +3. You can add your own offset for mapd just follow the readme under /selfdrive/mapd. +4. Not connecting to the internet for while might feel up device storage from all the gps traces. + + + + + DPUIPanel + + + Display Mode + + + + + 0 = Default +1 = Screen Off While Driving + + + + + Screen Brightness + + + + + Adjust your screen brightness. + + + + + + AUTO + + + + + Alert Volume + + + + + Adjust your alert volume. + + + + + Quiet Drive + + + + + Display alert and play important warning sound. Thanks @sunnyhaibin. + + + + + Display Side Info + + + + + Enable this to display steering angle / lead car distance / engine RPM. + + + + + Display Top Info + + + + + Enable this to display time / system temp / battery level. + + + DeclinePage @@ -108,152 +650,152 @@ DevicePanel - + Dongle ID Dongle ID - + N/A N/A - + Serial Serial - + Driver Camera 운전자 카메라 - + PREVIEW 미리보기 - + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 운전자 모니터링이 좋은 가시성을 갖도록 운전자를 향한 카메라를 미리 봅니다. (차량연결은 해제되어있어야 합니다) - + Reset Calibration 캘리브레이션 재설정 - + RESET 재설정 - + Are you sure you want to reset calibration? 캘리브레이션을 재설정하시겠습니까? - + Review Training Guide 트레이닝 가이드 다시보기 - + REVIEW 다시보기 - + Review the rules, features, and limitations of openpilot openpilot의 규칙, 기능 및 제한 다시보기 - + Are you sure you want to review the training guide? 트레이닝 가이드를 다시보시겠습니까? - + Regulatory 규제 - + VIEW 보기 - + Change Language 언어 변경 - + CHANGE 변경 - + Select a language 언어를 선택하세요 - + Reboot 재부팅 - + Power Off 전원 종료 - + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. openpilot은 장치를 좌측 또는 우측은 4° 이내, 위쪽 5° 또는 아래쪽은 8° 이내로 설치해야 합니다. openpilot은 지속적으로 보정되므로 리셋이 거의 필요하지 않습니다. - + Your device is pointed %1° %2 and %3° %4. 사용자의 장치가 %1° %2 및 %3° %4를 가리키고 있습니다. - + down 아래로 - + up 위로 - + left 좌측으로 - + right 우측으로 - + Are you sure you want to reboot? 재부팅 하시겠습니까? - + Disengage to Reboot 재부팅 하려면 해제하세요 - + Are you sure you want to power off? 전원을 종료하시겠습니까? - + Disengage to Power Off 전원을 종료하려면 해제하세요 @@ -294,7 +836,7 @@ DriverViewScene - + camera starting 카메라 시작중 @@ -340,27 +882,27 @@ MapETA - + eta 도착 - + min 분 - + hr 시간 - + km km - + mi mi @@ -368,22 +910,22 @@ MapInstructions - + km km - + m m - + mi mi - + ft ft @@ -418,21 +960,21 @@ prime subscription. Sign up now: https://connect.comma.ai 등록：https://connect.comma.ai - + No home location set 집 설정되지않음 - + No work location set 회사 설정되지않음 - + no recent destinations 최근 목적지 없음 @@ -490,48 +1032,88 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX MAX - - + + SPEED SPEED - - + + LIMIT LIMIT + + + REAL STEER + + + + + RAM PER + + + + + OFF + + + + + ENG RPM + + + + + NOR + + + + + SPT + + + + + ECO + + + + + ACCEL + + OffroadHome - + UPDATE 업데이트 - + ALERTS 알림 - + ALERT 알림 @@ -539,22 +1121,22 @@ location set PairingPopup - + Pair your device to your comma account 장치를 콤마 계정과 페어링합니다 - + Go to https://connect.comma.ai on your phone https://connect.comma.ai에 접속하세요 - + Click "add new device" and scan the QR code on the right "새 장치 추가"를 클릭하고 오른쪽 QR 코드를 검색합니다 - + Bookmark connect.comma.ai to your home screen to use it like an app connect.comma.ai을 앱처럼 사용하려면 홈 화면에 바로가기를 만드십시오 @@ -562,32 +1144,32 @@ location set PrimeAdWidget - + Upgrade Now 지금 업그레이드 - + Become a comma prime member at connect.comma.ai connect.comma.ai에서 comma prime에 가입합니다 - + PRIME FEATURES: PRIME 기능： - + Remote access 원격 접속 - + 1 year of storage 1년간 저장 - + Developer perks 개발자 혜택 @@ -595,22 +1177,22 @@ location set PrimeUserWidget - + ✓ SUBSCRIBED ✓ 구독함 - + comma prime comma prime - + CONNECT.COMMA.AI CONNECT.COMMA.AI - + COMMA POINTS COMMA POINTS @@ -628,14 +1210,12 @@ location set 종료 - dashcam - dashcam + dashcam - openpilot - openpilot + openpilot @@ -718,36 +1298,65 @@ location set SettingsWindow - × - × + × + + + + BACK + - + Device 장치 - - + + Network 네트워크 - + Toggles 토글 - + Software 소프트웨어 - + Navigation 네비게이션 + + + DP - General + + + + + DP - Controls + + + + + DP - UI + + + + + DP - Cars + + + + + DP - Maps + + Setup @@ -858,17 +1467,17 @@ location set SetupWidget - + Finish Setup 설정 완료 - + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 장치를 (connect.comma.ai)에서 페어링하고 comma prime 오퍼를 청구합니다. - + Pair device 장치 페어링 @@ -888,7 +1497,7 @@ location set - + ONLINE 온라인 @@ -898,84 +1507,81 @@ location set 오류 - - + + TEMP 온도 - HIGH - 높음 + 높음 - GOOD - 좋음 + 좋음 - OK - 경고 + 경고 - + VEHICLE 차량 - + NO NO - + PANDA PANDA - + GPS GPS - + SEARCH 검색중 - + -- -- - + Wi-Fi Wi-Fi - + ETH 이더넷 - + 2G 2G - + 3G 3G - + LTE LTE - + 5G 5G @@ -983,89 +1589,89 @@ location set SoftwarePanel - + Git Branch Git 브렌치 - + Git Commit Git 커밋 - + OS Version OS 버전 - + Version 버전 - + Last Update Check 최신 업데이트 검사 - + The last time openpilot successfully checked for an update. The updater only runs while the car is off. 최근에 openpilot이 업데이트를 성공적으로 확인했습니다. 업데이트 프로그램은 차량 연결이 해제되었을때만 작동합니다. - + Check for Update 업데이트 확인 - + CHECKING 확인중 - + Switch Branch 브랜치 변경 - + ENTER 입력하세요 - - + + The new branch will be pulled the next time the updater runs. 다음 업데이트 프로그램이 실행될 때 새 브랜치가 적용됩니다. - + Enter branch name 브랜치명 입력 - + UNINSTALL 제거 - + Uninstall %1 %1 제거 - + Are you sure you want to uninstall? 제거하시겠습니까? - + failed to fetch update 업데이트를 가져올수없습니다 - - + + CHECK 확인 @@ -1153,82 +1759,92 @@ location set TogglesPanel - + Enable openpilot openpilot 사용 - + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. 어댑티브 크루즈 컨트롤 및 차선 유지 운전자 보조를 위해 openpilot 시스템을 사용하십시오. 이 기능을 사용하려면 항상 주의를 기울여야 합니다. 이 설정을 변경하면 차량 전원이 꺼질 때 적용됩니다. - + Enable Lane Departure Warnings 차선 이탈 경고 사용 - + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향 지시등이 활성화되지 않은 상태에서 감지된 차선 위를 주행할 경우 차선이탈 경고를 사용합니다. - + + Enable Right-Hand Drive + + + + + Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat. + + + + Use Metric System 미터법 사용 - + Display speed in km/h instead of mph. mph 대신 km/h로 속도를 표시합니다. - + Record and Upload Driver Camera 운전자 카메라 녹화 및 업로드 - + Upload data from the driver facing camera and help improve the driver monitoring algorithm. 운전자 카메라에서 데이터를 업로드하고 운전자 모니터링 알고리즘을 개선합니다. - + Disengage On Accelerator Pedal 가속페달 조작시 해제 - + When enabled, pressing the accelerator pedal will disengage openpilot. 활성화된 경우 가속 페달을 누르면 openpilot이 해제됩니다. - + Show ETA in 24h format 24시간 형식으로 도착예정시간 표시 - + Use 24h format instead of am/pm 오전/오후 대신 24시간 형식 사용 - + Show Map on Left Side of UI UI 왼쪽에 지도 표시 - + Show map on left side when in split screen view. 분할 화면 보기에서 지도를 왼쪽에 표시합니다. - + openpilot Longitudinal Control openpilot Longitudinal Control - + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! openpilot은 차량'의 레이더를 무력화시키고 가속페달과 브레이크의 제어를 인계받을 것이다. 경고: AEB를 비활성화합니다! diff --git a/selfdrive/ui/translations/main_zh-CHS.qm b/selfdrive/ui/translations/main_zh-CHS.qm index 8f7f61d6d..9f15451c1 100644 Binary files a/selfdrive/ui/translations/main_zh-CHS.qm and b/selfdrive/ui/translations/main_zh-CHS.qm differ diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index cb09e8c2e..5fecd390a 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -73,6 +73,30 @@ 留空以自动配置 + + C2NetworkPanel + + + Wi-Fi Settings + + + + + + OPEN + + + + + Tethering Settings + + + + + IP Address + IP地址 + + ConfirmationDialog @@ -87,6 +111,524 @@ 取消 + + DPCarPanel + + + Override Speed When Below + + + + + Override feature will be enabled when set cruise speed is lower than this value. +1 km/h = 0.62 mph. + + + + + km/h + + + + + Turn On Cruise Speed Override + + + + + This feature will let you set your cruise speed below vehicle standard. (usually at 26~40 km/h) + + + + + Toyota / Lexus + + + + + Enable SnG Mod + + + + + Enable this to fix stop and go (SnG) issue on some models. +Reboot required. + + + + + Enable AM Physical Button Ctrl + + + + + Enable this to link Accel Mode (AM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES. +Reboot required. + + + + + Mazda + + + + + Display Below Steer Speed Alert + + + + + Enable this will show below steer speed alert. +Thanks to @TheCrowd + + + + + DPControlPanel + + + ALCA Delay + + + + + Once the vehicle meets all ALCA criteria, it will wait for the seconds set here before peforming lane change automatically. + + + + + secs + + + + + ALCA Min Speed + + + + + ALCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + + mph + + + + + LCA Min Speed + + + + + LCA minimum engage speed in mph. +1 mph = 1.61 km/h. + + + + + Lateral Ctrl Mode + + + + + 1 = Lane Change Assist (LCA) +2 = Auto Lane Change Assist (ALCA) + + + + + Longitudinal + + + + + Manually Control Accel Mode + + + + + Enable this if you wish to adjust openpilot's acceleration control. + + + + + Enable vision based turn control + + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + + + + + Lateral + + + + + Use Lane Lines + + + + + Enable this if you wish to use lane lines mode. + + + + + Use LQR Controller + + + + + Enable this if you wish to use LQR instead of PID or INDI controller. +WORKS WELL ONLY ON SOME VEHICLES. +More linear steering experience. + + + + + Always On Lateral + + + + + Use at your own risk! +1 = Stock Long +2 = OP Long +Run Panda Recovery if Panda is not detected after reboot. +Reboot required. + + + + + DPGeneralPanel + + + Auto Shutdown In + + + + + Adjust your shutdown waiting period. +0 = shutdown immediately. + + + + + mins + + + + + Enable Auto Shutdown + + + + + Enable this if you wish to shutdown your device automatically. + + + + + Use Custom API Server + + + + + Enable this if you wish to connect to a custom API server. +Default to "https://api.retropilot.org/", change "dp_api_custom" if you want to change API server URL. +Reboot required. + + + + + Services + + + + + Enable On-Road Dashcam + + + + + Enable this if you wish to record screen, just like a dashcam. +Reboot required. + + + + + Enable GPS Logger + + + + + This will store your track in /data/media/0/gpx_logs/. +Reboot required. + + + + + Hardware - General + + + + + Enable Hotspot On Boot + + + + + Reboot required. + + + + + Miscellaneous + + + + + Enable Jetson Support + + + + + Enable this option if you intend to run dp on Nvidia Jetson. +Reboot required. + + + + + Reboot recommended. +Reboot? + + + + + Flashing Panda Firmware + + + + + FLASH + + + + + Tap the button to update your panda firmware. +The device should reboot once if it finish updating. + + + + + + + Are you sure? + + + + + Pandas Firmware Recovery + + + + + RECOVER + + + + + Tap the button ONLY if your panda ran into issue. + + + + + Delete All Driving Log + + + + + DELETE + + + + + Tap the button to delete ALL your driving logs (including dashcam / gpx / driving logs). + + + + + DPMapsPanel + + + Show Full Screen Nav. + + + + + This will show navigation in full screen. +Please tap green boarder if you wish to switch back drive view. + + + + + Search Destination using Google Map + + + + + This will allow you to search destination in google map api. +You will need to apply your own google map api key.Enter your key detail in web interface once it's enabled. + + + + + Enable Nav. + + + + + This will let use the build in Navigation. +Use web interface to control it: *http://<device_ip>:8082*. +You will need to apply your own mapbox token at https://www.mapbox.com/. +Internet access from mobile phone (tethering) is required. +Reboot required. + + + + + Enable Speed Limit Control + + + + + Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits. + + + + + Enable Speed Limit Offset + + + + + Set speed limit slightly higher than actual speed limit for a more natural drive. + + + + + Enable Map Data Turn Control + + + + + Use curvature info from map data to define speed limits to take turns ahead + + + + + Show debug UI elements + + + + + Show UI elements that aid debugging. + + + + + Enable MapD + + + + + Use OSM to assist lateral/longitudinal control. +Please note: +1. This feature will works only when your car support OP longitudinal. +2. MapD will contribute your route to OSM for future improvement automatically. +3. You can add your own offset for mapd just follow the readme under /selfdrive/mapd. +4. Not connecting to the internet for while might feel up device storage from all the gps traces. + + + + + DPUIPanel + + + Display Mode + + + + + 0 = Default +1 = Screen Off While Driving + + + + + Screen Brightness + + + + + Adjust your screen brightness. + + + + + + AUTO + + + + + Alert Volume + + + + + Adjust your alert volume. + + + + + Quiet Drive + + + + + Display alert and play important warning sound. Thanks @sunnyhaibin. + + + + + Display Side Info + + + + + Enable this to display steering angle / lead car distance / engine RPM. + + + + + Display Top Info + + + + + Enable this to display time / system temp / battery level. + + + DeclinePage @@ -108,152 +650,152 @@ DevicePanel - + Dongle ID 设备ID（Dongle ID） - + N/A N/A - + Serial 序列号 - + Driver Camera 驾驶员摄像头 - + PREVIEW 预览 - + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 打开并预览驾驶员摄像头，以确保驾驶员监控具有良好视野。仅熄火时可用。 - + Reset Calibration 重置设备校准 - + RESET 重置 - + Are you sure you want to reset calibration? 您确定要重置设备校准吗？ - + Review Training Guide 新手指南 - + REVIEW 查看 - + Review the rules, features, and limitations of openpilot 查看openpilot的使用规则，以及其功能和限制。 - + Are you sure you want to review the training guide? 您确定要查看新手指南吗？ - + Regulatory 监管信息 - + VIEW 查看 - + Change Language 切换语言 - + CHANGE 切换 - + Select a language 选择语言 - + Reboot 重启 - + Power Off 关机 - + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. openpilot要求设备安装的偏航角在左4°和右4°之间，俯仰角在上5°和下8°之间。一般来说，openpilot会持续更新校准，很少需要重置。 - + Your device is pointed %1° %2 and %3° %4. 您的设备校准为%1° %2、%3° %4。 - + down 朝下 - + up 朝上 - + left 朝左 - + right 朝右 - + Are you sure you want to reboot? 您确定要重新启动吗？ - + Disengage to Reboot 取消openpilot以重新启动 - + Are you sure you want to power off? 您确定要关机吗？ - + Disengage to Power Off 取消openpilot以关机 @@ -294,7 +836,7 @@ DriverViewScene - + camera starting 正在启动相机 @@ -340,27 +882,27 @@ MapETA - + eta 埃塔 - + min 分钟 - + hr 小时 - + km km - + mi mi @@ -368,22 +910,22 @@ MapInstructions - + km km - + m m - + mi mi - + ft ft @@ -418,19 +960,19 @@ prime subscription. Sign up now: https://connect.comma.ai 立即注册：https://connect.comma.ai - + No home location set 家：未设定 - + No work location set 工作：未设定 - + no recent destinations 无最近目的地 @@ -488,48 +1030,88 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX 最高定速 - - + + SPEED SPEED - - + + LIMIT LIMIT + + + REAL STEER + + + + + RAM PER + + + + + OFF + + + + + ENG RPM + + + + + NOR + + + + + SPT + + + + + ECO + + + + + ACCEL + + OffroadHome - + UPDATE 更新 - + ALERTS 警报 - + ALERT 警报 @@ -537,22 +1119,22 @@ location set PairingPopup - + Pair your device to your comma account 将您的设备与comma账号配对 - + Go to https://connect.comma.ai on your phone 在手机上访问 https://connect.comma.ai - + Click "add new device" and scan the QR code on the right 点击“添加新设备”，扫描右侧二维码 - + Bookmark connect.comma.ai to your home screen to use it like an app 将 connect.comma.ai 收藏到您的主屏幕，以便像应用程序一样使用它 @@ -560,32 +1142,32 @@ location set PrimeAdWidget - + Upgrade Now 现在升级 - + Become a comma prime member at connect.comma.ai 打开connect.comma.ai以注册comma prime会员 - + PRIME FEATURES: comma prime特权： - + Remote access 远程访问 - + 1 year of storage 1年数据存储 - + Developer perks 开发者福利 @@ -593,22 +1175,22 @@ location set PrimeUserWidget - + ✓ SUBSCRIBED ✓ 已订阅 - + comma prime comma prime - + CONNECT.COMMA.AI CONNECT.COMMA.AI - + COMMA POINTS COMMA POINTS点数 @@ -626,14 +1208,12 @@ location set 退出 - dashcam - 行车记录仪 + 行车记录仪 - openpilot - openpilot + openpilot @@ -716,36 +1296,65 @@ location set SettingsWindow - × - × + × + + + + BACK + - + Device 设备 - - + + Network 网络 - + Toggles 设定 - + Software 软件 - + Navigation 导航 + + + DP - General + + + + + DP - Controls + + + + + DP - UI + + + + + DP - Cars + + + + + DP - Maps + + Setup @@ -856,17 +1465,17 @@ location set SetupWidget - + Finish Setup 完成设置 - + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 将您的设备与comma connect （connect.comma.ai）配对并领取您的comma prime优惠。 - + Pair device 配对设备 @@ -886,7 +1495,7 @@ location set - + ONLINE 在线 @@ -896,84 +1505,81 @@ location set 连接出错 - - + + TEMP 设备温度 - HIGH - 过热 + 过热 - GOOD - 良好 + 良好 - OK - 一般 + 一般 - + VEHICLE 车辆连接 - + NO 无 - + PANDA PANDA - + GPS GPS - + SEARCH 搜索中 - + -- -- - + Wi-Fi Wi-Fi - + ETH 以太网 - + 2G 2G - + 3G 3G - + LTE LTE - + 5G 5G @@ -981,89 +1587,89 @@ location set SoftwarePanel - + Git Branch Git Branch - + Git Commit Git Commit - + OS Version 系统版本 - + Version 软件版本 - + Last Update Check 上次检查更新 - + The last time openpilot successfully checked for an update. The updater only runs while the car is off. 上一次成功检查更新的时间。更新程序仅在汽车熄火时运行。 - + Check for Update 检查更新 - + CHECKING 正在检查更新 - + Switch Branch 切换分支 - + ENTER 输入 - - + + The new branch will be pulled the next time the updater runs. 分支将在更新服务下次启动时自动切换。 - + Enter branch name 输入分支名称 - + UNINSTALL 卸载 - + Uninstall %1 卸载 %1 - + Are you sure you want to uninstall? 您确定要卸载吗？ - + failed to fetch update 获取更新失败 - - + + CHECK 查看 @@ -1151,82 +1757,92 @@ location set TogglesPanel - + Enable openpilot 启用openpilot - + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. 使用openpilot进行自适应巡航和车道保持辅助。使用此功能时您必须时刻保持注意力。该设置的更改在熄火时生效。 - + Enable Lane Departure Warnings 启用车道偏离警告 - + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 车速超过31mph（50km/h）时，若检测到车辆越过车道线且未打转向灯，系统将发出警告以提醒您返回车道。 - + + Enable Right-Hand Drive + + + + + Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat. + + + + Use Metric System 使用公制单位 - + Display speed in km/h instead of mph. 显示车速时，以km/h代替mph。 - + Record and Upload Driver Camera 录制并上传驾驶员摄像头 - + Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上传驾驶员摄像头的数据，帮助改进驾驶员监控算法。 - + Disengage On Accelerator Pedal 踩油门时取消控制 - + When enabled, pressing the accelerator pedal will disengage openpilot. 启用后，踩下油门踏板将取消openpilot。 - + Show ETA in 24h format 以24小时格式显示预计到达时间 - + Use 24h format instead of am/pm 使用24小时制代替am/pm - + Show Map on Left Side of UI 在介面左侧显示地图 - + Show map on left side when in split screen view. 在分屏模式中，将地图置于屏幕左侧。 - + openpilot Longitudinal Control openpilot纵向控制 - + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! openpilot将禁用车辆的雷达并接管油门和刹车的控制。警告：AEB将被禁用！ diff --git a/selfdrive/ui/translations/main_zh-CHT.qm b/selfdrive/ui/translations/main_zh-CHT.qm index 78ec901c9..ba9aa5a47 100644 Binary files a/selfdrive/ui/translations/main_zh-CHT.qm and b/selfdrive/ui/translations/main_zh-CHT.qm differ diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 3980fc90b..354cf59bc 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -111,6 +111,556 @@ 取消 + + DPCarPanel + + + Override Speed When Below + 改寫定速當定速值低於： + + + + Override feature will be enabled when set cruise speed is lower than this value. +1 km/h = 0.62 mph. + 系統將會改寫定速值當低於此指定的值。 +1 km/h = 0.62 mph。 + + + + km/h + + + + + Turn On Cruise Speed Override + 啟用巡航速度改寫 + + + + This feature will let you set your cruise speed below vehicle standard. (usually at 26~40 km/h) + 開啟這個選項後，您將可以把定速至最低 5km/hr (一般最低為 26～40km/h) + + + + Toyota / Lexus + + + + + Enable SnG Mod + 啟用 SnG 協助 + + + + Enable this to fix stop and go (SnG) issue on some models. +Reboot required. + 開啟這個選項可以解決某些車型無法跟車到停後再啟動（SnG）的問題 +需要重新啟動。 + + + + Enable AM Physical Button Ctrl + 啟用加速模式和方向盤實體按鈕關聯 + + + + Enable this to link Accel Mode (AM) control to the physical button (TSS2). +ONLY WORK ON SOME OF TSS1 VEHICLES. +Reboot required. + 開啟這個選項，將加速模式的選擇和方向盤實體按鈕進行關聯，主要使用於TSS2車型， +在某些TSS1車型上也可以使用。 +需重新啟動。 + + + + Mazda + + + + + Display Below Steer Speed Alert + 顯示低速轉向失效提示 + + + + Enable this will show below steer speed alert. +Thanks to @TheCrowd + 啟用這個選項後，將在車速低於可輔助轉向的速度時，顯示提示 + + + + DPControlPanel + + + ALCA Delay + 設置自動換道延遲時間 + + + + Once the vehicle meets all ALCA criteria, it will wait for the seconds set here before peforming lane change automatically. + 一旦車輛滿足所有自動換道的啟動需求，它將會等待設定的秒數後再執行換道。 + + + + secs + 秒 + + + + ALCA Min Speed + 自動換道最低啟用速度 + + + + ALCA minimum engage speed in mph. +1 mph = 1.61 km/h. + 自動換道啟用速度 (單位為每小時英里)。 +1 英里 (mph) 約等於 1.61 公里 (km/h)。 + + + + + mph + 英里 + + + + LCA Min Speed + 輔助換道最低啟用速度 + + + + LCA minimum engage speed in mph. +1 mph = 1.61 km/h. + 輔助換道啟用速度 (單位為每小時英里)。 +1 英里 (mph) 約等於 1.61 公里 (km/h)。 + + + + Lateral Ctrl Mode + 橫向控制模式 + + + + 1 = Lane Change Assist (LCA) +2 = Auto Lane Change Assist (ALCA) + 1 = 輔助換道 (LCA) +2 = 自動換道 (ALCA) + + + + Longitudinal + 縱向控制 (油門/剎車) + + + + Manually Control Accel Mode + 手動控制加速模式 + + + + Enable this if you wish to adjust openpilot's acceleration control. + 啟用這個選項後您可以調整 openpilot 的加速模式 (節能、一般、運動)。 + + + + Enable vision based turn control + 啟用視覺模型路徑控制車速 + + + + Use vision path predictions to estimate the appropriate speed to drive through turns ahead. + 使用視覺模型預測的路徑來判斷前方的過彎速度。 + + + + Lateral + 橫向控制 (方向盤) + + + + Use Lane Lines + 使用車道線 + + + + Enable this if you wish to use lane lines mode. + 啟用這個選項後，模型將會以車道線判斷行駛路線。 + + + + Use LQR Controller + 使用 LQR 控制器 + + + + Enable this if you wish to use LQR instead of PID or INDI controller. +WORKS WELL ONLY ON SOME VEHICLES. +More linear steering experience. + 開啟這個選項後，系統將會強制使用 LQR 橫向控制器。 +只有在某些車型上會有更好的體驗。 +LQR 提供比 PID、INDI 更線性的控制。 + + + + Always On Lateral + 全時置中 + + + + Use at your own risk! +1 = Stock Long +2 = OP Long +Run Panda Recovery if Panda is not detected after reboot. +Reboot required. + 使用風險自負。 +1 = 使用原車 ACC +2 = 使用 op ACC +需重新啟動，如果遇到不能連線問題請使用恢復恢復 Panda 韌體功能。 + + + + DPGeneralPanel + + + Auto Shutdown In + 熄火後自動關機： + + + + Adjust your shutdown waiting period. +0 = shutdown immediately. + 調整自動關機時間。0 = 馬上關機 + + + + mins + 分鐘 + + + + Enable Auto Shutdown + 啟用自動關機 + + + + Enable this if you wish to shutdown your device automatically. + 啟用這個選項後，您的設備將會自動關機。 + + + + Use Custom API Server + 使用定制的 API 服務器 + + + + Enable this if you wish to connect to a custom API server. +Default to "https://api.retropilot.org/", change "dp_api_custom" if you want to change API server URL. +Reboot required. + 開啟這個選項，可以讓你鏈接到定製dp的API服務器 +默認地址"https://api.retropilot.org/", 修改為 "dp_api_custom" 如果您想要修改 API 服務器的網址. +需要重新啟動。 + + + + Services + 服務 + + + + Enable On-Road Dashcam + 啟用行車記錄功能 + + + + Enable this if you wish to record screen, just like a dashcam. +Reboot required. + 開啟這個選項錄制目前的行車畫面。 +需重新啟動。 + + + + Enable GPS Logger + 啟用 GPS 記錄服務 + + + + This will store your track in /data/media/0/gpx_logs/. +Reboot required. + 啟用這個選項後將會把您的 GPS 行車路徑記錄至 /data/media/0/gpx_logs/ 裡。 +需重新啟動。 + + + + Hardware - General + 硬體 - 一般 + + + + Enable Hotspot On Boot + 啟用開機自動開啟熱點 + + + + Reboot required. + 需要重新啟動。 + + + + Miscellaneous + 其它 + + + + Enable Jetson Support + 啟用 Jetson 支持 + + + + Enable this option if you intend to run dp on Nvidia Jetson. +Reboot required. + 如果您打算在 Nvidia Jetson NX 上跑 dp，請開啟這選項。 +需重新啟動。 + + + + Reboot recommended. +Reboot? + 我們建議您重新啟動。 +要重新啟動嗎？ + + + + Flashing Panda Firmware + 刷新 Panda 韌體 + + + + FLASH + 刷寫 + + + + Tap the button to update your panda firmware. +The device should reboot once if it finish updating. + 點擊右方按鈕來刷寫更新 Panda 的韌體。 +你的裝置將在刷寫完成後自動重新開機。 + + + + + + Are you sure? + 您確定嗎？ + + + + Pandas Firmware Recovery + 恢復 Panda 韌體 + + + + RECOVER + 恢復 + + + + Tap the button ONLY if your panda ran into issue. + 僅當您的 Panda 遇到問題時才點擊按鈕。 + + + + Delete All Driving Log + 刪除所有的行駛記錄 + + + + DELETE + 刪除 + + + + Tap the button to delete ALL your driving logs (including dashcam / gpx / driving logs). + 點擊按鈕來清空您所有的行駛記錄（包含 dashcam / gpx 等等）。 + + + + DPMapsPanel + + + Show Full Screen Nav. + 啟用全螢幕道航 + + + + This will show navigation in full screen. +Please tap green boarder if you wish to switch back drive view. + 這將會把導航畫面擴展至全螢幕。 +如果想切換回行車畫面，請點擊或滑動綠色邊框區域。 + + + + Search Destination using Google Map + 使用 Google Map 搜尋目的地 + + + + This will allow you to search destination in google map api. +You will need to apply your own google map api key.Enter your key detail in web interface once it's enabled. + 這將讓您使用 google map API 來搜尋目的地。 +您將需要申請自已的 google map api 密鑰，申請完後進入網頁介面輸入即可。 + + + + Enable Nav. + 啟用導航服務 + + + + This will let use the build in Navigation. +Use web interface to control it: *http://<device_ip>:8082*. +You will need to apply your own mapbox token at https://www.mapbox.com/. +Internet access from mobile phone (tethering) is required. +Reboot required. + 啟用這個功能將可以讓您使用內建的導航系統。 +請掃描二維碼或是連至 http://[設備 IP]:8082 網頁介面控制它。 +請至 mapbox (https://www.mapbox.com) 申請您自已的密鑰。 +您的裝置需要有個人熱點連線。 +需要重新開機。 + + + + Enable Speed Limit Control + 啟用速限控制 + + + + Use speed limit signs information from map data and car interface to automatically adapt cruise speed to road limits. + 依照圖資上的速限控制您的車速。 + + + + Enable Speed Limit Offset + 啟用限速控制偏移 + + + + Set speed limit slightly higher than actual speed limit for a more natural drive. + 將速度限製設置為略高於實際速度限制，實現更自然的駕駛。 + + + + Enable Map Data Turn Control + 啟用圖資路徑控制車速 + + + + Use curvature info from map data to define speed limits to take turns ahead + 使用圖資上的路徑來判斷前方的過彎速度 +使用速度顯示為UI中左下方三角形內數值 + + + + Show debug UI elements + 顯示除錯用圖示 + + + + Show UI elements that aid debugging. + 於右上方顯示除錯用訊息 + + + + Enable MapD + 啟用 MapD 服務 + + + + Use OSM to assist lateral/longitudinal control. +Please note: +1. This feature will works only when your car support OP longitudinal. +2. MapD will contribute your route to OSM for future improvement automatically. +3. You can add your own offset for mapd just follow the readme under /selfdrive/mapd. +4. Not connecting to the internet for while might feel up device storage from all the gps traces. + 使用 OpenStreetMap (OSM) 圖資來協助縱向控制。 +請注意： +1. 您的車需要 OP 縱向控制的支援。 +2. MapD 將會把您的行車路線貢獻給 OSM 以供日後改進。 +3. 若有需要，請按照 /selfdrive/mapd/ 下的自述文件調整修改參數。 +4. 在沒有連上網的情況下儲存的 gpx 行車路線記錄有可能會塞滿存儲空間。 + + + + DPUIPanel + + + Display Mode + 顯示模式 + + + + 0 = Default +1 = Screen Off While Driving + 0 = 預設 +1 = 行駛時關閉畫面 + + + + Screen Brightness + 螢幕亮度 + + + + Adjust your screen brightness. + 調整螢幕的亮度。 + + + + + AUTO + 自動 + + + + Alert Volume + 提示音量 + + + + Adjust your alert volume. + 調整設備的提示音量。 + + + + Quiet Drive + 靜音模式 + + + + Display alert and play important warning sound. Thanks @sunnyhaibin. + 開啟這個選項後，設備只會在重要的警示時顯示和發出聲響。謝謝 @sunnyhaibin. + + + + Display Side Info + 顯示側邊資訊欄 + + + + Enable this to display steering angle / lead car distance / engine RPM. + 開啟這個選項後，畫面側邊將會顯示額外的訊息。 + + + + Display Top Info + 顯示上方資訊欄 + + + + Enable this to display time / system temp / battery level. + 開啟這個選項後，畫面將會顯示額外的訊息。 + + DeclinePage @@ -132,152 +682,152 @@ DevicePanel - + Dongle ID Dongle ID - + N/A 無法使用 - + Serial 序號 - + Driver Camera 駕駛員攝像頭 - + PREVIEW 預覽 - + Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) 預覽駕駛員監控鏡頭畫面，以確保其具有良好視野。（僅在熄火時可用） - + Reset Calibration 重置校準 - + RESET 重置 - + Are you sure you want to reset calibration? 您確定要重置校準嗎？ - + Review Training Guide 觀看使用教學 - + REVIEW 觀看 - + Review the rules, features, and limitations of openpilot 觀看 openpilot 的使用規則、功能和限制 - + Are you sure you want to review the training guide? 您確定要觀看使用教學嗎？ - + Regulatory 法規/監管 - + VIEW 觀看 - + Change Language 更改語言 - + CHANGE 更改 - + Select a language 選擇語言 - + Reboot 重新啟動 - + Power Off 關機 - + openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. openpilot 需要將裝置固定在左右偏差 4° 以內，朝上偏差 5° 以内或朝下偏差 8° 以内。鏡頭在後台會持續自動校準，很少有需要重置的情况。 - + Your device is pointed %1° %2 and %3° %4. 你的設備目前朝%2 %1° 以及朝%4 %3° 。 - + down 下 - + up 上 - + left 左 - + right 右 - + Are you sure you want to reboot? 您確定要重新啟動嗎？ - + Disengage to Reboot 請先取消控車才能重新啟動 - + Are you sure you want to power off? 您確定您要關機嗎？ - + Disengage to Power Off 請先取消控車才能關機 @@ -318,7 +868,7 @@ DriverViewScene - + camera starting 開啟相機中 @@ -364,27 +914,27 @@ MapETA - + eta 抵達 - + min 分鐘 - + hr 小時 - + km km - + mi mi @@ -392,22 +942,22 @@ MapInstructions - + km km - + m m - + mi mi - + ft ft @@ -442,21 +992,21 @@ prime subscription. Sign up now: https://connect.comma.ai 立即註冊：https://connect.comma.ai - + No home location set 未設定 住家位置 - + No work location set 未設定 工作位置 - + no recent destinations 沒有最近的導航記錄 @@ -514,70 +1064,70 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX 最高 - - + + SPEED 速度 - - + + LIMIT 速限 - + REAL STEER 實際轉角 - + RAM PER - + 記憶體用量 - + OFF 停止 - + ENG RPM 引擎轉速 - + NOR 一般 - + SPT 運動 - + ECO 節能 - + ACCEL 加速 @@ -603,22 +1153,22 @@ location set PairingPopup - + Pair your device to your comma account 將設備與您的 comma 帳號配對 - + Go to https://connect.comma.ai on your phone 用手機連至 https://connect.comma.ai - + Click "add new device" and scan the QR code on the right 點選 "add new device" 後掃描右邊的二維碼 - + Bookmark connect.comma.ai to your home screen to use it like an app 將 connect.comma.ai 加入您的主屏幕，以便像手機 App 一樣使用它 @@ -626,32 +1176,32 @@ location set PrimeAdWidget - + Upgrade Now 馬上升級 - + Become a comma prime member at connect.comma.ai 成為 connect.comma.ai 的高級會員 - + PRIME FEATURES: 高級會員特點： - + Remote access 遠程訪問 - + 1 year of storage 一年的雲端行車記錄 - + Developer perks 開發者福利 @@ -659,22 +1209,22 @@ location set PrimeUserWidget - + ✓ SUBSCRIBED ✓ 已訂閱 - + comma prime comma 高級會員 - + CONNECT.COMMA.AI CONNECT.COMMA.AI - + COMMA POINTS COMMA 積分 @@ -692,14 +1242,12 @@ location set 離開 - dashcam - 行車記錄器 + 行車記錄器 - openpilot - openpilot + openpilot @@ -782,9 +1330,8 @@ location set SettingsWindow - × - × + × @@ -792,28 +1339,28 @@ location set 回上頁 - + Device 設備 - - + + Network 網路 - + Toggles 設定 - + Software 軟體 - + Navigation 導航 @@ -952,17 +1499,17 @@ location set SetupWidget - + Finish Setup 完成設置 - + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. 將您的設備與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 - + Pair device 配對設備 @@ -982,7 +1529,7 @@ location set - + ONLINE 已連線 @@ -992,84 +1539,81 @@ location set 錯誤 - - + + TEMP 溫度 - HIGH - 偏高 + 偏高 - GOOD - 正常 + 正常 - OK - 一般 + 一般 - + VEHICLE 車輛通訊 - + NO 未連線 - + PANDA 車輛通訊 - + GPS GPS - + SEARCH 車輛通訊 - + -- -- - + Wi-Fi - + ETH - + 2G - + 3G - + LTE - + 5G @@ -1077,89 +1621,89 @@ location set SoftwarePanel - + Git Branch Git 分支 - + Git Commit Git 提交 - + OS Version 系統版本 - + Version 版本 - + Last Update Check 上次檢查時間 - + The last time openpilot successfully checked for an update. The updater only runs while the car is off. 上次成功檢查更新的時間。更新系統只會在車子熄火時執行。 - + Check for Update 檢查更新 - + CHECKING 檢查中 - + Switch Branch 切換分支 - + ENTER 切換 - - + + The new branch will be pulled the next time the updater runs. 新的分支將會在下次檢查更新時切換過去。 - + Enter branch name 輸入分支名稱 - + UNINSTALL 卸載 - + Uninstall %1 卸載 %1 - + Are you sure you want to uninstall? 您確定您要卸載嗎？ - + failed to fetch update 下載更新失敗 - - + + CHECK 檢查 @@ -1247,22 +1791,22 @@ location set TogglesPanel - + Enable openpilot 啟用 openpilot - + Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. 使用 openpilot 的主動式巡航和車道保持功能，開啟後您需要持續集中注意力，設定變更在重新啟動車輛後生效。 - + Enable Lane Departure Warnings 啟用車道偏離警告 - + Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). 車速在時速 50 公里 (31 英里) 以上且未打方向燈的情況下，如果偵測到車輛駛出目前車道線時，發出車道偏離警告。 @@ -1277,62 +1821,62 @@ location set openpilot 將對右側駕駛進行監控 (但仍遵守靠左駕的交通慣例)。 - + Use Metric System 使用公制單位 - + Display speed in km/h instead of mph. 啟用後，速度單位顯示將從 mp/h 改為 km/h。 - + Record and Upload Driver Camera 記錄並上傳駕駛監控影像 - + Upload data from the driver facing camera and help improve the driver monitoring algorithm. 上傳駕駛監控的錄像來協助我們提升駕駛監控的準確率。 - + Disengage On Accelerator Pedal 油門取消控車 - + When enabled, pressing the accelerator pedal will disengage openpilot. 啟用後，踩踏油門將會取消 openpilot 控制。 - + Show ETA in 24h format 預計到達時間單位改用 24 小時制 - + Use 24h format instead of am/pm 使用 24 小時制。(預設值為 12 小時制) - + Show Map on Left Side of UI 將地圖顯示在畫面的左側 - + Show map on left side when in split screen view. 進入分割畫面後，地圖將會顯示在畫面的左側。 - + openpilot Longitudinal Control openpilot 縱向控制 - + openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB! openpilot 將會關閉雷達訊號並接管油門和剎車的控制。注意：這也會關閉自動緊急煞車 (AEB) 系統！ diff --git a/system/camerad/camerad b/system/camerad/camerad index e6ac7a7ba..98d1091d7 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/clocksd/clocksd b/system/clocksd/clocksd index 756c547cf..efc9a44f4 100755 Binary files a/system/clocksd/clocksd and b/system/clocksd/clocksd differ