mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-10 18:13:40 +08:00
dragonpilot 2022-08-30T07:26:22 for EON/C2
version: dragonpilot v0.8.17 beta for EON/C2 date: 2022-08-30T07:26:22 dp-dev(priv2) master commit: 0b0eacf73b22b4ff5341143a3beb2d2d681d989b
This commit is contained in:
Dragonpilot Team
@@ -1,5 +1,13 @@
|
||||
dragonpilot [latest] - EON/C2 Release
|
||||
========================
|
||||
* NEW: Synced to openpilot master 2022.08.29 commits.
|
||||
* Nuclear Grade driving model.
|
||||
* TWEAK: Used fused location service on mapd.
|
||||
* TWEAK: Used fused location service on gpxd.
|
||||
* NEW: Added back auto boot on C2.
|
||||
|
||||
dragonpilot 2022.08.29 - EON/C2 Release
|
||||
========================
|
||||
* NEW: Synced to openpilot master 2022.08.23 commits.
|
||||
* New driving model.
|
||||
* NEW: Added back honda eps mod toggle.
|
||||
|
||||
@@ -42,7 +42,7 @@ To use openpilot in a car, you need four things
|
||||
* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three).
|
||||
* This software. The setup procedure of the comma three allows the user to enter a URL for custom software.
|
||||
The URL, openpilot.comma.ai will install the release version of openpilot. To install openpilot master, you can use installer.comma.ai/commaai/master, and replacing commaai with another GitHub username can install a fork.
|
||||
* One of [the 150+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot.
|
||||
* One of [the 200+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run openpilot.
|
||||
* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car.
|
||||
|
||||
We have detailed instructions for [how to mount the device in a car](https://comma.ai/setup).
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
Version 0.8.17 (2022-XX-XX)
|
||||
========================
|
||||
* New driving model
|
||||
* Internal feature space accuracy increased tenfold during training, this makes the model dramatically more accurate.
|
||||
|
||||
Version 0.8.16 (2022-08-26)
|
||||
========================
|
||||
|
||||
+3
-3
@@ -470,7 +470,7 @@ struct CarParams {
|
||||
vEgoStarting @59 :Float32; # Speed at which the car goes into starting state
|
||||
directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake
|
||||
stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping
|
||||
stopAccel @60 :Float32; # Required acceleraton to keep vehicle stationary
|
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary
|
||||
steerControlType @34 :SteerControlType;
|
||||
radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN
|
||||
stoppingDecelRate @52 :Float32; # m/s^2/s while trying to stop
|
||||
@@ -585,8 +585,8 @@ struct CarParams {
|
||||
subaruLegacy @22; # pre-Global platform
|
||||
hyundaiLegacy @23;
|
||||
hyundaiCommunity @24;
|
||||
stellantis @25;
|
||||
faw @26;
|
||||
stellantisDEPRECATED @25; # Consolidated with Chrysler; may be recycled for the next new model
|
||||
hongqi @26;
|
||||
body @27;
|
||||
hyundaiCanfd @28;
|
||||
}
|
||||
|
||||
Binary file not shown.
+1
-1
@@ -1238,7 +1238,7 @@ struct UbloxGnss {
|
||||
carrierPhaseValid @1 :Bool;
|
||||
# half cycle valid
|
||||
halfCycleValid @2 :Bool;
|
||||
# half sycle subtracted from phase
|
||||
# half cycle subtracted from phase
|
||||
halfCycleSubtracted @3 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
Binary file not shown.
+15
-15
@@ -1720,7 +1720,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_8get(struct __pyx_obj_6co
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_10get_bool(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_12put(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key, PyObject *__pyx_v_dat); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_14put_bool(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key, bool __pyx_v_val); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_16remove(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_18get_param_path(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_20__reduce_cython__(CYTHON_UNUSED struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self); /* proto */
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_22__setstate_cython__(CYTHON_UNUSED struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, CYTHON_UNUSED PyObject *__pyx_v___pyx_state); /* proto */
|
||||
@@ -3114,7 +3114,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_14put_bool(struct __pyx_o
|
||||
* with nogil:
|
||||
* self.p.putBool(k, val) # <<<<<<<<<<<<<<
|
||||
*
|
||||
* def delete(self, key):
|
||||
* def remove(self, key):
|
||||
*/
|
||||
(void)(__pyx_v_self->p->putBool(__pyx_v_k, __pyx_v_val));
|
||||
}
|
||||
@@ -3164,25 +3164,25 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_14put_bool(struct __pyx_o
|
||||
/* "common/params_pyx.pyx":93
|
||||
* self.p.putBool(k, val)
|
||||
*
|
||||
* def delete(self, key): # <<<<<<<<<<<<<<
|
||||
* def remove(self, key): # <<<<<<<<<<<<<<
|
||||
* cdef string k = self.check_key(key)
|
||||
* with nogil:
|
||||
*/
|
||||
|
||||
/* Python wrapper */
|
||||
static PyObject *__pyx_pw_6common_10params_pyx_6Params_17delete(PyObject *__pyx_v_self, PyObject *__pyx_v_key); /*proto*/
|
||||
static PyObject *__pyx_pw_6common_10params_pyx_6Params_17delete(PyObject *__pyx_v_self, PyObject *__pyx_v_key) {
|
||||
static PyObject *__pyx_pw_6common_10params_pyx_6Params_17remove(PyObject *__pyx_v_self, PyObject *__pyx_v_key); /*proto*/
|
||||
static PyObject *__pyx_pw_6common_10params_pyx_6Params_17remove(PyObject *__pyx_v_self, PyObject *__pyx_v_key) {
|
||||
PyObject *__pyx_r = 0;
|
||||
__Pyx_RefNannyDeclarations
|
||||
__Pyx_RefNannySetupContext("delete (wrapper)", 0);
|
||||
__pyx_r = __pyx_pf_6common_10params_pyx_6Params_16delete(((struct __pyx_obj_6common_10params_pyx_Params *)__pyx_v_self), ((PyObject *)__pyx_v_key));
|
||||
__Pyx_RefNannySetupContext("remove (wrapper)", 0);
|
||||
__pyx_r = __pyx_pf_6common_10params_pyx_6Params_16remove(((struct __pyx_obj_6common_10params_pyx_Params *)__pyx_v_self), ((PyObject *)__pyx_v_key));
|
||||
|
||||
/* function exit code */
|
||||
__Pyx_RefNannyFinishContext();
|
||||
return __pyx_r;
|
||||
}
|
||||
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key) {
|
||||
static PyObject *__pyx_pf_6common_10params_pyx_6Params_16remove(struct __pyx_obj_6common_10params_pyx_Params *__pyx_v_self, PyObject *__pyx_v_key) {
|
||||
std::string __pyx_v_k;
|
||||
PyObject *__pyx_r = NULL;
|
||||
__Pyx_RefNannyDeclarations
|
||||
@@ -3193,11 +3193,11 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj
|
||||
int __pyx_lineno = 0;
|
||||
const char *__pyx_filename = NULL;
|
||||
int __pyx_clineno = 0;
|
||||
__Pyx_RefNannySetupContext("delete", 0);
|
||||
__Pyx_RefNannySetupContext("remove", 0);
|
||||
|
||||
/* "common/params_pyx.pyx":94
|
||||
*
|
||||
* def delete(self, key):
|
||||
* def remove(self, key):
|
||||
* cdef string k = self.check_key(key) # <<<<<<<<<<<<<<
|
||||
* with nogil:
|
||||
* self.p.remove(k)
|
||||
@@ -3224,7 +3224,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj
|
||||
__pyx_v_k = __pyx_t_4;
|
||||
|
||||
/* "common/params_pyx.pyx":95
|
||||
* def delete(self, key):
|
||||
* def remove(self, key):
|
||||
* cdef string k = self.check_key(key)
|
||||
* with nogil: # <<<<<<<<<<<<<<
|
||||
* self.p.remove(k)
|
||||
@@ -3249,7 +3249,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj
|
||||
}
|
||||
|
||||
/* "common/params_pyx.pyx":95
|
||||
* def delete(self, key):
|
||||
* def remove(self, key):
|
||||
* cdef string k = self.check_key(key)
|
||||
* with nogil: # <<<<<<<<<<<<<<
|
||||
* self.p.remove(k)
|
||||
@@ -3270,7 +3270,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj
|
||||
/* "common/params_pyx.pyx":93
|
||||
* self.p.putBool(k, val)
|
||||
*
|
||||
* def delete(self, key): # <<<<<<<<<<<<<<
|
||||
* def remove(self, key): # <<<<<<<<<<<<<<
|
||||
* cdef string k = self.check_key(key)
|
||||
* with nogil:
|
||||
*/
|
||||
@@ -3282,7 +3282,7 @@ static PyObject *__pyx_pf_6common_10params_pyx_6Params_16delete(struct __pyx_obj
|
||||
__Pyx_XDECREF(__pyx_t_1);
|
||||
__Pyx_XDECREF(__pyx_t_2);
|
||||
__Pyx_XDECREF(__pyx_t_3);
|
||||
__Pyx_AddTraceback("common.params_pyx.Params.delete", __pyx_clineno, __pyx_lineno, __pyx_filename);
|
||||
__Pyx_AddTraceback("common.params_pyx.Params.remove", __pyx_clineno, __pyx_lineno, __pyx_filename);
|
||||
__pyx_r = NULL;
|
||||
__pyx_L0:;
|
||||
__Pyx_XGIVEREF(__pyx_r);
|
||||
@@ -6060,7 +6060,7 @@ static PyMethodDef __pyx_methods_6common_10params_pyx_Params[] = {
|
||||
{"get_bool", (PyCFunction)__pyx_pw_6common_10params_pyx_6Params_11get_bool, METH_O, 0},
|
||||
{"put", (PyCFunction)(void*)(PyCFunctionWithKeywords)__pyx_pw_6common_10params_pyx_6Params_13put, METH_VARARGS|METH_KEYWORDS, __pyx_doc_6common_10params_pyx_6Params_12put},
|
||||
{"put_bool", (PyCFunction)(void*)(PyCFunctionWithKeywords)__pyx_pw_6common_10params_pyx_6Params_15put_bool, METH_VARARGS|METH_KEYWORDS, 0},
|
||||
{"delete", (PyCFunction)__pyx_pw_6common_10params_pyx_6Params_17delete, METH_O, 0},
|
||||
{"remove", (PyCFunction)__pyx_pw_6common_10params_pyx_6Params_17remove, METH_O, 0},
|
||||
{"get_param_path", (PyCFunction)(void*)(PyCFunctionWithKeywords)__pyx_pw_6common_10params_pyx_6Params_19get_param_path, METH_VARARGS|METH_KEYWORDS, 0},
|
||||
{"__reduce_cython__", (PyCFunction)__pyx_pw_6common_10params_pyx_6Params_21__reduce_cython__, METH_NOARGS, 0},
|
||||
{"__setstate_cython__", (PyCFunction)__pyx_pw_6common_10params_pyx_6Params_23__setstate_cython__, METH_O, 0},
|
||||
|
||||
@@ -90,7 +90,7 @@ cdef class Params:
|
||||
with nogil:
|
||||
self.p.putBool(k, val)
|
||||
|
||||
def delete(self, key):
|
||||
def remove(self, key):
|
||||
cdef string k = self.check_key(key)
|
||||
with nogil:
|
||||
self.p.remove(k)
|
||||
|
||||
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "2022.08.26"
|
||||
#define COMMA_VERSION "2022.08.30"
|
||||
|
||||
Binary file not shown.
+31
-1
@@ -296,7 +296,9 @@ BO_ 1408 Motor_Flexia: 8 XXX
|
||||
SG_ Verschleissindex : 16|8@1+ (1,0) [0|254] "" XXX
|
||||
SG_ Russindex : 8|8@1+ (1,0) [0|254] "" XXX
|
||||
SG_ Verbrennungsart : 7|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Frei_Motor_Flexia_1 : 4|3@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Frei_Motor_Flexia_1 : 6|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Warm_Up_Cycle : 5|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Driving_Cycle : 4|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Zaehler_Motor_Flexia : 1|3@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Multiplex_Schalter_Motor_Flexia M : 0|1@1+ (1,0) [0|0] "" XXX
|
||||
|
||||
@@ -1457,19 +1459,28 @@ CM_ SG_ 640 Fahrerwunschmoment "Driver Requested Torque";
|
||||
CM_ SG_ 640 mechanisches_Motor_Verlustmomen "Mechanical Torque Loss";
|
||||
CM_ SG_ 640 Fahrpedalwert_oder_Drosselklapp "Accelerator Pedal or Throttle Position";
|
||||
CM_ SG_ 640 Motordrehzahl "Engine Speed";
|
||||
CM_ SG_ 640 Momentenangaben_ungenau "Approximate Torque Values";
|
||||
|
||||
CM_ SG_ 644 MOB_CHECKSUM "Checksum MOB";
|
||||
CM_ SG_ 644 MOB_COUNTER "Counter MOB";
|
||||
|
||||
CM_ SG_ 648 Minimales_Motormoment_bei_Zuend "Minimum Torque Adjustment";
|
||||
CM_ SG_ 648 Begrenzungsmoment "Maximum Inner Torque";
|
||||
CM_ SG_ 648 Bremstestschalter "Brake Test Switch";
|
||||
CM_ SG_ 648 Soll_Geschwindigkeit_bei_GRA_Be "Desired Vehicle Speed";
|
||||
CM_ SG_ 648 Bremslichtschalter "Brake Light Switch";
|
||||
CM_ SG_ 648 Leerlaufsolldrehzahl__Motor_2_ "Target Idle Speed";
|
||||
CM_ SG_ 648 Fahrzeuggeschwindigkeit "Vehicle Speed";
|
||||
CM_ SG_ 648 Kuehlmitteltemperatur__Motor_2_ "Coolant Temperature";
|
||||
|
||||
CM_ SG_ 896 Drosselklappenpoti "Throttle Position";
|
||||
CM_ SG_ 896 Motor_Wunschdrehzahl "Desired engine speed";
|
||||
CM_ SG_ 896 Motordrehzahlbeeinflussung "Shift Target Influence";
|
||||
CM_ SG_ 896 Fahrpedal_Rohsignal "Accelerator Pedal Position";
|
||||
CM_ SG_ 896 Ansauglufttemperatur "Intake Air Temperature";
|
||||
CM_ SG_ 896 Kein_E_Gas "ETB flag";
|
||||
CM_ SG_ 896 Kein_Start_Stop "Start/stop flag";
|
||||
CM_ SG_ 896 Rad_Wunschmoment "Desired wheel torque";
|
||||
|
||||
CM_ SG_ 912 GK1_Fa_Tuerkont "Status of the driver's door rotary latch";
|
||||
CM_ SG_ 912 BSK_HL_geoeffnet "Status of the rear left door rotary latch";
|
||||
@@ -1480,6 +1491,15 @@ CM_ SG_ 912 BSK_HD_Hauptraste "Status of trunk lid main detent";
|
||||
CM_ SG_ 1088 Zaehler_Getriebe_1 "Counter Getriebe_1";
|
||||
CM_ SG_ 1088 Waehlhebelposition__Getriebe_1_ "Gear Selector Position";
|
||||
|
||||
CM_ SG_ 1056 Fehlerstatus_Aussentemp__4_1 "ambient temp error";
|
||||
CM_ SG_ 1056 Fehlerstatus_Oeltemperatur_4_1 "oil temp error";
|
||||
CM_ SG_ 1056 Fehlerst__Kuehlmitteltemp__4_1 "water temp error";
|
||||
CM_ SG_ 1056 Aussentemperatur_gefiltert "outside temp, filtered";
|
||||
CM_ SG_ 1056 Oeltemperatur_4_1 "kombi oil temperature";
|
||||
CM_ SG_ 1056 Kuehlmitteltemp__4_1__Kombi_2_ "kombi coolant temperature";
|
||||
|
||||
CM_ SG_ 1096 Zaehler_Waehlhebel_1 "Counter Waehlhebel_1";
|
||||
|
||||
CM_ SG_ 1152 Checksumme_Motor_5 "Checksum Motor_5";
|
||||
|
||||
CM_ SG_ 1160 Zaehler_Motor_6 "Counter Motor_6";
|
||||
@@ -1492,6 +1512,16 @@ CM_ SG_ 1344 Zahler_Getriebe_2 "Counter Getriebe_2";
|
||||
|
||||
CM_ SG_ 1386 ACA_V_Wunsch "255=unset";
|
||||
|
||||
CM_ SG_ 1408 Zaehler_Motor_Flexia "Counter Motor_Flexia";
|
||||
CM_ SG_ 1408 Verbrennungsart "Type of combustion";
|
||||
CM_ SG_ 1408 Max_Drehmoment "Maximum torque";
|
||||
CM_ SG_ 1408 Drehzahl_MaxNorm "RPM of maximum torque";
|
||||
CM_ SG_ 1408 Hubraum "Displacement";
|
||||
CM_ SG_ 1408 Anzahl_Zylinder "Number of cylinders";
|
||||
CM_ SG_ 1408 Anzahl_Ventile "Number of valves";
|
||||
CM_ SG_ 1408 Ansaugsystem "Induction System";
|
||||
CM_ SG_ 1408 Motorleistung "Maximum engine power";
|
||||
|
||||
CM_ SG_ 1416 Ladedruck "Boost Pressure";
|
||||
|
||||
CM_ SG_ 1470 LDW_Direction "0=right,1=left";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,12 +1,13 @@
|
||||
# python library to interface with panda
|
||||
import datetime
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import usb1
|
||||
import struct
|
||||
import hashlib
|
||||
import usb1
|
||||
import os
|
||||
import time
|
||||
import datetime
|
||||
import traceback
|
||||
import sys
|
||||
import warnings
|
||||
from functools import wraps
|
||||
from typing import Optional
|
||||
from itertools import accumulate
|
||||
@@ -410,7 +411,11 @@ class Panda:
|
||||
for device in context.getDeviceList(skip_on_error=True):
|
||||
if device.getVendorID() == 0xbbaa and device.getProductID() in (0xddcc, 0xddee):
|
||||
try:
|
||||
ret.append(device.getSerialNumber())
|
||||
serial = device.getSerialNumber()
|
||||
if len(serial) == 24:
|
||||
ret.append(serial)
|
||||
else:
|
||||
warnings.warn(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
|
||||
except Exception:
|
||||
continue
|
||||
except Exception:
|
||||
|
||||
Binary file not shown.
@@ -740,14 +740,14 @@ def main():
|
||||
break
|
||||
except (ConnectionError, TimeoutError, WebSocketException):
|
||||
conn_retries += 1
|
||||
params.delete("LastAthenaPingTime")
|
||||
params.remove("LastAthenaPingTime")
|
||||
except socket.timeout:
|
||||
params.delete("LastAthenaPingTime")
|
||||
params.remove("LastAthenaPingTime")
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.main.exception")
|
||||
|
||||
conn_retries += 1
|
||||
params.delete("LastAthenaPingTime")
|
||||
params.remove("LastAthenaPingTime")
|
||||
|
||||
time.sleep(backoff(conn_retries))
|
||||
|
||||
|
||||
@@ -27,7 +27,7 @@ def main():
|
||||
except Exception:
|
||||
cloudlog.exception("manage_athenad.exception")
|
||||
finally:
|
||||
params.delete(ATHENA_MGR_PID_PARAM)
|
||||
params.remove(ATHENA_MGR_PID_PARAM)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -83,7 +83,7 @@ def main() -> NoReturn:
|
||||
|
||||
while True:
|
||||
try:
|
||||
params.delete("PandaSignatures")
|
||||
params.remove("PandaSignatures")
|
||||
|
||||
# Flash all Pandas in DFU mode
|
||||
for p in PandaDFU.list():
|
||||
|
||||
@@ -170,6 +170,7 @@ FW_VERSIONS = {
|
||||
b'68448163AJ',
|
||||
b'68500630AD',
|
||||
b'68539650AD',
|
||||
b'68378758AM ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68360078AL',
|
||||
|
||||
@@ -97,6 +97,11 @@ CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(0xf132)
|
||||
|
||||
CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
|
||||
CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
|
||||
|
||||
CHRYSLER_RX_OFFSET = -0x280
|
||||
|
||||
FORD_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
@@ -208,12 +213,20 @@ REQUESTS: List[Request] = [
|
||||
"chrysler",
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.esp, Ecu.eps, Ecu.srs, Ecu.gateway, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.combinationMeter],
|
||||
rx_offset=CHRYSLER_RX_OFFSET,
|
||||
),
|
||||
Request(
|
||||
"chrysler",
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.transmission],
|
||||
),
|
||||
Request(
|
||||
"chrysler",
|
||||
[CHRYSLER_SOFTWARE_VERSION_REQUEST],
|
||||
[CHRYSLER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.transmission],
|
||||
),
|
||||
# Ford
|
||||
Request(
|
||||
|
||||
@@ -138,28 +138,29 @@ class CarController:
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
|
||||
self.gas = interceptor_gas_cmd
|
||||
|
||||
# ui mesg is at 1Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
if self.CP.carFingerprint != CAR.PRIUS_V:
|
||||
# ui mesg is at 1Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
send_ui = False
|
||||
if ((fcw_alert or steer_alert) and not self.alert_active) or \
|
||||
(not (fcw_alert or steer_alert) and self.alert_active):
|
||||
send_ui = True
|
||||
self.alert_active = not self.alert_active
|
||||
elif pcm_cancel_cmd:
|
||||
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
|
||||
send_ui = True
|
||||
send_ui = False
|
||||
if ((fcw_alert or steer_alert) and not self.alert_active) or \
|
||||
(not (fcw_alert or steer_alert) and self.alert_active):
|
||||
send_ui = True
|
||||
self.alert_active = not self.alert_active
|
||||
elif pcm_cancel_cmd:
|
||||
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
|
||||
send_ui = True
|
||||
|
||||
if (self.frame % 100 == 0 or send_ui) and (self.CP.carFingerprint != CAR.PRIUS_V):
|
||||
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
|
||||
hud_control.rightLaneDepart, CC.enabled))
|
||||
if self.frame % 100 == 0 or send_ui:
|
||||
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
|
||||
hud_control.rightLaneDepart, CC.enabled))
|
||||
|
||||
if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw_alert))
|
||||
if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw_alert))
|
||||
|
||||
# *** static msgs ***
|
||||
for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
|
||||
|
||||
@@ -1268,6 +1268,7 @@ FW_VERSIONS = {
|
||||
b'\x02896634A18100\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
b'\x02896634A43000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
|
||||
b'\x02896634A47000\x00\x00\x00\x00897CF4201001\x00\x00\x00\x00',
|
||||
b'\x028966342Z8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'\x01F15260R210\x00\x00\x00\x00\x00\x00',
|
||||
|
||||
@@ -369,6 +369,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x870D9300012 \xf1\x894937',
|
||||
b'\xf1\x870D9300012 \xf1\x895045',
|
||||
b'\xf1\x870D9300014M \xf1\x895004',
|
||||
b'\xf1\x870D9300014Q \xf1\x895006',
|
||||
b'\xf1\x870D9300020S \xf1\x895201',
|
||||
b'\xf1\x870D9300040A \xf1\x893613',
|
||||
b'\xf1\x870D9300040S \xf1\x894311',
|
||||
|
||||
@@ -177,14 +177,14 @@ class Controls:
|
||||
self.state = State.disabled
|
||||
self.enabled = False
|
||||
self.active = False
|
||||
self.can_rcv_error = False
|
||||
self.can_rcv_timeout = False
|
||||
self.soft_disable_timer = 0
|
||||
self.v_cruise_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_cluster_kph = V_CRUISE_INITIAL
|
||||
self.v_cruise_kph_last = 0
|
||||
self.mismatch_counter = 0
|
||||
self.cruise_mismatch_counter = 0
|
||||
self.can_rcv_error_counter = 0
|
||||
self.can_rcv_timeout_counter = 0
|
||||
self.last_blinker_frame = 0
|
||||
self.distance_traveled = 0
|
||||
self.last_functional_fan_frame = 0
|
||||
@@ -356,19 +356,19 @@ class Controls:
|
||||
# generic catch-all. ideally, a more specific event should be added above instead
|
||||
has_disable_events = self.events.any(ET.NO_ENTRY) and (self.events.any(ET.SOFT_DISABLE) or self.events.any(ET.IMMEDIATE_DISABLE))
|
||||
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
|
||||
if (not self.sm.all_checks() or self.can_rcv_error) and no_system_errors:
|
||||
if (not self.sm.all_checks() or self.can_rcv_timeout) and no_system_errors:
|
||||
if not self.sm.all_alive():
|
||||
self.events.add(EventName.commIssue)
|
||||
elif not self.sm.all_freq_ok():
|
||||
self.events.add(EventName.commIssueAvgFreq)
|
||||
else: # invalid or can_rcv_error.
|
||||
else: # invalid or can_rcv_timeout.
|
||||
self.events.add(EventName.commIssue)
|
||||
|
||||
logs = {
|
||||
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
|
||||
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
|
||||
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
|
||||
'can_error': self.can_rcv_error,
|
||||
'can_rcv_timeout': self.can_rcv_timeout,
|
||||
}
|
||||
if logs != self.logged_comm_issue:
|
||||
cloudlog.event("commIssue", error=True, **logs)
|
||||
@@ -459,10 +459,10 @@ class Controls:
|
||||
|
||||
# Check for CAN timeout
|
||||
if not can_strs:
|
||||
self.can_rcv_error_counter += 1
|
||||
self.can_rcv_error = True
|
||||
self.can_rcv_timeout_counter += 1
|
||||
self.can_rcv_timeout = True
|
||||
else:
|
||||
self.can_rcv_error = False
|
||||
self.can_rcv_timeout = False
|
||||
|
||||
# When the panda and controlsd do not agree on controls_allowed
|
||||
# we want to disengage openpilot. However the status from the panda goes through
|
||||
@@ -814,7 +814,7 @@ class Controls:
|
||||
controlsState.cumLagMs = -self.rk.remaining * 1000.
|
||||
controlsState.startMonoTime = int(start_time * 1e9)
|
||||
controlsState.forceDecel = bool(force_decel)
|
||||
controlsState.canErrorCounter = self.can_rcv_error_counter
|
||||
controlsState.canErrorCounter = self.can_rcv_timeout_counter
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.joystick_mode:
|
||||
|
||||
@@ -22,7 +22,7 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] =
|
||||
a['text'] += extra_text
|
||||
Params().put(alert, json.dumps(a))
|
||||
else:
|
||||
Params().delete(alert)
|
||||
Params().remove(alert)
|
||||
|
||||
|
||||
@dataclass
|
||||
|
||||
@@ -28,17 +28,30 @@ import datetime
|
||||
import signal
|
||||
import threading
|
||||
from common.realtime import Ratekeeper
|
||||
import math
|
||||
from cereal import log
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
# customisable values
|
||||
GPX_LOG_PATH = '/data/media/0/gpx_logs/'
|
||||
LOG_HERTZ = 10 # 10 hz = 0.1 sec, higher for higher accuracy, 10hz seems fine
|
||||
LOG_LENGTH = 10 # mins, higher means it keeps more data in the memory, will take more time to write into a file too.
|
||||
LOST_SIGNAL_COUNT_LENGTH = 30 # secs, output log file if we lost signal for this long
|
||||
LOST_SIGNAL_COUNT_LENGTH = 10 # secs, output log file if we lost signal for this long
|
||||
|
||||
# do not change
|
||||
LOST_SIGNAL_COUNT_MAX = LOST_SIGNAL_COUNT_LENGTH * LOG_HERTZ # secs,
|
||||
LOGS_PER_FILE = LOG_LENGTH * 60 * LOG_HERTZ # e.g. 10 * 60 * 10 = 6000 points per file
|
||||
|
||||
_DEBUG = False
|
||||
_CLOUDLOG_DEBUG = True
|
||||
|
||||
|
||||
def _debug(msg, log_to_cloud=True):
|
||||
if _CLOUDLOG_DEBUG and log_to_cloud:
|
||||
cloudlog.debug(msg)
|
||||
if _DEBUG:
|
||||
print(msg)
|
||||
|
||||
class WaitTimeHelper:
|
||||
ready_event = threading.Event()
|
||||
shutdown = False
|
||||
@@ -60,27 +73,33 @@ class GpxD():
|
||||
self.wait_helper = WaitTimeHelper()
|
||||
self.started_time = datetime.datetime.utcnow().isoformat()
|
||||
self.v_ego_prev = 0.
|
||||
self.pause = False
|
||||
self.pause = True
|
||||
|
||||
def log(self, sm):
|
||||
location = sm['liveLocationKalman']
|
||||
gps = sm['gpsLocationExternal']
|
||||
v_ego = sm['carState'].vEgo
|
||||
|
||||
if abs(v_ego) > 0.01:
|
||||
self.pause = False
|
||||
|
||||
# do not log when no fix or accuracy is too low, add lost_signal_count
|
||||
if gps.flags % 2 == 0 or gps.accuracy > 5.:
|
||||
location_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
_debug("gpxd: location_valid - %s" % location_valid)
|
||||
if not location_valid or self.pause:
|
||||
if self.log_count > 0:
|
||||
self.lost_signal_count += 1
|
||||
elif self.pause:
|
||||
pass
|
||||
else:
|
||||
self.logs.append([datetime.datetime.utcfromtimestamp(gps.unixTimestampMillis*0.001).isoformat(), str(gps.latitude), str(gps.longitude), str(gps.altitude)])
|
||||
lat = location.positionGeodetic.value[0]
|
||||
lon = location.positionGeodetic.value[1]
|
||||
alt = gps.altitude
|
||||
|
||||
_debug("gpxd: logged - %s %s %s %s" % (datetime.datetime.utcfromtimestamp(location.unixTimestampMillis*0.001).isoformat(), str(lat), str(lon), str(alt)))
|
||||
self.logs.append([datetime.datetime.utcfromtimestamp(location.unixTimestampMillis*0.001).isoformat(), str(lat), str(lon), str(alt)])
|
||||
self.log_count += 1
|
||||
self.lost_signal_count = 0
|
||||
|
||||
if abs(v_ego) < 0.01 and abs(self.v_ego_prev) > 0.:
|
||||
if not self.pause and abs(v_ego) < 0.01:
|
||||
_debug("gpxd: paused")
|
||||
self.pause = True
|
||||
|
||||
self.v_ego_prev = v_ego
|
||||
@@ -90,6 +109,7 @@ class GpxD():
|
||||
return
|
||||
|
||||
if force or (self.log_count >= LOGS_PER_FILE or self.lost_signal_count >= LOST_SIGNAL_COUNT_MAX):
|
||||
_debug("gpxd: save to log")
|
||||
self._write_gpx()
|
||||
self.lost_signal_count = 0
|
||||
self.log_count = 0
|
||||
@@ -129,7 +149,7 @@ class GpxD():
|
||||
|
||||
def gpxd_thread(sm=None, pm=None):
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'carState'])
|
||||
sm = messaging.SubMaster(['liveLocationKalman', 'gpsLocationExternal', 'carState'])
|
||||
|
||||
wait_helper = WaitTimeHelper()
|
||||
gpxd = GpxD()
|
||||
|
||||
Binary file not shown.
@@ -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_729362937437787457) {
|
||||
out_729362937437787457[0] = delta_x[0] + nom_x[0];
|
||||
out_729362937437787457[1] = delta_x[1] + nom_x[1];
|
||||
out_729362937437787457[2] = delta_x[2] + nom_x[2];
|
||||
out_729362937437787457[3] = delta_x[3] + nom_x[3];
|
||||
out_729362937437787457[4] = delta_x[4] + nom_x[4];
|
||||
out_729362937437787457[5] = delta_x[5] + nom_x[5];
|
||||
out_729362937437787457[6] = delta_x[6] + nom_x[6];
|
||||
out_729362937437787457[7] = delta_x[7] + nom_x[7];
|
||||
out_729362937437787457[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_6258559787192254109) {
|
||||
out_6258559787192254109[0] = delta_x[0] + nom_x[0];
|
||||
out_6258559787192254109[1] = delta_x[1] + nom_x[1];
|
||||
out_6258559787192254109[2] = delta_x[2] + nom_x[2];
|
||||
out_6258559787192254109[3] = delta_x[3] + nom_x[3];
|
||||
out_6258559787192254109[4] = delta_x[4] + nom_x[4];
|
||||
out_6258559787192254109[5] = delta_x[5] + nom_x[5];
|
||||
out_6258559787192254109[6] = delta_x[6] + nom_x[6];
|
||||
out_6258559787192254109[7] = delta_x[7] + nom_x[7];
|
||||
out_6258559787192254109[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_4929143687762803407) {
|
||||
out_4929143687762803407[0] = -nom_x[0] + true_x[0];
|
||||
out_4929143687762803407[1] = -nom_x[1] + true_x[1];
|
||||
out_4929143687762803407[2] = -nom_x[2] + true_x[2];
|
||||
out_4929143687762803407[3] = -nom_x[3] + true_x[3];
|
||||
out_4929143687762803407[4] = -nom_x[4] + true_x[4];
|
||||
out_4929143687762803407[5] = -nom_x[5] + true_x[5];
|
||||
out_4929143687762803407[6] = -nom_x[6] + true_x[6];
|
||||
out_4929143687762803407[7] = -nom_x[7] + true_x[7];
|
||||
out_4929143687762803407[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8483442434741396819) {
|
||||
out_8483442434741396819[0] = -nom_x[0] + true_x[0];
|
||||
out_8483442434741396819[1] = -nom_x[1] + true_x[1];
|
||||
out_8483442434741396819[2] = -nom_x[2] + true_x[2];
|
||||
out_8483442434741396819[3] = -nom_x[3] + true_x[3];
|
||||
out_8483442434741396819[4] = -nom_x[4] + true_x[4];
|
||||
out_8483442434741396819[5] = -nom_x[5] + true_x[5];
|
||||
out_8483442434741396819[6] = -nom_x[6] + true_x[6];
|
||||
out_8483442434741396819[7] = -nom_x[7] + true_x[7];
|
||||
out_8483442434741396819[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_8315888887196402654) {
|
||||
out_8315888887196402654[0] = 1.0;
|
||||
out_8315888887196402654[1] = 0;
|
||||
out_8315888887196402654[2] = 0;
|
||||
out_8315888887196402654[3] = 0;
|
||||
out_8315888887196402654[4] = 0;
|
||||
out_8315888887196402654[5] = 0;
|
||||
out_8315888887196402654[6] = 0;
|
||||
out_8315888887196402654[7] = 0;
|
||||
out_8315888887196402654[8] = 0;
|
||||
out_8315888887196402654[9] = 0;
|
||||
out_8315888887196402654[10] = 1.0;
|
||||
out_8315888887196402654[11] = 0;
|
||||
out_8315888887196402654[12] = 0;
|
||||
out_8315888887196402654[13] = 0;
|
||||
out_8315888887196402654[14] = 0;
|
||||
out_8315888887196402654[15] = 0;
|
||||
out_8315888887196402654[16] = 0;
|
||||
out_8315888887196402654[17] = 0;
|
||||
out_8315888887196402654[18] = 0;
|
||||
out_8315888887196402654[19] = 0;
|
||||
out_8315888887196402654[20] = 1.0;
|
||||
out_8315888887196402654[21] = 0;
|
||||
out_8315888887196402654[22] = 0;
|
||||
out_8315888887196402654[23] = 0;
|
||||
out_8315888887196402654[24] = 0;
|
||||
out_8315888887196402654[25] = 0;
|
||||
out_8315888887196402654[26] = 0;
|
||||
out_8315888887196402654[27] = 0;
|
||||
out_8315888887196402654[28] = 0;
|
||||
out_8315888887196402654[29] = 0;
|
||||
out_8315888887196402654[30] = 1.0;
|
||||
out_8315888887196402654[31] = 0;
|
||||
out_8315888887196402654[32] = 0;
|
||||
out_8315888887196402654[33] = 0;
|
||||
out_8315888887196402654[34] = 0;
|
||||
out_8315888887196402654[35] = 0;
|
||||
out_8315888887196402654[36] = 0;
|
||||
out_8315888887196402654[37] = 0;
|
||||
out_8315888887196402654[38] = 0;
|
||||
out_8315888887196402654[39] = 0;
|
||||
out_8315888887196402654[40] = 1.0;
|
||||
out_8315888887196402654[41] = 0;
|
||||
out_8315888887196402654[42] = 0;
|
||||
out_8315888887196402654[43] = 0;
|
||||
out_8315888887196402654[44] = 0;
|
||||
out_8315888887196402654[45] = 0;
|
||||
out_8315888887196402654[46] = 0;
|
||||
out_8315888887196402654[47] = 0;
|
||||
out_8315888887196402654[48] = 0;
|
||||
out_8315888887196402654[49] = 0;
|
||||
out_8315888887196402654[50] = 1.0;
|
||||
out_8315888887196402654[51] = 0;
|
||||
out_8315888887196402654[52] = 0;
|
||||
out_8315888887196402654[53] = 0;
|
||||
out_8315888887196402654[54] = 0;
|
||||
out_8315888887196402654[55] = 0;
|
||||
out_8315888887196402654[56] = 0;
|
||||
out_8315888887196402654[57] = 0;
|
||||
out_8315888887196402654[58] = 0;
|
||||
out_8315888887196402654[59] = 0;
|
||||
out_8315888887196402654[60] = 1.0;
|
||||
out_8315888887196402654[61] = 0;
|
||||
out_8315888887196402654[62] = 0;
|
||||
out_8315888887196402654[63] = 0;
|
||||
out_8315888887196402654[64] = 0;
|
||||
out_8315888887196402654[65] = 0;
|
||||
out_8315888887196402654[66] = 0;
|
||||
out_8315888887196402654[67] = 0;
|
||||
out_8315888887196402654[68] = 0;
|
||||
out_8315888887196402654[69] = 0;
|
||||
out_8315888887196402654[70] = 1.0;
|
||||
out_8315888887196402654[71] = 0;
|
||||
out_8315888887196402654[72] = 0;
|
||||
out_8315888887196402654[73] = 0;
|
||||
out_8315888887196402654[74] = 0;
|
||||
out_8315888887196402654[75] = 0;
|
||||
out_8315888887196402654[76] = 0;
|
||||
out_8315888887196402654[77] = 0;
|
||||
out_8315888887196402654[78] = 0;
|
||||
out_8315888887196402654[79] = 0;
|
||||
out_8315888887196402654[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_317258154932342734) {
|
||||
out_317258154932342734[0] = 1.0;
|
||||
out_317258154932342734[1] = 0;
|
||||
out_317258154932342734[2] = 0;
|
||||
out_317258154932342734[3] = 0;
|
||||
out_317258154932342734[4] = 0;
|
||||
out_317258154932342734[5] = 0;
|
||||
out_317258154932342734[6] = 0;
|
||||
out_317258154932342734[7] = 0;
|
||||
out_317258154932342734[8] = 0;
|
||||
out_317258154932342734[9] = 0;
|
||||
out_317258154932342734[10] = 1.0;
|
||||
out_317258154932342734[11] = 0;
|
||||
out_317258154932342734[12] = 0;
|
||||
out_317258154932342734[13] = 0;
|
||||
out_317258154932342734[14] = 0;
|
||||
out_317258154932342734[15] = 0;
|
||||
out_317258154932342734[16] = 0;
|
||||
out_317258154932342734[17] = 0;
|
||||
out_317258154932342734[18] = 0;
|
||||
out_317258154932342734[19] = 0;
|
||||
out_317258154932342734[20] = 1.0;
|
||||
out_317258154932342734[21] = 0;
|
||||
out_317258154932342734[22] = 0;
|
||||
out_317258154932342734[23] = 0;
|
||||
out_317258154932342734[24] = 0;
|
||||
out_317258154932342734[25] = 0;
|
||||
out_317258154932342734[26] = 0;
|
||||
out_317258154932342734[27] = 0;
|
||||
out_317258154932342734[28] = 0;
|
||||
out_317258154932342734[29] = 0;
|
||||
out_317258154932342734[30] = 1.0;
|
||||
out_317258154932342734[31] = 0;
|
||||
out_317258154932342734[32] = 0;
|
||||
out_317258154932342734[33] = 0;
|
||||
out_317258154932342734[34] = 0;
|
||||
out_317258154932342734[35] = 0;
|
||||
out_317258154932342734[36] = 0;
|
||||
out_317258154932342734[37] = 0;
|
||||
out_317258154932342734[38] = 0;
|
||||
out_317258154932342734[39] = 0;
|
||||
out_317258154932342734[40] = 1.0;
|
||||
out_317258154932342734[41] = 0;
|
||||
out_317258154932342734[42] = 0;
|
||||
out_317258154932342734[43] = 0;
|
||||
out_317258154932342734[44] = 0;
|
||||
out_317258154932342734[45] = 0;
|
||||
out_317258154932342734[46] = 0;
|
||||
out_317258154932342734[47] = 0;
|
||||
out_317258154932342734[48] = 0;
|
||||
out_317258154932342734[49] = 0;
|
||||
out_317258154932342734[50] = 1.0;
|
||||
out_317258154932342734[51] = 0;
|
||||
out_317258154932342734[52] = 0;
|
||||
out_317258154932342734[53] = 0;
|
||||
out_317258154932342734[54] = 0;
|
||||
out_317258154932342734[55] = 0;
|
||||
out_317258154932342734[56] = 0;
|
||||
out_317258154932342734[57] = 0;
|
||||
out_317258154932342734[58] = 0;
|
||||
out_317258154932342734[59] = 0;
|
||||
out_317258154932342734[60] = 1.0;
|
||||
out_317258154932342734[61] = 0;
|
||||
out_317258154932342734[62] = 0;
|
||||
out_317258154932342734[63] = 0;
|
||||
out_317258154932342734[64] = 0;
|
||||
out_317258154932342734[65] = 0;
|
||||
out_317258154932342734[66] = 0;
|
||||
out_317258154932342734[67] = 0;
|
||||
out_317258154932342734[68] = 0;
|
||||
out_317258154932342734[69] = 0;
|
||||
out_317258154932342734[70] = 1.0;
|
||||
out_317258154932342734[71] = 0;
|
||||
out_317258154932342734[72] = 0;
|
||||
out_317258154932342734[73] = 0;
|
||||
out_317258154932342734[74] = 0;
|
||||
out_317258154932342734[75] = 0;
|
||||
out_317258154932342734[76] = 0;
|
||||
out_317258154932342734[77] = 0;
|
||||
out_317258154932342734[78] = 0;
|
||||
out_317258154932342734[79] = 0;
|
||||
out_317258154932342734[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_7554938137534762046) {
|
||||
out_7554938137534762046[0] = state[0];
|
||||
out_7554938137534762046[1] = state[1];
|
||||
out_7554938137534762046[2] = state[2];
|
||||
out_7554938137534762046[3] = state[3];
|
||||
out_7554938137534762046[4] = state[4];
|
||||
out_7554938137534762046[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_7554938137534762046[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_7554938137534762046[7] = state[7];
|
||||
out_7554938137534762046[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_5753327761686972465) {
|
||||
out_5753327761686972465[0] = state[0];
|
||||
out_5753327761686972465[1] = state[1];
|
||||
out_5753327761686972465[2] = state[2];
|
||||
out_5753327761686972465[3] = state[3];
|
||||
out_5753327761686972465[4] = state[4];
|
||||
out_5753327761686972465[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_5753327761686972465[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_5753327761686972465[7] = state[7];
|
||||
out_5753327761686972465[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_6130812160595680525) {
|
||||
out_6130812160595680525[0] = 1;
|
||||
out_6130812160595680525[1] = 0;
|
||||
out_6130812160595680525[2] = 0;
|
||||
out_6130812160595680525[3] = 0;
|
||||
out_6130812160595680525[4] = 0;
|
||||
out_6130812160595680525[5] = 0;
|
||||
out_6130812160595680525[6] = 0;
|
||||
out_6130812160595680525[7] = 0;
|
||||
out_6130812160595680525[8] = 0;
|
||||
out_6130812160595680525[9] = 0;
|
||||
out_6130812160595680525[10] = 1;
|
||||
out_6130812160595680525[11] = 0;
|
||||
out_6130812160595680525[12] = 0;
|
||||
out_6130812160595680525[13] = 0;
|
||||
out_6130812160595680525[14] = 0;
|
||||
out_6130812160595680525[15] = 0;
|
||||
out_6130812160595680525[16] = 0;
|
||||
out_6130812160595680525[17] = 0;
|
||||
out_6130812160595680525[18] = 0;
|
||||
out_6130812160595680525[19] = 0;
|
||||
out_6130812160595680525[20] = 1;
|
||||
out_6130812160595680525[21] = 0;
|
||||
out_6130812160595680525[22] = 0;
|
||||
out_6130812160595680525[23] = 0;
|
||||
out_6130812160595680525[24] = 0;
|
||||
out_6130812160595680525[25] = 0;
|
||||
out_6130812160595680525[26] = 0;
|
||||
out_6130812160595680525[27] = 0;
|
||||
out_6130812160595680525[28] = 0;
|
||||
out_6130812160595680525[29] = 0;
|
||||
out_6130812160595680525[30] = 1;
|
||||
out_6130812160595680525[31] = 0;
|
||||
out_6130812160595680525[32] = 0;
|
||||
out_6130812160595680525[33] = 0;
|
||||
out_6130812160595680525[34] = 0;
|
||||
out_6130812160595680525[35] = 0;
|
||||
out_6130812160595680525[36] = 0;
|
||||
out_6130812160595680525[37] = 0;
|
||||
out_6130812160595680525[38] = 0;
|
||||
out_6130812160595680525[39] = 0;
|
||||
out_6130812160595680525[40] = 1;
|
||||
out_6130812160595680525[41] = 0;
|
||||
out_6130812160595680525[42] = 0;
|
||||
out_6130812160595680525[43] = 0;
|
||||
out_6130812160595680525[44] = 0;
|
||||
out_6130812160595680525[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_6130812160595680525[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_6130812160595680525[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6130812160595680525[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6130812160595680525[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_6130812160595680525[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_6130812160595680525[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_6130812160595680525[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6130812160595680525[53] = -9.8000000000000007*dt;
|
||||
out_6130812160595680525[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_6130812160595680525[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_6130812160595680525[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6130812160595680525[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6130812160595680525[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_6130812160595680525[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_6130812160595680525[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_6130812160595680525[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6130812160595680525[62] = 0;
|
||||
out_6130812160595680525[63] = 0;
|
||||
out_6130812160595680525[64] = 0;
|
||||
out_6130812160595680525[65] = 0;
|
||||
out_6130812160595680525[66] = 0;
|
||||
out_6130812160595680525[67] = 0;
|
||||
out_6130812160595680525[68] = 0;
|
||||
out_6130812160595680525[69] = 0;
|
||||
out_6130812160595680525[70] = 1;
|
||||
out_6130812160595680525[71] = 0;
|
||||
out_6130812160595680525[72] = 0;
|
||||
out_6130812160595680525[73] = 0;
|
||||
out_6130812160595680525[74] = 0;
|
||||
out_6130812160595680525[75] = 0;
|
||||
out_6130812160595680525[76] = 0;
|
||||
out_6130812160595680525[77] = 0;
|
||||
out_6130812160595680525[78] = 0;
|
||||
out_6130812160595680525[79] = 0;
|
||||
out_6130812160595680525[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_8289678864136186323) {
|
||||
out_8289678864136186323[0] = 1;
|
||||
out_8289678864136186323[1] = 0;
|
||||
out_8289678864136186323[2] = 0;
|
||||
out_8289678864136186323[3] = 0;
|
||||
out_8289678864136186323[4] = 0;
|
||||
out_8289678864136186323[5] = 0;
|
||||
out_8289678864136186323[6] = 0;
|
||||
out_8289678864136186323[7] = 0;
|
||||
out_8289678864136186323[8] = 0;
|
||||
out_8289678864136186323[9] = 0;
|
||||
out_8289678864136186323[10] = 1;
|
||||
out_8289678864136186323[11] = 0;
|
||||
out_8289678864136186323[12] = 0;
|
||||
out_8289678864136186323[13] = 0;
|
||||
out_8289678864136186323[14] = 0;
|
||||
out_8289678864136186323[15] = 0;
|
||||
out_8289678864136186323[16] = 0;
|
||||
out_8289678864136186323[17] = 0;
|
||||
out_8289678864136186323[18] = 0;
|
||||
out_8289678864136186323[19] = 0;
|
||||
out_8289678864136186323[20] = 1;
|
||||
out_8289678864136186323[21] = 0;
|
||||
out_8289678864136186323[22] = 0;
|
||||
out_8289678864136186323[23] = 0;
|
||||
out_8289678864136186323[24] = 0;
|
||||
out_8289678864136186323[25] = 0;
|
||||
out_8289678864136186323[26] = 0;
|
||||
out_8289678864136186323[27] = 0;
|
||||
out_8289678864136186323[28] = 0;
|
||||
out_8289678864136186323[29] = 0;
|
||||
out_8289678864136186323[30] = 1;
|
||||
out_8289678864136186323[31] = 0;
|
||||
out_8289678864136186323[32] = 0;
|
||||
out_8289678864136186323[33] = 0;
|
||||
out_8289678864136186323[34] = 0;
|
||||
out_8289678864136186323[35] = 0;
|
||||
out_8289678864136186323[36] = 0;
|
||||
out_8289678864136186323[37] = 0;
|
||||
out_8289678864136186323[38] = 0;
|
||||
out_8289678864136186323[39] = 0;
|
||||
out_8289678864136186323[40] = 1;
|
||||
out_8289678864136186323[41] = 0;
|
||||
out_8289678864136186323[42] = 0;
|
||||
out_8289678864136186323[43] = 0;
|
||||
out_8289678864136186323[44] = 0;
|
||||
out_8289678864136186323[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_8289678864136186323[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_8289678864136186323[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8289678864136186323[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8289678864136186323[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_8289678864136186323[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_8289678864136186323[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_8289678864136186323[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_8289678864136186323[53] = -9.8000000000000007*dt;
|
||||
out_8289678864136186323[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_8289678864136186323[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_8289678864136186323[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8289678864136186323[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8289678864136186323[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_8289678864136186323[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_8289678864136186323[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_8289678864136186323[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_8289678864136186323[62] = 0;
|
||||
out_8289678864136186323[63] = 0;
|
||||
out_8289678864136186323[64] = 0;
|
||||
out_8289678864136186323[65] = 0;
|
||||
out_8289678864136186323[66] = 0;
|
||||
out_8289678864136186323[67] = 0;
|
||||
out_8289678864136186323[68] = 0;
|
||||
out_8289678864136186323[69] = 0;
|
||||
out_8289678864136186323[70] = 1;
|
||||
out_8289678864136186323[71] = 0;
|
||||
out_8289678864136186323[72] = 0;
|
||||
out_8289678864136186323[73] = 0;
|
||||
out_8289678864136186323[74] = 0;
|
||||
out_8289678864136186323[75] = 0;
|
||||
out_8289678864136186323[76] = 0;
|
||||
out_8289678864136186323[77] = 0;
|
||||
out_8289678864136186323[78] = 0;
|
||||
out_8289678864136186323[79] = 0;
|
||||
out_8289678864136186323[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_8951477070372806105) {
|
||||
out_8951477070372806105[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_5741342572066533641) {
|
||||
out_5741342572066533641[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_9087508506817314964) {
|
||||
out_9087508506817314964[0] = 0;
|
||||
out_9087508506817314964[1] = 0;
|
||||
out_9087508506817314964[2] = 0;
|
||||
out_9087508506817314964[3] = 0;
|
||||
out_9087508506817314964[4] = 0;
|
||||
out_9087508506817314964[5] = 0;
|
||||
out_9087508506817314964[6] = 1;
|
||||
out_9087508506817314964[7] = 0;
|
||||
out_9087508506817314964[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_1067105512112888160) {
|
||||
out_1067105512112888160[0] = 0;
|
||||
out_1067105512112888160[1] = 0;
|
||||
out_1067105512112888160[2] = 0;
|
||||
out_1067105512112888160[3] = 0;
|
||||
out_1067105512112888160[4] = 0;
|
||||
out_1067105512112888160[5] = 0;
|
||||
out_1067105512112888160[6] = 1;
|
||||
out_1067105512112888160[7] = 0;
|
||||
out_1067105512112888160[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_4067561432286125622) {
|
||||
out_4067561432286125622[0] = state[4];
|
||||
out_4067561432286125622[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_3184705636802324409) {
|
||||
out_3184705636802324409[0] = state[4];
|
||||
out_3184705636802324409[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_1669116873140388071) {
|
||||
out_1669116873140388071[0] = 0;
|
||||
out_1669116873140388071[1] = 0;
|
||||
out_1669116873140388071[2] = 0;
|
||||
out_1669116873140388071[3] = 0;
|
||||
out_1669116873140388071[4] = 1;
|
||||
out_1669116873140388071[5] = 0;
|
||||
out_1669116873140388071[6] = 0;
|
||||
out_1669116873140388071[7] = 0;
|
||||
out_1669116873140388071[8] = 0;
|
||||
out_1669116873140388071[9] = 0;
|
||||
out_1669116873140388071[10] = 0;
|
||||
out_1669116873140388071[11] = 0;
|
||||
out_1669116873140388071[12] = 0;
|
||||
out_1669116873140388071[13] = 0;
|
||||
out_1669116873140388071[14] = 1;
|
||||
out_1669116873140388071[15] = 0;
|
||||
out_1669116873140388071[16] = 0;
|
||||
out_1669116873140388071[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_3679403814853620874) {
|
||||
out_3679403814853620874[0] = 0;
|
||||
out_3679403814853620874[1] = 0;
|
||||
out_3679403814853620874[2] = 0;
|
||||
out_3679403814853620874[3] = 0;
|
||||
out_3679403814853620874[4] = 1;
|
||||
out_3679403814853620874[5] = 0;
|
||||
out_3679403814853620874[6] = 0;
|
||||
out_3679403814853620874[7] = 0;
|
||||
out_3679403814853620874[8] = 0;
|
||||
out_3679403814853620874[9] = 0;
|
||||
out_3679403814853620874[10] = 0;
|
||||
out_3679403814853620874[11] = 0;
|
||||
out_3679403814853620874[12] = 0;
|
||||
out_3679403814853620874[13] = 0;
|
||||
out_3679403814853620874[14] = 1;
|
||||
out_3679403814853620874[15] = 0;
|
||||
out_3679403814853620874[16] = 0;
|
||||
out_3679403814853620874[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_7500845677743922938) {
|
||||
out_7500845677743922938[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_5619733767201413514) {
|
||||
out_5619733767201413514[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_4831539236764628454) {
|
||||
out_4831539236764628454[0] = 0;
|
||||
out_4831539236764628454[1] = 0;
|
||||
out_4831539236764628454[2] = 0;
|
||||
out_4831539236764628454[3] = 0;
|
||||
out_4831539236764628454[4] = 1;
|
||||
out_4831539236764628454[5] = 0;
|
||||
out_4831539236764628454[6] = 0;
|
||||
out_4831539236764628454[7] = 0;
|
||||
out_4831539236764628454[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3585438470620136787) {
|
||||
out_3585438470620136787[0] = 0;
|
||||
out_3585438470620136787[1] = 0;
|
||||
out_3585438470620136787[2] = 0;
|
||||
out_3585438470620136787[3] = 0;
|
||||
out_3585438470620136787[4] = 1;
|
||||
out_3585438470620136787[5] = 0;
|
||||
out_3585438470620136787[6] = 0;
|
||||
out_3585438470620136787[7] = 0;
|
||||
out_3585438470620136787[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_8341493075156931027) {
|
||||
out_8341493075156931027[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_138251146792324442) {
|
||||
out_138251146792324442[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5617732248018180428) {
|
||||
out_5617732248018180428[0] = 0;
|
||||
out_5617732248018180428[1] = 0;
|
||||
out_5617732248018180428[2] = 0;
|
||||
out_5617732248018180428[3] = 0;
|
||||
out_5617732248018180428[4] = 0;
|
||||
out_5617732248018180428[5] = 0;
|
||||
out_5617732248018180428[6] = 0;
|
||||
out_5617732248018180428[7] = 1;
|
||||
out_5617732248018180428[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_2674397806761168064) {
|
||||
out_2674397806761168064[0] = 0;
|
||||
out_2674397806761168064[1] = 0;
|
||||
out_2674397806761168064[2] = 0;
|
||||
out_2674397806761168064[3] = 0;
|
||||
out_2674397806761168064[4] = 0;
|
||||
out_2674397806761168064[5] = 0;
|
||||
out_2674397806761168064[6] = 0;
|
||||
out_2674397806761168064[7] = 1;
|
||||
out_2674397806761168064[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_5342806970691419479) {
|
||||
out_5342806970691419479[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_7703728233982172330) {
|
||||
out_7703728233982172330[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_2656775924964203543) {
|
||||
out_2656775924964203543[0] = 0;
|
||||
out_2656775924964203543[1] = 0;
|
||||
out_2656775924964203543[2] = 0;
|
||||
out_2656775924964203543[3] = 1;
|
||||
out_2656775924964203543[4] = 0;
|
||||
out_2656775924964203543[5] = 0;
|
||||
out_2656775924964203543[6] = 0;
|
||||
out_2656775924964203543[7] = 0;
|
||||
out_2656775924964203543[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5809032541804080004) {
|
||||
out_5809032541804080004[0] = 0;
|
||||
out_5809032541804080004[1] = 0;
|
||||
out_5809032541804080004[2] = 0;
|
||||
out_5809032541804080004[3] = 1;
|
||||
out_5809032541804080004[4] = 0;
|
||||
out_5809032541804080004[5] = 0;
|
||||
out_5809032541804080004[6] = 0;
|
||||
out_5809032541804080004[7] = 0;
|
||||
out_5809032541804080004[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_2538465680062629706) {
|
||||
out_2538465680062629706[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_3842044757553670678) {
|
||||
out_3842044757553670678[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_5341770581079020638) {
|
||||
out_5341770581079020638[0] = 0;
|
||||
out_5341770581079020638[1] = 1;
|
||||
out_5341770581079020638[2] = 0;
|
||||
out_5341770581079020638[3] = 0;
|
||||
out_5341770581079020638[4] = 0;
|
||||
out_5341770581079020638[5] = 0;
|
||||
out_5341770581079020638[6] = 0;
|
||||
out_5341770581079020638[7] = 0;
|
||||
out_5341770581079020638[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_4095669814934528971) {
|
||||
out_4095669814934528971[0] = 0;
|
||||
out_4095669814934528971[1] = 1;
|
||||
out_4095669814934528971[2] = 0;
|
||||
out_4095669814934528971[3] = 0;
|
||||
out_4095669814934528971[4] = 0;
|
||||
out_4095669814934528971[5] = 0;
|
||||
out_4095669814934528971[6] = 0;
|
||||
out_4095669814934528971[7] = 0;
|
||||
out_4095669814934528971[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_572823783042442857) {
|
||||
out_572823783042442857[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_3081542204526064016) {
|
||||
out_3081542204526064016[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7305400852644346889) {
|
||||
out_7305400852644346889[0] = 1;
|
||||
out_7305400852644346889[1] = 0;
|
||||
out_7305400852644346889[2] = 0;
|
||||
out_7305400852644346889[3] = 0;
|
||||
out_7305400852644346889[4] = 0;
|
||||
out_7305400852644346889[5] = 0;
|
||||
out_7305400852644346889[6] = 0;
|
||||
out_7305400852644346889[7] = 0;
|
||||
out_7305400852644346889[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_986729202135001603) {
|
||||
out_986729202135001603[0] = 1;
|
||||
out_986729202135001603[1] = 0;
|
||||
out_986729202135001603[2] = 0;
|
||||
out_986729202135001603[3] = 0;
|
||||
out_986729202135001603[4] = 0;
|
||||
out_986729202135001603[5] = 0;
|
||||
out_986729202135001603[6] = 0;
|
||||
out_986729202135001603[7] = 0;
|
||||
out_986729202135001603[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_8893848845723381957) {
|
||||
out_8893848845723381957[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_2103893043812511291) {
|
||||
out_2103893043812511291[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_4991524145784828952) {
|
||||
out_4991524145784828952[0] = 0;
|
||||
out_4991524145784828952[1] = 0;
|
||||
out_4991524145784828952[2] = 0;
|
||||
out_4991524145784828952[3] = 0;
|
||||
out_4991524145784828952[4] = 0;
|
||||
out_4991524145784828952[5] = 0;
|
||||
out_4991524145784828952[6] = 0;
|
||||
out_4991524145784828952[7] = 0;
|
||||
out_4991524145784828952[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_1097751473989848588) {
|
||||
out_1097751473989848588[0] = 0;
|
||||
out_1097751473989848588[1] = 0;
|
||||
out_1097751473989848588[2] = 0;
|
||||
out_1097751473989848588[3] = 0;
|
||||
out_1097751473989848588[4] = 0;
|
||||
out_1097751473989848588[5] = 0;
|
||||
out_1097751473989848588[6] = 0;
|
||||
out_1097751473989848588[7] = 0;
|
||||
out_1097751473989848588[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_729362937437787457) {
|
||||
err_fun(nom_x, delta_x, out_729362937437787457);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_6258559787192254109) {
|
||||
err_fun(nom_x, delta_x, out_6258559787192254109);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4929143687762803407) {
|
||||
inv_err_fun(nom_x, true_x, out_4929143687762803407);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8483442434741396819) {
|
||||
inv_err_fun(nom_x, true_x, out_8483442434741396819);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_8315888887196402654) {
|
||||
H_mod_fun(state, out_8315888887196402654);
|
||||
void car_H_mod_fun(double *state, double *out_317258154932342734) {
|
||||
H_mod_fun(state, out_317258154932342734);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_7554938137534762046) {
|
||||
f_fun(state, dt, out_7554938137534762046);
|
||||
void car_f_fun(double *state, double dt, double *out_5753327761686972465) {
|
||||
f_fun(state, dt, out_5753327761686972465);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_6130812160595680525) {
|
||||
F_fun(state, dt, out_6130812160595680525);
|
||||
void car_F_fun(double *state, double dt, double *out_8289678864136186323) {
|
||||
F_fun(state, dt, out_8289678864136186323);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_8951477070372806105) {
|
||||
h_25(state, unused, out_8951477070372806105);
|
||||
void car_h_25(double *state, double *unused, double *out_5741342572066533641) {
|
||||
h_25(state, unused, out_5741342572066533641);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_9087508506817314964) {
|
||||
H_25(state, unused, out_9087508506817314964);
|
||||
void car_H_25(double *state, double *unused, double *out_1067105512112888160) {
|
||||
H_25(state, unused, out_1067105512112888160);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_4067561432286125622) {
|
||||
h_24(state, unused, out_4067561432286125622);
|
||||
void car_h_24(double *state, double *unused, double *out_3184705636802324409) {
|
||||
h_24(state, unused, out_3184705636802324409);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_1669116873140388071) {
|
||||
H_24(state, unused, out_1669116873140388071);
|
||||
void car_H_24(double *state, double *unused, double *out_3679403814853620874) {
|
||||
H_24(state, unused, out_3679403814853620874);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_7500845677743922938) {
|
||||
h_30(state, unused, out_7500845677743922938);
|
||||
void car_h_30(double *state, double *unused, double *out_5619733767201413514) {
|
||||
h_30(state, unused, out_5619733767201413514);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_4831539236764628454) {
|
||||
H_30(state, unused, out_4831539236764628454);
|
||||
void car_H_30(double *state, double *unused, double *out_3585438470620136787) {
|
||||
H_30(state, unused, out_3585438470620136787);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_8341493075156931027) {
|
||||
h_26(state, unused, out_8341493075156931027);
|
||||
void car_h_26(double *state, double *unused, double *out_138251146792324442) {
|
||||
h_26(state, unused, out_138251146792324442);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5617732248018180428) {
|
||||
H_26(state, unused, out_5617732248018180428);
|
||||
void car_H_26(double *state, double *unused, double *out_2674397806761168064) {
|
||||
H_26(state, unused, out_2674397806761168064);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_5342806970691419479) {
|
||||
h_27(state, unused, out_5342806970691419479);
|
||||
void car_h_27(double *state, double *unused, double *out_7703728233982172330) {
|
||||
h_27(state, unused, out_7703728233982172330);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_2656775924964203543) {
|
||||
H_27(state, unused, out_2656775924964203543);
|
||||
void car_H_27(double *state, double *unused, double *out_5809032541804080004) {
|
||||
H_27(state, unused, out_5809032541804080004);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_2538465680062629706) {
|
||||
h_29(state, unused, out_2538465680062629706);
|
||||
void car_h_29(double *state, double *unused, double *out_3842044757553670678) {
|
||||
h_29(state, unused, out_3842044757553670678);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_5341770581079020638) {
|
||||
H_29(state, unused, out_5341770581079020638);
|
||||
void car_H_29(double *state, double *unused, double *out_4095669814934528971) {
|
||||
H_29(state, unused, out_4095669814934528971);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_572823783042442857) {
|
||||
h_28(state, unused, out_572823783042442857);
|
||||
void car_h_28(double *state, double *unused, double *out_3081542204526064016) {
|
||||
h_28(state, unused, out_3081542204526064016);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7305400852644346889) {
|
||||
H_28(state, unused, out_7305400852644346889);
|
||||
void car_H_28(double *state, double *unused, double *out_986729202135001603) {
|
||||
H_28(state, unused, out_986729202135001603);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_8893848845723381957) {
|
||||
h_31(state, unused, out_8893848845723381957);
|
||||
void car_h_31(double *state, double *unused, double *out_2103893043812511291) {
|
||||
h_31(state, unused, out_2103893043812511291);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_4991524145784828952) {
|
||||
H_31(state, unused, out_4991524145784828952);
|
||||
void car_H_31(double *state, double *unused, double *out_1097751473989848588) {
|
||||
H_31(state, unused, out_1097751473989848588);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_729362937437787457);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4929143687762803407);
|
||||
void car_H_mod_fun(double *state, double *out_8315888887196402654);
|
||||
void car_f_fun(double *state, double dt, double *out_7554938137534762046);
|
||||
void car_F_fun(double *state, double dt, double *out_6130812160595680525);
|
||||
void car_h_25(double *state, double *unused, double *out_8951477070372806105);
|
||||
void car_H_25(double *state, double *unused, double *out_9087508506817314964);
|
||||
void car_h_24(double *state, double *unused, double *out_4067561432286125622);
|
||||
void car_H_24(double *state, double *unused, double *out_1669116873140388071);
|
||||
void car_h_30(double *state, double *unused, double *out_7500845677743922938);
|
||||
void car_H_30(double *state, double *unused, double *out_4831539236764628454);
|
||||
void car_h_26(double *state, double *unused, double *out_8341493075156931027);
|
||||
void car_H_26(double *state, double *unused, double *out_5617732248018180428);
|
||||
void car_h_27(double *state, double *unused, double *out_5342806970691419479);
|
||||
void car_H_27(double *state, double *unused, double *out_2656775924964203543);
|
||||
void car_h_29(double *state, double *unused, double *out_2538465680062629706);
|
||||
void car_H_29(double *state, double *unused, double *out_5341770581079020638);
|
||||
void car_h_28(double *state, double *unused, double *out_572823783042442857);
|
||||
void car_H_28(double *state, double *unused, double *out_7305400852644346889);
|
||||
void car_h_31(double *state, double *unused, double *out_8893848845723381957);
|
||||
void car_H_31(double *state, double *unused, double *out_4991524145784828952);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_6258559787192254109);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8483442434741396819);
|
||||
void car_H_mod_fun(double *state, double *out_317258154932342734);
|
||||
void car_f_fun(double *state, double dt, double *out_5753327761686972465);
|
||||
void car_F_fun(double *state, double dt, double *out_8289678864136186323);
|
||||
void car_h_25(double *state, double *unused, double *out_5741342572066533641);
|
||||
void car_H_25(double *state, double *unused, double *out_1067105512112888160);
|
||||
void car_h_24(double *state, double *unused, double *out_3184705636802324409);
|
||||
void car_H_24(double *state, double *unused, double *out_3679403814853620874);
|
||||
void car_h_30(double *state, double *unused, double *out_5619733767201413514);
|
||||
void car_H_30(double *state, double *unused, double *out_3585438470620136787);
|
||||
void car_h_26(double *state, double *unused, double *out_138251146792324442);
|
||||
void car_H_26(double *state, double *unused, double *out_2674397806761168064);
|
||||
void car_h_27(double *state, double *unused, double *out_7703728233982172330);
|
||||
void car_H_27(double *state, double *unused, double *out_5809032541804080004);
|
||||
void car_h_29(double *state, double *unused, double *out_3842044757553670678);
|
||||
void car_H_29(double *state, double *unused, double *out_4095669814934528971);
|
||||
void car_h_28(double *state, double *unused, double *out_3081542204526064016);
|
||||
void car_H_28(double *state, double *unused, double *out_986729202135001603);
|
||||
void car_h_31(double *state, double *unused, double *out_2103893043812511291);
|
||||
void car_H_31(double *state, double *unused, double *out_1097751473989848588);
|
||||
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);
|
||||
|
||||
@@ -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_3985733553669045281) {
|
||||
out_3985733553669045281[0] = delta_x[0] + nom_x[0];
|
||||
out_3985733553669045281[1] = delta_x[1] + nom_x[1];
|
||||
out_3985733553669045281[2] = delta_x[2] + nom_x[2];
|
||||
out_3985733553669045281[3] = delta_x[3] + nom_x[3];
|
||||
out_3985733553669045281[4] = delta_x[4] + nom_x[4];
|
||||
out_3985733553669045281[5] = delta_x[5] + nom_x[5];
|
||||
out_3985733553669045281[6] = delta_x[6] + nom_x[6];
|
||||
out_3985733553669045281[7] = delta_x[7] + nom_x[7];
|
||||
out_3985733553669045281[8] = delta_x[8] + nom_x[8];
|
||||
out_3985733553669045281[9] = delta_x[9] + nom_x[9];
|
||||
out_3985733553669045281[10] = delta_x[10] + nom_x[10];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_4640013062227700412) {
|
||||
out_4640013062227700412[0] = delta_x[0] + nom_x[0];
|
||||
out_4640013062227700412[1] = delta_x[1] + nom_x[1];
|
||||
out_4640013062227700412[2] = delta_x[2] + nom_x[2];
|
||||
out_4640013062227700412[3] = delta_x[3] + nom_x[3];
|
||||
out_4640013062227700412[4] = delta_x[4] + nom_x[4];
|
||||
out_4640013062227700412[5] = delta_x[5] + nom_x[5];
|
||||
out_4640013062227700412[6] = delta_x[6] + nom_x[6];
|
||||
out_4640013062227700412[7] = delta_x[7] + nom_x[7];
|
||||
out_4640013062227700412[8] = delta_x[8] + nom_x[8];
|
||||
out_4640013062227700412[9] = delta_x[9] + nom_x[9];
|
||||
out_4640013062227700412[10] = delta_x[10] + nom_x[10];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_668705734016375974) {
|
||||
out_668705734016375974[0] = -nom_x[0] + true_x[0];
|
||||
out_668705734016375974[1] = -nom_x[1] + true_x[1];
|
||||
out_668705734016375974[2] = -nom_x[2] + true_x[2];
|
||||
out_668705734016375974[3] = -nom_x[3] + true_x[3];
|
||||
out_668705734016375974[4] = -nom_x[4] + true_x[4];
|
||||
out_668705734016375974[5] = -nom_x[5] + true_x[5];
|
||||
out_668705734016375974[6] = -nom_x[6] + true_x[6];
|
||||
out_668705734016375974[7] = -nom_x[7] + true_x[7];
|
||||
out_668705734016375974[8] = -nom_x[8] + true_x[8];
|
||||
out_668705734016375974[9] = -nom_x[9] + true_x[9];
|
||||
out_668705734016375974[10] = -nom_x[10] + true_x[10];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_4915148707996812550) {
|
||||
out_4915148707996812550[0] = -nom_x[0] + true_x[0];
|
||||
out_4915148707996812550[1] = -nom_x[1] + true_x[1];
|
||||
out_4915148707996812550[2] = -nom_x[2] + true_x[2];
|
||||
out_4915148707996812550[3] = -nom_x[3] + true_x[3];
|
||||
out_4915148707996812550[4] = -nom_x[4] + true_x[4];
|
||||
out_4915148707996812550[5] = -nom_x[5] + true_x[5];
|
||||
out_4915148707996812550[6] = -nom_x[6] + true_x[6];
|
||||
out_4915148707996812550[7] = -nom_x[7] + true_x[7];
|
||||
out_4915148707996812550[8] = -nom_x[8] + true_x[8];
|
||||
out_4915148707996812550[9] = -nom_x[9] + true_x[9];
|
||||
out_4915148707996812550[10] = -nom_x[10] + true_x[10];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_1646191520139698591) {
|
||||
out_1646191520139698591[0] = 1.0;
|
||||
out_1646191520139698591[1] = 0;
|
||||
out_1646191520139698591[2] = 0;
|
||||
out_1646191520139698591[3] = 0;
|
||||
out_1646191520139698591[4] = 0;
|
||||
out_1646191520139698591[5] = 0;
|
||||
out_1646191520139698591[6] = 0;
|
||||
out_1646191520139698591[7] = 0;
|
||||
out_1646191520139698591[8] = 0;
|
||||
out_1646191520139698591[9] = 0;
|
||||
out_1646191520139698591[10] = 0;
|
||||
out_1646191520139698591[11] = 0;
|
||||
out_1646191520139698591[12] = 1.0;
|
||||
out_1646191520139698591[13] = 0;
|
||||
out_1646191520139698591[14] = 0;
|
||||
out_1646191520139698591[15] = 0;
|
||||
out_1646191520139698591[16] = 0;
|
||||
out_1646191520139698591[17] = 0;
|
||||
out_1646191520139698591[18] = 0;
|
||||
out_1646191520139698591[19] = 0;
|
||||
out_1646191520139698591[20] = 0;
|
||||
out_1646191520139698591[21] = 0;
|
||||
out_1646191520139698591[22] = 0;
|
||||
out_1646191520139698591[23] = 0;
|
||||
out_1646191520139698591[24] = 1.0;
|
||||
out_1646191520139698591[25] = 0;
|
||||
out_1646191520139698591[26] = 0;
|
||||
out_1646191520139698591[27] = 0;
|
||||
out_1646191520139698591[28] = 0;
|
||||
out_1646191520139698591[29] = 0;
|
||||
out_1646191520139698591[30] = 0;
|
||||
out_1646191520139698591[31] = 0;
|
||||
out_1646191520139698591[32] = 0;
|
||||
out_1646191520139698591[33] = 0;
|
||||
out_1646191520139698591[34] = 0;
|
||||
out_1646191520139698591[35] = 0;
|
||||
out_1646191520139698591[36] = 1.0;
|
||||
out_1646191520139698591[37] = 0;
|
||||
out_1646191520139698591[38] = 0;
|
||||
out_1646191520139698591[39] = 0;
|
||||
out_1646191520139698591[40] = 0;
|
||||
out_1646191520139698591[41] = 0;
|
||||
out_1646191520139698591[42] = 0;
|
||||
out_1646191520139698591[43] = 0;
|
||||
out_1646191520139698591[44] = 0;
|
||||
out_1646191520139698591[45] = 0;
|
||||
out_1646191520139698591[46] = 0;
|
||||
out_1646191520139698591[47] = 0;
|
||||
out_1646191520139698591[48] = 1.0;
|
||||
out_1646191520139698591[49] = 0;
|
||||
out_1646191520139698591[50] = 0;
|
||||
out_1646191520139698591[51] = 0;
|
||||
out_1646191520139698591[52] = 0;
|
||||
out_1646191520139698591[53] = 0;
|
||||
out_1646191520139698591[54] = 0;
|
||||
out_1646191520139698591[55] = 0;
|
||||
out_1646191520139698591[56] = 0;
|
||||
out_1646191520139698591[57] = 0;
|
||||
out_1646191520139698591[58] = 0;
|
||||
out_1646191520139698591[59] = 0;
|
||||
out_1646191520139698591[60] = 1.0;
|
||||
out_1646191520139698591[61] = 0;
|
||||
out_1646191520139698591[62] = 0;
|
||||
out_1646191520139698591[63] = 0;
|
||||
out_1646191520139698591[64] = 0;
|
||||
out_1646191520139698591[65] = 0;
|
||||
out_1646191520139698591[66] = 0;
|
||||
out_1646191520139698591[67] = 0;
|
||||
out_1646191520139698591[68] = 0;
|
||||
out_1646191520139698591[69] = 0;
|
||||
out_1646191520139698591[70] = 0;
|
||||
out_1646191520139698591[71] = 0;
|
||||
out_1646191520139698591[72] = 1.0;
|
||||
out_1646191520139698591[73] = 0;
|
||||
out_1646191520139698591[74] = 0;
|
||||
out_1646191520139698591[75] = 0;
|
||||
out_1646191520139698591[76] = 0;
|
||||
out_1646191520139698591[77] = 0;
|
||||
out_1646191520139698591[78] = 0;
|
||||
out_1646191520139698591[79] = 0;
|
||||
out_1646191520139698591[80] = 0;
|
||||
out_1646191520139698591[81] = 0;
|
||||
out_1646191520139698591[82] = 0;
|
||||
out_1646191520139698591[83] = 0;
|
||||
out_1646191520139698591[84] = 1.0;
|
||||
out_1646191520139698591[85] = 0;
|
||||
out_1646191520139698591[86] = 0;
|
||||
out_1646191520139698591[87] = 0;
|
||||
out_1646191520139698591[88] = 0;
|
||||
out_1646191520139698591[89] = 0;
|
||||
out_1646191520139698591[90] = 0;
|
||||
out_1646191520139698591[91] = 0;
|
||||
out_1646191520139698591[92] = 0;
|
||||
out_1646191520139698591[93] = 0;
|
||||
out_1646191520139698591[94] = 0;
|
||||
out_1646191520139698591[95] = 0;
|
||||
out_1646191520139698591[96] = 1.0;
|
||||
out_1646191520139698591[97] = 0;
|
||||
out_1646191520139698591[98] = 0;
|
||||
out_1646191520139698591[99] = 0;
|
||||
out_1646191520139698591[100] = 0;
|
||||
out_1646191520139698591[101] = 0;
|
||||
out_1646191520139698591[102] = 0;
|
||||
out_1646191520139698591[103] = 0;
|
||||
out_1646191520139698591[104] = 0;
|
||||
out_1646191520139698591[105] = 0;
|
||||
out_1646191520139698591[106] = 0;
|
||||
out_1646191520139698591[107] = 0;
|
||||
out_1646191520139698591[108] = 1.0;
|
||||
out_1646191520139698591[109] = 0;
|
||||
out_1646191520139698591[110] = 0;
|
||||
out_1646191520139698591[111] = 0;
|
||||
out_1646191520139698591[112] = 0;
|
||||
out_1646191520139698591[113] = 0;
|
||||
out_1646191520139698591[114] = 0;
|
||||
out_1646191520139698591[115] = 0;
|
||||
out_1646191520139698591[116] = 0;
|
||||
out_1646191520139698591[117] = 0;
|
||||
out_1646191520139698591[118] = 0;
|
||||
out_1646191520139698591[119] = 0;
|
||||
out_1646191520139698591[120] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_8574237875034322764) {
|
||||
out_8574237875034322764[0] = 1.0;
|
||||
out_8574237875034322764[1] = 0;
|
||||
out_8574237875034322764[2] = 0;
|
||||
out_8574237875034322764[3] = 0;
|
||||
out_8574237875034322764[4] = 0;
|
||||
out_8574237875034322764[5] = 0;
|
||||
out_8574237875034322764[6] = 0;
|
||||
out_8574237875034322764[7] = 0;
|
||||
out_8574237875034322764[8] = 0;
|
||||
out_8574237875034322764[9] = 0;
|
||||
out_8574237875034322764[10] = 0;
|
||||
out_8574237875034322764[11] = 0;
|
||||
out_8574237875034322764[12] = 1.0;
|
||||
out_8574237875034322764[13] = 0;
|
||||
out_8574237875034322764[14] = 0;
|
||||
out_8574237875034322764[15] = 0;
|
||||
out_8574237875034322764[16] = 0;
|
||||
out_8574237875034322764[17] = 0;
|
||||
out_8574237875034322764[18] = 0;
|
||||
out_8574237875034322764[19] = 0;
|
||||
out_8574237875034322764[20] = 0;
|
||||
out_8574237875034322764[21] = 0;
|
||||
out_8574237875034322764[22] = 0;
|
||||
out_8574237875034322764[23] = 0;
|
||||
out_8574237875034322764[24] = 1.0;
|
||||
out_8574237875034322764[25] = 0;
|
||||
out_8574237875034322764[26] = 0;
|
||||
out_8574237875034322764[27] = 0;
|
||||
out_8574237875034322764[28] = 0;
|
||||
out_8574237875034322764[29] = 0;
|
||||
out_8574237875034322764[30] = 0;
|
||||
out_8574237875034322764[31] = 0;
|
||||
out_8574237875034322764[32] = 0;
|
||||
out_8574237875034322764[33] = 0;
|
||||
out_8574237875034322764[34] = 0;
|
||||
out_8574237875034322764[35] = 0;
|
||||
out_8574237875034322764[36] = 1.0;
|
||||
out_8574237875034322764[37] = 0;
|
||||
out_8574237875034322764[38] = 0;
|
||||
out_8574237875034322764[39] = 0;
|
||||
out_8574237875034322764[40] = 0;
|
||||
out_8574237875034322764[41] = 0;
|
||||
out_8574237875034322764[42] = 0;
|
||||
out_8574237875034322764[43] = 0;
|
||||
out_8574237875034322764[44] = 0;
|
||||
out_8574237875034322764[45] = 0;
|
||||
out_8574237875034322764[46] = 0;
|
||||
out_8574237875034322764[47] = 0;
|
||||
out_8574237875034322764[48] = 1.0;
|
||||
out_8574237875034322764[49] = 0;
|
||||
out_8574237875034322764[50] = 0;
|
||||
out_8574237875034322764[51] = 0;
|
||||
out_8574237875034322764[52] = 0;
|
||||
out_8574237875034322764[53] = 0;
|
||||
out_8574237875034322764[54] = 0;
|
||||
out_8574237875034322764[55] = 0;
|
||||
out_8574237875034322764[56] = 0;
|
||||
out_8574237875034322764[57] = 0;
|
||||
out_8574237875034322764[58] = 0;
|
||||
out_8574237875034322764[59] = 0;
|
||||
out_8574237875034322764[60] = 1.0;
|
||||
out_8574237875034322764[61] = 0;
|
||||
out_8574237875034322764[62] = 0;
|
||||
out_8574237875034322764[63] = 0;
|
||||
out_8574237875034322764[64] = 0;
|
||||
out_8574237875034322764[65] = 0;
|
||||
out_8574237875034322764[66] = 0;
|
||||
out_8574237875034322764[67] = 0;
|
||||
out_8574237875034322764[68] = 0;
|
||||
out_8574237875034322764[69] = 0;
|
||||
out_8574237875034322764[70] = 0;
|
||||
out_8574237875034322764[71] = 0;
|
||||
out_8574237875034322764[72] = 1.0;
|
||||
out_8574237875034322764[73] = 0;
|
||||
out_8574237875034322764[74] = 0;
|
||||
out_8574237875034322764[75] = 0;
|
||||
out_8574237875034322764[76] = 0;
|
||||
out_8574237875034322764[77] = 0;
|
||||
out_8574237875034322764[78] = 0;
|
||||
out_8574237875034322764[79] = 0;
|
||||
out_8574237875034322764[80] = 0;
|
||||
out_8574237875034322764[81] = 0;
|
||||
out_8574237875034322764[82] = 0;
|
||||
out_8574237875034322764[83] = 0;
|
||||
out_8574237875034322764[84] = 1.0;
|
||||
out_8574237875034322764[85] = 0;
|
||||
out_8574237875034322764[86] = 0;
|
||||
out_8574237875034322764[87] = 0;
|
||||
out_8574237875034322764[88] = 0;
|
||||
out_8574237875034322764[89] = 0;
|
||||
out_8574237875034322764[90] = 0;
|
||||
out_8574237875034322764[91] = 0;
|
||||
out_8574237875034322764[92] = 0;
|
||||
out_8574237875034322764[93] = 0;
|
||||
out_8574237875034322764[94] = 0;
|
||||
out_8574237875034322764[95] = 0;
|
||||
out_8574237875034322764[96] = 1.0;
|
||||
out_8574237875034322764[97] = 0;
|
||||
out_8574237875034322764[98] = 0;
|
||||
out_8574237875034322764[99] = 0;
|
||||
out_8574237875034322764[100] = 0;
|
||||
out_8574237875034322764[101] = 0;
|
||||
out_8574237875034322764[102] = 0;
|
||||
out_8574237875034322764[103] = 0;
|
||||
out_8574237875034322764[104] = 0;
|
||||
out_8574237875034322764[105] = 0;
|
||||
out_8574237875034322764[106] = 0;
|
||||
out_8574237875034322764[107] = 0;
|
||||
out_8574237875034322764[108] = 1.0;
|
||||
out_8574237875034322764[109] = 0;
|
||||
out_8574237875034322764[110] = 0;
|
||||
out_8574237875034322764[111] = 0;
|
||||
out_8574237875034322764[112] = 0;
|
||||
out_8574237875034322764[113] = 0;
|
||||
out_8574237875034322764[114] = 0;
|
||||
out_8574237875034322764[115] = 0;
|
||||
out_8574237875034322764[116] = 0;
|
||||
out_8574237875034322764[117] = 0;
|
||||
out_8574237875034322764[118] = 0;
|
||||
out_8574237875034322764[119] = 0;
|
||||
out_8574237875034322764[120] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_8136264243716214718) {
|
||||
out_8136264243716214718[0] = dt*state[3] + state[0];
|
||||
out_8136264243716214718[1] = dt*state[4] + state[1];
|
||||
out_8136264243716214718[2] = dt*state[5] + state[2];
|
||||
out_8136264243716214718[3] = state[3];
|
||||
out_8136264243716214718[4] = state[4];
|
||||
out_8136264243716214718[5] = state[5];
|
||||
out_8136264243716214718[6] = dt*state[7] + state[6];
|
||||
out_8136264243716214718[7] = dt*state[8] + state[7];
|
||||
out_8136264243716214718[8] = state[8];
|
||||
out_8136264243716214718[9] = state[9];
|
||||
out_8136264243716214718[10] = state[10];
|
||||
void f_fun(double *state, double dt, double *out_5636831905979297775) {
|
||||
out_5636831905979297775[0] = dt*state[3] + state[0];
|
||||
out_5636831905979297775[1] = dt*state[4] + state[1];
|
||||
out_5636831905979297775[2] = dt*state[5] + state[2];
|
||||
out_5636831905979297775[3] = state[3];
|
||||
out_5636831905979297775[4] = state[4];
|
||||
out_5636831905979297775[5] = state[5];
|
||||
out_5636831905979297775[6] = dt*state[7] + state[6];
|
||||
out_5636831905979297775[7] = dt*state[8] + state[7];
|
||||
out_5636831905979297775[8] = state[8];
|
||||
out_5636831905979297775[9] = state[9];
|
||||
out_5636831905979297775[10] = state[10];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_4236213521667949352) {
|
||||
out_4236213521667949352[0] = 1;
|
||||
out_4236213521667949352[1] = 0;
|
||||
out_4236213521667949352[2] = 0;
|
||||
out_4236213521667949352[3] = dt;
|
||||
out_4236213521667949352[4] = 0;
|
||||
out_4236213521667949352[5] = 0;
|
||||
out_4236213521667949352[6] = 0;
|
||||
out_4236213521667949352[7] = 0;
|
||||
out_4236213521667949352[8] = 0;
|
||||
out_4236213521667949352[9] = 0;
|
||||
out_4236213521667949352[10] = 0;
|
||||
out_4236213521667949352[11] = 0;
|
||||
out_4236213521667949352[12] = 1;
|
||||
out_4236213521667949352[13] = 0;
|
||||
out_4236213521667949352[14] = 0;
|
||||
out_4236213521667949352[15] = dt;
|
||||
out_4236213521667949352[16] = 0;
|
||||
out_4236213521667949352[17] = 0;
|
||||
out_4236213521667949352[18] = 0;
|
||||
out_4236213521667949352[19] = 0;
|
||||
out_4236213521667949352[20] = 0;
|
||||
out_4236213521667949352[21] = 0;
|
||||
out_4236213521667949352[22] = 0;
|
||||
out_4236213521667949352[23] = 0;
|
||||
out_4236213521667949352[24] = 1;
|
||||
out_4236213521667949352[25] = 0;
|
||||
out_4236213521667949352[26] = 0;
|
||||
out_4236213521667949352[27] = dt;
|
||||
out_4236213521667949352[28] = 0;
|
||||
out_4236213521667949352[29] = 0;
|
||||
out_4236213521667949352[30] = 0;
|
||||
out_4236213521667949352[31] = 0;
|
||||
out_4236213521667949352[32] = 0;
|
||||
out_4236213521667949352[33] = 0;
|
||||
out_4236213521667949352[34] = 0;
|
||||
out_4236213521667949352[35] = 0;
|
||||
out_4236213521667949352[36] = 1;
|
||||
out_4236213521667949352[37] = 0;
|
||||
out_4236213521667949352[38] = 0;
|
||||
out_4236213521667949352[39] = 0;
|
||||
out_4236213521667949352[40] = 0;
|
||||
out_4236213521667949352[41] = 0;
|
||||
out_4236213521667949352[42] = 0;
|
||||
out_4236213521667949352[43] = 0;
|
||||
out_4236213521667949352[44] = 0;
|
||||
out_4236213521667949352[45] = 0;
|
||||
out_4236213521667949352[46] = 0;
|
||||
out_4236213521667949352[47] = 0;
|
||||
out_4236213521667949352[48] = 1;
|
||||
out_4236213521667949352[49] = 0;
|
||||
out_4236213521667949352[50] = 0;
|
||||
out_4236213521667949352[51] = 0;
|
||||
out_4236213521667949352[52] = 0;
|
||||
out_4236213521667949352[53] = 0;
|
||||
out_4236213521667949352[54] = 0;
|
||||
out_4236213521667949352[55] = 0;
|
||||
out_4236213521667949352[56] = 0;
|
||||
out_4236213521667949352[57] = 0;
|
||||
out_4236213521667949352[58] = 0;
|
||||
out_4236213521667949352[59] = 0;
|
||||
out_4236213521667949352[60] = 1;
|
||||
out_4236213521667949352[61] = 0;
|
||||
out_4236213521667949352[62] = 0;
|
||||
out_4236213521667949352[63] = 0;
|
||||
out_4236213521667949352[64] = 0;
|
||||
out_4236213521667949352[65] = 0;
|
||||
out_4236213521667949352[66] = 0;
|
||||
out_4236213521667949352[67] = 0;
|
||||
out_4236213521667949352[68] = 0;
|
||||
out_4236213521667949352[69] = 0;
|
||||
out_4236213521667949352[70] = 0;
|
||||
out_4236213521667949352[71] = 0;
|
||||
out_4236213521667949352[72] = 1;
|
||||
out_4236213521667949352[73] = dt;
|
||||
out_4236213521667949352[74] = 0;
|
||||
out_4236213521667949352[75] = 0;
|
||||
out_4236213521667949352[76] = 0;
|
||||
out_4236213521667949352[77] = 0;
|
||||
out_4236213521667949352[78] = 0;
|
||||
out_4236213521667949352[79] = 0;
|
||||
out_4236213521667949352[80] = 0;
|
||||
out_4236213521667949352[81] = 0;
|
||||
out_4236213521667949352[82] = 0;
|
||||
out_4236213521667949352[83] = 0;
|
||||
out_4236213521667949352[84] = 1;
|
||||
out_4236213521667949352[85] = dt;
|
||||
out_4236213521667949352[86] = 0;
|
||||
out_4236213521667949352[87] = 0;
|
||||
out_4236213521667949352[88] = 0;
|
||||
out_4236213521667949352[89] = 0;
|
||||
out_4236213521667949352[90] = 0;
|
||||
out_4236213521667949352[91] = 0;
|
||||
out_4236213521667949352[92] = 0;
|
||||
out_4236213521667949352[93] = 0;
|
||||
out_4236213521667949352[94] = 0;
|
||||
out_4236213521667949352[95] = 0;
|
||||
out_4236213521667949352[96] = 1;
|
||||
out_4236213521667949352[97] = 0;
|
||||
out_4236213521667949352[98] = 0;
|
||||
out_4236213521667949352[99] = 0;
|
||||
out_4236213521667949352[100] = 0;
|
||||
out_4236213521667949352[101] = 0;
|
||||
out_4236213521667949352[102] = 0;
|
||||
out_4236213521667949352[103] = 0;
|
||||
out_4236213521667949352[104] = 0;
|
||||
out_4236213521667949352[105] = 0;
|
||||
out_4236213521667949352[106] = 0;
|
||||
out_4236213521667949352[107] = 0;
|
||||
out_4236213521667949352[108] = 1;
|
||||
out_4236213521667949352[109] = 0;
|
||||
out_4236213521667949352[110] = 0;
|
||||
out_4236213521667949352[111] = 0;
|
||||
out_4236213521667949352[112] = 0;
|
||||
out_4236213521667949352[113] = 0;
|
||||
out_4236213521667949352[114] = 0;
|
||||
out_4236213521667949352[115] = 0;
|
||||
out_4236213521667949352[116] = 0;
|
||||
out_4236213521667949352[117] = 0;
|
||||
out_4236213521667949352[118] = 0;
|
||||
out_4236213521667949352[119] = 0;
|
||||
out_4236213521667949352[120] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2727942986966876467) {
|
||||
out_2727942986966876467[0] = 1;
|
||||
out_2727942986966876467[1] = 0;
|
||||
out_2727942986966876467[2] = 0;
|
||||
out_2727942986966876467[3] = dt;
|
||||
out_2727942986966876467[4] = 0;
|
||||
out_2727942986966876467[5] = 0;
|
||||
out_2727942986966876467[6] = 0;
|
||||
out_2727942986966876467[7] = 0;
|
||||
out_2727942986966876467[8] = 0;
|
||||
out_2727942986966876467[9] = 0;
|
||||
out_2727942986966876467[10] = 0;
|
||||
out_2727942986966876467[11] = 0;
|
||||
out_2727942986966876467[12] = 1;
|
||||
out_2727942986966876467[13] = 0;
|
||||
out_2727942986966876467[14] = 0;
|
||||
out_2727942986966876467[15] = dt;
|
||||
out_2727942986966876467[16] = 0;
|
||||
out_2727942986966876467[17] = 0;
|
||||
out_2727942986966876467[18] = 0;
|
||||
out_2727942986966876467[19] = 0;
|
||||
out_2727942986966876467[20] = 0;
|
||||
out_2727942986966876467[21] = 0;
|
||||
out_2727942986966876467[22] = 0;
|
||||
out_2727942986966876467[23] = 0;
|
||||
out_2727942986966876467[24] = 1;
|
||||
out_2727942986966876467[25] = 0;
|
||||
out_2727942986966876467[26] = 0;
|
||||
out_2727942986966876467[27] = dt;
|
||||
out_2727942986966876467[28] = 0;
|
||||
out_2727942986966876467[29] = 0;
|
||||
out_2727942986966876467[30] = 0;
|
||||
out_2727942986966876467[31] = 0;
|
||||
out_2727942986966876467[32] = 0;
|
||||
out_2727942986966876467[33] = 0;
|
||||
out_2727942986966876467[34] = 0;
|
||||
out_2727942986966876467[35] = 0;
|
||||
out_2727942986966876467[36] = 1;
|
||||
out_2727942986966876467[37] = 0;
|
||||
out_2727942986966876467[38] = 0;
|
||||
out_2727942986966876467[39] = 0;
|
||||
out_2727942986966876467[40] = 0;
|
||||
out_2727942986966876467[41] = 0;
|
||||
out_2727942986966876467[42] = 0;
|
||||
out_2727942986966876467[43] = 0;
|
||||
out_2727942986966876467[44] = 0;
|
||||
out_2727942986966876467[45] = 0;
|
||||
out_2727942986966876467[46] = 0;
|
||||
out_2727942986966876467[47] = 0;
|
||||
out_2727942986966876467[48] = 1;
|
||||
out_2727942986966876467[49] = 0;
|
||||
out_2727942986966876467[50] = 0;
|
||||
out_2727942986966876467[51] = 0;
|
||||
out_2727942986966876467[52] = 0;
|
||||
out_2727942986966876467[53] = 0;
|
||||
out_2727942986966876467[54] = 0;
|
||||
out_2727942986966876467[55] = 0;
|
||||
out_2727942986966876467[56] = 0;
|
||||
out_2727942986966876467[57] = 0;
|
||||
out_2727942986966876467[58] = 0;
|
||||
out_2727942986966876467[59] = 0;
|
||||
out_2727942986966876467[60] = 1;
|
||||
out_2727942986966876467[61] = 0;
|
||||
out_2727942986966876467[62] = 0;
|
||||
out_2727942986966876467[63] = 0;
|
||||
out_2727942986966876467[64] = 0;
|
||||
out_2727942986966876467[65] = 0;
|
||||
out_2727942986966876467[66] = 0;
|
||||
out_2727942986966876467[67] = 0;
|
||||
out_2727942986966876467[68] = 0;
|
||||
out_2727942986966876467[69] = 0;
|
||||
out_2727942986966876467[70] = 0;
|
||||
out_2727942986966876467[71] = 0;
|
||||
out_2727942986966876467[72] = 1;
|
||||
out_2727942986966876467[73] = dt;
|
||||
out_2727942986966876467[74] = 0;
|
||||
out_2727942986966876467[75] = 0;
|
||||
out_2727942986966876467[76] = 0;
|
||||
out_2727942986966876467[77] = 0;
|
||||
out_2727942986966876467[78] = 0;
|
||||
out_2727942986966876467[79] = 0;
|
||||
out_2727942986966876467[80] = 0;
|
||||
out_2727942986966876467[81] = 0;
|
||||
out_2727942986966876467[82] = 0;
|
||||
out_2727942986966876467[83] = 0;
|
||||
out_2727942986966876467[84] = 1;
|
||||
out_2727942986966876467[85] = dt;
|
||||
out_2727942986966876467[86] = 0;
|
||||
out_2727942986966876467[87] = 0;
|
||||
out_2727942986966876467[88] = 0;
|
||||
out_2727942986966876467[89] = 0;
|
||||
out_2727942986966876467[90] = 0;
|
||||
out_2727942986966876467[91] = 0;
|
||||
out_2727942986966876467[92] = 0;
|
||||
out_2727942986966876467[93] = 0;
|
||||
out_2727942986966876467[94] = 0;
|
||||
out_2727942986966876467[95] = 0;
|
||||
out_2727942986966876467[96] = 1;
|
||||
out_2727942986966876467[97] = 0;
|
||||
out_2727942986966876467[98] = 0;
|
||||
out_2727942986966876467[99] = 0;
|
||||
out_2727942986966876467[100] = 0;
|
||||
out_2727942986966876467[101] = 0;
|
||||
out_2727942986966876467[102] = 0;
|
||||
out_2727942986966876467[103] = 0;
|
||||
out_2727942986966876467[104] = 0;
|
||||
out_2727942986966876467[105] = 0;
|
||||
out_2727942986966876467[106] = 0;
|
||||
out_2727942986966876467[107] = 0;
|
||||
out_2727942986966876467[108] = 1;
|
||||
out_2727942986966876467[109] = 0;
|
||||
out_2727942986966876467[110] = 0;
|
||||
out_2727942986966876467[111] = 0;
|
||||
out_2727942986966876467[112] = 0;
|
||||
out_2727942986966876467[113] = 0;
|
||||
out_2727942986966876467[114] = 0;
|
||||
out_2727942986966876467[115] = 0;
|
||||
out_2727942986966876467[116] = 0;
|
||||
out_2727942986966876467[117] = 0;
|
||||
out_2727942986966876467[118] = 0;
|
||||
out_2727942986966876467[119] = 0;
|
||||
out_2727942986966876467[120] = 1;
|
||||
}
|
||||
void h_6(double *state, double *sat_pos, double *out_8352111296884602789) {
|
||||
out_8352111296884602789[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_4554215930920042575) {
|
||||
out_4554215930920042575[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_2202359500111385714) {
|
||||
out_2202359500111385714[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_2202359500111385714[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_2202359500111385714[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_2202359500111385714[3] = 0;
|
||||
out_2202359500111385714[4] = 0;
|
||||
out_2202359500111385714[5] = 0;
|
||||
out_2202359500111385714[6] = 1;
|
||||
out_2202359500111385714[7] = 0;
|
||||
out_2202359500111385714[8] = 0;
|
||||
out_2202359500111385714[9] = 0;
|
||||
out_2202359500111385714[10] = 0;
|
||||
void H_6(double *state, double *sat_pos, double *out_895428558576169950) {
|
||||
out_895428558576169950[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_895428558576169950[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_895428558576169950[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_895428558576169950[3] = 0;
|
||||
out_895428558576169950[4] = 0;
|
||||
out_895428558576169950[5] = 0;
|
||||
out_895428558576169950[6] = 1;
|
||||
out_895428558576169950[7] = 0;
|
||||
out_895428558576169950[8] = 0;
|
||||
out_895428558576169950[9] = 0;
|
||||
out_895428558576169950[10] = 0;
|
||||
}
|
||||
void h_20(double *state, double *sat_pos, double *out_4887672178428492502) {
|
||||
out_4887672178428492502[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_6704926246356191984) {
|
||||
out_6704926246356191984[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_4961949425089895635) {
|
||||
out_4961949425089895635[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_4961949425089895635[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_4961949425089895635[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_4961949425089895635[3] = 0;
|
||||
out_4961949425089895635[4] = 0;
|
||||
out_4961949425089895635[5] = 0;
|
||||
out_4961949425089895635[6] = 1;
|
||||
out_4961949425089895635[7] = 0;
|
||||
out_4961949425089895635[8] = 0;
|
||||
out_4961949425089895635[9] = 1;
|
||||
out_4961949425089895635[10] = sat_pos[3];
|
||||
void H_20(double *state, double *sat_pos, double *out_3575717658219428128) {
|
||||
out_3575717658219428128[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_3575717658219428128[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_3575717658219428128[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_3575717658219428128[3] = 0;
|
||||
out_3575717658219428128[4] = 0;
|
||||
out_3575717658219428128[5] = 0;
|
||||
out_3575717658219428128[6] = 1;
|
||||
out_3575717658219428128[7] = 0;
|
||||
out_3575717658219428128[8] = 0;
|
||||
out_3575717658219428128[9] = 1;
|
||||
out_3575717658219428128[10] = sat_pos[3];
|
||||
}
|
||||
void h_7(double *state, double *sat_pos_vel, double *out_191603051630805475) {
|
||||
out_191603051630805475[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_6430340606903371855) {
|
||||
out_6430340606903371855[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_7197851591855342328) {
|
||||
out_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[6] = 0;
|
||||
out_7197851591855342328[7] = 1;
|
||||
out_7197851591855342328[8] = 0;
|
||||
out_7197851591855342328[9] = 0;
|
||||
out_7197851591855342328[10] = 0;
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_4789129994195662485) {
|
||||
out_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[6] = 0;
|
||||
out_4789129994195662485[7] = 1;
|
||||
out_4789129994195662485[8] = 0;
|
||||
out_4789129994195662485[9] = 0;
|
||||
out_4789129994195662485[10] = 0;
|
||||
}
|
||||
void h_21(double *state, double *sat_pos_vel, double *out_191603051630805475) {
|
||||
out_191603051630805475[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_6430340606903371855) {
|
||||
out_6430340606903371855[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_7197851591855342328) {
|
||||
out_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[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_7197851591855342328[6] = 0;
|
||||
out_7197851591855342328[7] = 1;
|
||||
out_7197851591855342328[8] = 0;
|
||||
out_7197851591855342328[9] = 0;
|
||||
out_7197851591855342328[10] = 0;
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_4789129994195662485) {
|
||||
out_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[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_4789129994195662485[6] = 0;
|
||||
out_4789129994195662485[7] = 1;
|
||||
out_4789129994195662485[8] = 0;
|
||||
out_4789129994195662485[9] = 0;
|
||||
out_4789129994195662485[10] = 0;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
|
||||
}
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_3985733553669045281) {
|
||||
err_fun(nom_x, delta_x, out_3985733553669045281);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_4640013062227700412) {
|
||||
err_fun(nom_x, delta_x, out_4640013062227700412);
|
||||
}
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_668705734016375974) {
|
||||
inv_err_fun(nom_x, true_x, out_668705734016375974);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_4915148707996812550) {
|
||||
inv_err_fun(nom_x, true_x, out_4915148707996812550);
|
||||
}
|
||||
void gnss_H_mod_fun(double *state, double *out_1646191520139698591) {
|
||||
H_mod_fun(state, out_1646191520139698591);
|
||||
void gnss_H_mod_fun(double *state, double *out_8574237875034322764) {
|
||||
H_mod_fun(state, out_8574237875034322764);
|
||||
}
|
||||
void gnss_f_fun(double *state, double dt, double *out_8136264243716214718) {
|
||||
f_fun(state, dt, out_8136264243716214718);
|
||||
void gnss_f_fun(double *state, double dt, double *out_5636831905979297775) {
|
||||
f_fun(state, dt, out_5636831905979297775);
|
||||
}
|
||||
void gnss_F_fun(double *state, double dt, double *out_4236213521667949352) {
|
||||
F_fun(state, dt, out_4236213521667949352);
|
||||
void gnss_F_fun(double *state, double dt, double *out_2727942986966876467) {
|
||||
F_fun(state, dt, out_2727942986966876467);
|
||||
}
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8352111296884602789) {
|
||||
h_6(state, sat_pos, out_8352111296884602789);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_4554215930920042575) {
|
||||
h_6(state, sat_pos, out_4554215930920042575);
|
||||
}
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_2202359500111385714) {
|
||||
H_6(state, sat_pos, out_2202359500111385714);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_895428558576169950) {
|
||||
H_6(state, sat_pos, out_895428558576169950);
|
||||
}
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_4887672178428492502) {
|
||||
h_20(state, sat_pos, out_4887672178428492502);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_6704926246356191984) {
|
||||
h_20(state, sat_pos, out_6704926246356191984);
|
||||
}
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_4961949425089895635) {
|
||||
H_20(state, sat_pos, out_4961949425089895635);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_3575717658219428128) {
|
||||
H_20(state, sat_pos, out_3575717658219428128);
|
||||
}
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_191603051630805475) {
|
||||
h_7(state, sat_pos_vel, out_191603051630805475);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_6430340606903371855) {
|
||||
h_7(state, sat_pos_vel, out_6430340606903371855);
|
||||
}
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7197851591855342328) {
|
||||
H_7(state, sat_pos_vel, out_7197851591855342328);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_4789129994195662485) {
|
||||
H_7(state, sat_pos_vel, out_4789129994195662485);
|
||||
}
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_191603051630805475) {
|
||||
h_21(state, sat_pos_vel, out_191603051630805475);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_6430340606903371855) {
|
||||
h_21(state, sat_pos_vel, out_6430340606903371855);
|
||||
}
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7197851591855342328) {
|
||||
H_21(state, sat_pos_vel, out_7197851591855342328);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_4789129994195662485) {
|
||||
H_21(state, sat_pos_vel, out_4789129994195662485);
|
||||
}
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_3985733553669045281);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_668705734016375974);
|
||||
void gnss_H_mod_fun(double *state, double *out_1646191520139698591);
|
||||
void gnss_f_fun(double *state, double dt, double *out_8136264243716214718);
|
||||
void gnss_F_fun(double *state, double dt, double *out_4236213521667949352);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8352111296884602789);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_2202359500111385714);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_4887672178428492502);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_4961949425089895635);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_191603051630805475);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_7197851591855342328);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_191603051630805475);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_7197851591855342328);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_4640013062227700412);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_4915148707996812550);
|
||||
void gnss_H_mod_fun(double *state, double *out_8574237875034322764);
|
||||
void gnss_f_fun(double *state, double dt, double *out_5636831905979297775);
|
||||
void gnss_F_fun(double *state, double dt, double *out_2727942986966876467);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_4554215930920042575);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_895428558576169950);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_6704926246356191984);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_3575717658219428128);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_6430340606903371855);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_4789129994195662485);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_6430340606903371855);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_4789129994195662485);
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -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_4368495907718343571);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_4465536791498929799);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_4242153094211941637);
|
||||
void live_H_mod_fun(double *state, double *out_8694534732588551911);
|
||||
void live_f_fun(double *state, double dt, double *out_331980123976991440);
|
||||
void live_F_fun(double *state, double dt, double *out_7523521905118560137);
|
||||
void live_h_4(double *state, double *unused, double *out_874158167075593511);
|
||||
void live_H_4(double *state, double *unused, double *out_2890470532950944797);
|
||||
void live_h_9(double *state, double *unused, double *out_7169843762143865122);
|
||||
void live_H_9(double *state, double *unused, double *out_5779332085231024139);
|
||||
void live_h_10(double *state, double *unused, double *out_3163124755553653720);
|
||||
void live_H_10(double *state, double *unused, double *out_4896893583908803739);
|
||||
void live_h_12(double *state, double *unused, double *out_6187507614461106032);
|
||||
void live_H_12(double *state, double *unused, double *out_7889145227076156327);
|
||||
void live_h_35(double *state, double *unused, double *out_4266066339955691863);
|
||||
void live_H_35(double *state, double *unused, double *out_5143582194751142618);
|
||||
void live_h_32(double *state, double *unused, double *out_9055313354289894635);
|
||||
void live_H_32(double *state, double *unused, double *out_6388297375264292642);
|
||||
void live_h_13(double *state, double *unused, double *out_7078564321862276630);
|
||||
void live_H_13(double *state, double *unused, double *out_7646665591930494052);
|
||||
void live_h_14(double *state, double *unused, double *out_7169843762143865122);
|
||||
void live_H_14(double *state, double *unused, double *out_5779332085231024139);
|
||||
void live_h_33(double *state, double *unused, double *out_3878198370963190653);
|
||||
void live_H_33(double *state, double *unused, double *out_1993025190112285014);
|
||||
void live_H(double *in_vec, double *out_6240008686817713408);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_3690665086918622964);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_8615868703535681088);
|
||||
void live_H_mod_fun(double *state, double *out_1988291488770769521);
|
||||
void live_f_fun(double *state, double dt, double *out_3263988285527381505);
|
||||
void live_F_fun(double *state, double dt, double *out_4332092747232242913);
|
||||
void live_h_4(double *state, double *unused, double *out_6582152946104087701);
|
||||
void live_H_4(double *state, double *unused, double *out_8426954437225368770);
|
||||
void live_h_9(double *state, double *unused, double *out_7444569565063482614);
|
||||
void live_H_9(double *state, double *unused, double *out_7130928084204103504);
|
||||
void live_h_10(double *state, double *unused, double *out_1939752611965379146);
|
||||
void live_H_10(double *state, double *unused, double *out_6385265893824183484);
|
||||
void live_h_12(double *state, double *unused, double *out_7296993372404775801);
|
||||
void live_H_12(double *state, double *unused, double *out_2352661322801732354);
|
||||
void live_h_35(double *state, double *unused, double *out_947171662528612088);
|
||||
void live_H_35(double *state, double *unused, double *out_392901709523281355);
|
||||
void live_h_32(double *state, double *unused, double *out_2929900686975731664);
|
||||
void live_H_32(double *state, double *unused, double *out_8907135071749521650);
|
||||
void live_h_13(double *state, double *unused, double *out_4581312559012542837);
|
||||
void live_H_13(double *state, double *unused, double *out_569470278268388809);
|
||||
void live_h_14(double *state, double *unused, double *out_7444569565063482614);
|
||||
void live_H_14(double *state, double *unused, double *out_7130928084204103504);
|
||||
void live_h_33(double *state, double *unused, double *out_7317084925230058116);
|
||||
void live_H_33(double *state, double *unused, double *out_3543458714162138959);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
@@ -55,7 +55,7 @@ def manager_init() -> None:
|
||||
params.put_bool("RecordFront", True)
|
||||
|
||||
if not params.get_bool("DisableRadar_Allow"):
|
||||
params.delete("DisableRadar")
|
||||
params.remove("DisableRadar")
|
||||
|
||||
# set unset params
|
||||
for k, v in default_params:
|
||||
@@ -137,7 +137,9 @@ def manager_thread() -> None:
|
||||
dp_otisserv = params.get_bool('dp_otisserv')
|
||||
ignore += ['dmonitoringmodeld', 'dmonitoringd'] if params.get_bool('dp_jetson') else []
|
||||
ignore += ['otisserv'] if not dp_otisserv else []
|
||||
ignore += ['gpxd'] if not dp_otisserv and not params.get_bool('dp_gpxd') else []
|
||||
dp_mapd = params.get_bool('dp_mapd')
|
||||
ignore += ['mapd'] if not dp_mapd else []
|
||||
ignore += ['gpxd'] if not dp_otisserv and not dp_mapd and not params.get_bool('dp_gpxd') else []
|
||||
ignore += ['uploader'] if not params.get_bool('dp_api_custom') and (int(params.get('dp_atl', encoding='utf8')) > 0 or params.get_bool('dp_jetson')) else []
|
||||
|
||||
if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID):
|
||||
|
||||
@@ -45,6 +45,7 @@ procs = [
|
||||
# PythonProcess("navd", "selfdrive.navd.navd"),
|
||||
PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
|
||||
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
|
||||
PythonProcess("pigeond", "selfdrive.sensord.pigeond", enabled=TICI),
|
||||
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
|
||||
PythonProcess("radard", "selfdrive.controls.radard"),
|
||||
PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
|
||||
|
||||
+38
-8
@@ -12,6 +12,7 @@ from selfdrive.mapd.config import QUERY_RADIUS, MIN_DISTANCE_FOR_NEW_QUERY, FULL
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
# dp
|
||||
import math
|
||||
from common.params import Params
|
||||
import json
|
||||
|
||||
@@ -86,17 +87,46 @@ class MapD():
|
||||
|
||||
log = sm[sock]
|
||||
self.last_gps = log
|
||||
skip_gps_query = False
|
||||
location_valid = False
|
||||
|
||||
# ignore the message if the fix is invalid
|
||||
if log.flags % 2 == 0:
|
||||
return
|
||||
skip_gps_query = True
|
||||
|
||||
self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
|
||||
self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
|
||||
self.location_deg = (log.latitude, log.longitude)
|
||||
self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
|
||||
self.gps_speed = log.speed
|
||||
self.location_stdev = log.accuracy # log accuracies are presumably 1 standard deviation.
|
||||
if not skip_gps_query:
|
||||
self.last_gps_fix_timestamp = log.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
|
||||
self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
|
||||
self.location_deg = (log.latitude, log.longitude)
|
||||
self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
|
||||
self.gps_speed = log.speed
|
||||
self.location_stdev = log.accuracy # log accuracies are presumably 1 standard deviation.
|
||||
|
||||
sock = 'liveLocationKalman'
|
||||
if sm.updated[sock] and sm.valid[sock]:
|
||||
location = sm[sock]
|
||||
location_valid = location.positionGeodetic.valid
|
||||
|
||||
if location_valid:
|
||||
_debug("Mapd: Using liveLocationKalman")
|
||||
lat = float(location.positionGeodetic.value[0])
|
||||
lon = float(location.positionGeodetic.value[1])
|
||||
bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
|
||||
self.last_gps_fix_timestamp = location.unixTimestampMillis # Unix TS. Milliseconds since January 1, 1970.
|
||||
self.location_rad = np.radians(np.array([lat, lon], dtype=float))
|
||||
self.location_deg = (lat, lon)
|
||||
self.bearing_rad = np.radians(bearing, dtype=float)
|
||||
self.gps_speed = sm['carState'].vEgo
|
||||
self.location_stdev = 1.0 if log.accuracy > 5 else log.accuracy
|
||||
|
||||
# self.last_gps.latitude = lat
|
||||
# self.last_gps.longitude = lon
|
||||
# self.last_gps.speed = self.gps_speed
|
||||
# self.last_gps.bearingDeg = bearing
|
||||
|
||||
if skip_gps_query and not location_valid:
|
||||
return
|
||||
|
||||
_debug('Mapd: ********* Got GPS fix'
|
||||
+ f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n'
|
||||
@@ -275,7 +305,7 @@ def mapd_thread(sm=None, pm=None):
|
||||
|
||||
# *** setup messaging
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'controlsState'])
|
||||
sm = messaging.SubMaster(['gpsLocationExternal', 'controlsState', 'liveLocationKalman', 'carState'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['liveMapData'])
|
||||
|
||||
|
||||
Binary file not shown.
@@ -253,8 +253,8 @@ constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE;
|
||||
|
||||
// TODO: convert remaining arrays to std::array and update model runners
|
||||
struct ModelState {
|
||||
ModelFrame *frame;
|
||||
ModelFrame *wide_frame;
|
||||
ModelFrame *frame = nullptr;
|
||||
ModelFrame *wide_frame = nullptr;
|
||||
std::array<float, NET_OUTPUT_SIZE> output = {};
|
||||
std::unique_ptr<RunModel> m;
|
||||
#ifdef DESIRE
|
||||
|
||||
Binary file not shown.
@@ -239,7 +239,7 @@ class RouteEngine:
|
||||
self.recompute_countdown = 0
|
||||
else:
|
||||
cloudlog.warning("Destination reached")
|
||||
Params().delete("NavDestination")
|
||||
Params().remove("NavDestination")
|
||||
|
||||
# Clear route if driving away from destination
|
||||
dist = self.nav_destination.distance_to(self.last_position)
|
||||
|
||||
Binary file not shown.
Executable
+255
@@ -0,0 +1,255 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import time
|
||||
import signal
|
||||
import serial
|
||||
import struct
|
||||
import requests
|
||||
import urllib.parse
|
||||
from datetime import datetime
|
||||
|
||||
from cereal import messaging
|
||||
from common.params import Params
|
||||
from system.swaglog import cloudlog
|
||||
from selfdrive.hardware import TICI
|
||||
from common.gpio import gpio_init, gpio_set
|
||||
from selfdrive.hardware.tici.pins import GPIO
|
||||
|
||||
UBLOX_TTY = "/dev/ttyHS0"
|
||||
|
||||
UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
|
||||
UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
|
||||
UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
|
||||
UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
|
||||
UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
|
||||
UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
|
||||
|
||||
def set_power(enabled):
|
||||
gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_init(GPIO.UBLOX_PWR_EN, True)
|
||||
gpio_init(GPIO.UBLOX_RST_N, True)
|
||||
|
||||
gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, enabled)
|
||||
gpio_set(GPIO.UBLOX_RST_N, enabled)
|
||||
|
||||
|
||||
def add_ubx_checksum(msg):
|
||||
A = B = 0
|
||||
for b in msg[2:]:
|
||||
A = (A + b) % 256
|
||||
B = (B + A) % 256
|
||||
return msg + bytes([A, B])
|
||||
|
||||
def get_assistnow_messages(token):
|
||||
# make request
|
||||
# TODO: implement adding the last known location
|
||||
r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
|
||||
'token': token,
|
||||
'gnss': 'gps,glo',
|
||||
'datatype': 'eph,alm,aux',
|
||||
}, safe=':,'), timeout=5)
|
||||
assert r.status_code == 200, "Got invalid status code"
|
||||
dat = r.content
|
||||
|
||||
# split up messages
|
||||
msgs = []
|
||||
while len(dat) > 0:
|
||||
assert dat[:2] == b"\xB5\x62"
|
||||
msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
|
||||
msgs.append(dat[:msg_len])
|
||||
dat = dat[msg_len:]
|
||||
return msgs
|
||||
|
||||
|
||||
class TTYPigeon():
|
||||
def __init__(self, path):
|
||||
self.path = path
|
||||
self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
|
||||
|
||||
def send(self, dat):
|
||||
self.tty.write(dat)
|
||||
|
||||
def receive(self):
|
||||
dat = b''
|
||||
while len(dat) < 0x1000:
|
||||
d = self.tty.read(0x40)
|
||||
dat += d
|
||||
if len(d) == 0:
|
||||
break
|
||||
return dat
|
||||
|
||||
def set_baud(self, baud):
|
||||
self.tty.baudrate = baud
|
||||
|
||||
def wait_for_ack(self, ack=UBLOX_ACK, nack=UBLOX_NACK, timeout=0.5):
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
if ack in dat:
|
||||
cloudlog.debug("Received ACK from ublox")
|
||||
return True
|
||||
elif nack in dat:
|
||||
cloudlog.error("Received NACK from ublox")
|
||||
return False
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
def send_with_ack(self, dat, ack=UBLOX_ACK, nack=UBLOX_NACK):
|
||||
self.send(dat)
|
||||
self.wait_for_ack(ack, nack)
|
||||
|
||||
def wait_for_backup_restore_status(self, timeout=1):
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
|
||||
if position >= 0 and len(dat) >= position + 11:
|
||||
return dat[position + 10]
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No backup restore response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
|
||||
def initialize_pigeon(pigeon):
|
||||
# try initializing a few times
|
||||
for _ in range(10):
|
||||
try:
|
||||
pigeon.set_baud(9600)
|
||||
|
||||
# up baud rate
|
||||
pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A")
|
||||
time.sleep(0.1)
|
||||
pigeon.set_baud(460800)
|
||||
|
||||
# other configuration messages
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x00\x00\x06\x18")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x01\x08\x22")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x00\x01\x00\x03\x0A\x24")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
|
||||
cloudlog.debug("pigeon configured")
|
||||
|
||||
# try restoring almanac backup
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
|
||||
restore_status = pigeon.wait_for_backup_restore_status()
|
||||
if restore_status == 2:
|
||||
cloudlog.warning("almanac backup restored")
|
||||
elif restore_status == 3:
|
||||
cloudlog.warning("no almanac backup found")
|
||||
else:
|
||||
cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
|
||||
|
||||
# sending time to ublox
|
||||
t_now = datetime.utcnow()
|
||||
if t_now >= datetime(2021, 6, 1):
|
||||
cloudlog.warning("Sending current time to ublox")
|
||||
|
||||
# UBX-MGA-INI-TIME_UTC
|
||||
msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
|
||||
0x10,
|
||||
0x00,
|
||||
0x00,
|
||||
0x80,
|
||||
t_now.year,
|
||||
t_now.month,
|
||||
t_now.day,
|
||||
t_now.hour,
|
||||
t_now.minute,
|
||||
t_now.second,
|
||||
0,
|
||||
30,
|
||||
0
|
||||
))
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
|
||||
# try getting AssistNow if we have a token
|
||||
token = Params().get('AssistNowToken')
|
||||
if token is not None:
|
||||
try:
|
||||
for msg in get_assistnow_messages(token):
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
cloudlog.warning("AssistNow messages sent")
|
||||
except Exception:
|
||||
cloudlog.warning("failed to get AssistNow messages")
|
||||
|
||||
cloudlog.warning("Pigeon GPS on!")
|
||||
break
|
||||
except TimeoutError:
|
||||
cloudlog.warning("Initialization failed, trying again!")
|
||||
|
||||
def deinitialize_and_exit(pigeon):
|
||||
cloudlog.warning("Storing almanac in ublox flash")
|
||||
|
||||
# controlled GNSS stop
|
||||
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
|
||||
|
||||
# store almanac in flash
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
|
||||
try:
|
||||
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
|
||||
cloudlog.warning("Done storing almanac")
|
||||
else:
|
||||
cloudlog.error("Error storing almanac")
|
||||
except TimeoutError:
|
||||
pass
|
||||
|
||||
# turn off power and exit cleanly
|
||||
set_power(False)
|
||||
sys.exit(0)
|
||||
|
||||
def main():
|
||||
assert TICI, "unsupported hardware for pigeond"
|
||||
|
||||
pm = messaging.PubMaster(['ubloxRaw'])
|
||||
|
||||
# power cycle ublox
|
||||
set_power(False)
|
||||
time.sleep(0.1)
|
||||
set_power(True)
|
||||
time.sleep(0.5)
|
||||
|
||||
pigeon = TTYPigeon(UBLOX_TTY)
|
||||
initialize_pigeon(pigeon)
|
||||
|
||||
# register exit handler
|
||||
signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
|
||||
|
||||
# start receiving data
|
||||
while True:
|
||||
dat = pigeon.receive()
|
||||
if len(dat) > 0:
|
||||
if dat[0] == 0x00:
|
||||
cloudlog.warning("received invalid data from ublox, re-initing!")
|
||||
initialize_pigeon(pigeon)
|
||||
continue
|
||||
|
||||
# send out to socket
|
||||
msg = messaging.new_message('ubloxRaw', len(dat))
|
||||
msg.ubloxRaw = dat[:]
|
||||
pm.send('ubloxRaw', msg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,6 +1,6 @@
|
||||
{
|
||||
"English": "main_en",
|
||||
"Português": "main_pt",
|
||||
"Português": "main_pt-BR",
|
||||
"中文（繁體）": "main_zh-CHT",
|
||||
"中文（简体）": "main_zh-CHS",
|
||||
"한국어": "main_ko",
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -118,7 +118,7 @@ def set_params(new_version: bool, failed_count: int, exception: Optional[str]) -
|
||||
pass
|
||||
|
||||
if exception is None:
|
||||
params.delete("LastUpdateException")
|
||||
params.remove("LastUpdateException")
|
||||
else:
|
||||
params.put("LastUpdateException", exception)
|
||||
|
||||
|
||||
Binary file not shown.
@@ -87,7 +87,7 @@ def snapshot():
|
||||
subprocess.check_call(["pgrep", "camerad"])
|
||||
print("Camerad already running")
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
params.delete("Offroad_IsTakingSnapshot")
|
||||
params.remove("Offroad_IsTakingSnapshot")
|
||||
return None, None
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user