diff --git a/cereal/libcereal.a b/cereal/libcereal.a index c300000da..791c5c19e 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/common/libcommon.a b/common/libcommon.a index 3a2e9e39d..831c50d68 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 831238f6c..d3cd47dcb 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -270,6 +270,20 @@ inline static std::unordered_map keys = { {"LongPitch", {PERSISTENT, BOOL, "1", "0", 2}}, {"RemoteStartBootsComma", {PERSISTENT, BOOL, "0", "0"}}, {"RemapCancelToDistance", {PERSISTENT, BOOL, "0", "0"}}, + {"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1", "1"}}, + {"NAPFollowDistance", {PERSISTENT, INT, "4", "4"}}, + {"NAPForcePreAP", {PERSISTENT, BOOL, "0", "0"}}, + {"NAPPedalEnabled", {PERSISTENT, BOOL, "0", "0"}}, + {"NAPPedalCanBus", {PERSISTENT, INT, "2", "2"}}, + {"NAPPedalCalibDone", {PERSISTENT, BOOL, "0", "0"}}, + {"NAPPedalCalibMin", {PERSISTENT, FLOAT, "-3.0", "-3.0"}}, + {"NAPPedalCalibMax", {PERSISTENT, FLOAT, "99.6", "99.6"}}, + {"NAPPedalCalibFactor", {PERSISTENT, FLOAT, "1.0", "1.0"}}, + {"NAPPedalCalibZero", {PERSISTENT, FLOAT, "0.0", "0.0"}}, + {"NAPPedalProfile", {PERSISTENT, INT, "4", "4"}}, + {"NAPRadarBehindNosecone", {PERSISTENT, BOOL, "0", "0"}}, + {"NAPRadarEnabled", {PERSISTENT, BOOL, "1", "1"}}, + {"NAPRadarOffset", {PERSISTENT, FLOAT, "0.0", "0.0"}}, {"ForceAutoTune", {PERSISTENT, BOOL, "0", "0", 3}}, {"ForceAutoTuneOff", {PERSISTENT, BOOL, "1", "0", 2}}, {"ForceFingerprint", {PERSISTENT, BOOL, "0", "0", 2}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index d757271b0..8c88b2fe7 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/car.capnp b/opendbc_repo/opendbc/car/car.capnp index d2b8d758a..9baf6d785 100644 --- a/opendbc_repo/opendbc/car/car.capnp +++ b/opendbc_repo/opendbc/car/car.capnp @@ -635,6 +635,7 @@ struct CarParams { fcaGiorgio @32; rivian @33; volkswagenMeb @34; + teslaPreap @35; } enum SteerControlType { diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index 5732e3209..dffc5e75b 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -211,6 +211,11 @@ def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_mu disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False) ecu_rx_addrs = set() + if not fixed_fingerprint and Params().get_bool("NAPForcePreAP"): + fixed_fingerprint = "TESLA_MODEL_S_PREAP" + skip_fw_query = True + carlog.warning("NAPForcePreAP enabled; forcing TESLA_MODEL_S_PREAP fingerprint") + start_time = time.monotonic() if not skip_fw_query: if cached_params is not None and cached_params.brand != "mock" and len(cached_params.carFw) > 0 and \ diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 273e4d5d7..38817132d 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -247,6 +247,9 @@ class CarInterfaceBase(ABC): if 0x23 in fingerprint[0]: fp_ret.flags |= ToyotaStarPilotFlags.ZSS.value + elif platform.config.platform_str == "TESLA_MODEL_S_PREAP": + fp_ret.canUsePedal = True + return fp_ret @staticmethod diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index bcf801c1d..ad8b1bd07 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/opendbc_repo/opendbc/car/tesla/carcontroller.py @@ -4,15 +4,15 @@ from opendbc.car import Bus from opendbc.car.lateral import apply_steer_angle_limits_vm from opendbc.car.interfaces import CarControllerBase from opendbc.car.tesla.teslacan import TeslaCAN -from opendbc.car.tesla.values import CarControllerParams +from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can +from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer +from opendbc.car.tesla.values import CAR, CarControllerParams from opendbc.car.vehicle_model import VehicleModel def get_safety_CP(): - # We use the TESLA_MODEL_Y platform for lateral limiting to match safety - # A Model 3 at 40 m/s using the Model Y limits sees a <0.3% difference in max angle (from curvature factor) from opendbc.car.tesla.interface import CarInterface - return CarInterface.get_non_essential_params("TESLA_MODEL_Y") + return CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP if getattr(get_safety_CP, "_preap", False) else CAR.TESLA_MODEL_Y) class CarController(CarControllerBase): @@ -21,11 +21,23 @@ class CarController(CarControllerBase): self.apply_angle_last = 0 self.packer = CANPacker(dbc_names[Bus.party]) self.tesla_can = TeslaCAN(self.packer) + self.preap_long = None + self.stock_cc = None + + if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + get_safety_CP._preap = True + self.tesla_can = init_preap_can(dbc_names) + self.preap_long = PreAPLongController() + self.stock_cc = StockCCSpoofer() # Vehicle model used for lateral limiting self.VM = VehicleModel(get_safety_CP()) + get_safety_CP._preap = False def update(self, CC, CS, now_nanos, starpilot_toggles): + if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + return self._update_preap(CC, CS) + actuators = CC.actuators can_sends = [] @@ -64,3 +76,42 @@ class CarController(CarControllerBase): self.frame += 1 return new_actuators, can_sends + + def _update_preap(self, CC, CS): + actuators = CC.actuators + can_sends = [] + lat_active = CC.latActive and CS.hands_on_level < 3 + + if CC.cruiseControl.cancel and CS.cruiseEnabled: + CS.cruiseEnabled = False + CS.enableLongControl = False + CS.enableJustCC = False + CS.pedal_speed_kph = 0.0 + CS.preap_cc_cancel_needed = True + if hasattr(CS, "engagement"): + CS.engagement.cruiseEnabled = False + CS.engagement.enableLongControl = False + CS.engagement.enableJustCC = False + CS.engagement.pending_enable = False + CS.engagement.pedal_speed_kph = 0.0 + + if self.frame % 2 == 0: + self.apply_angle_last = apply_steer_angle_limits_vm( + actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg, + lat_active, CarControllerParams, self.VM, + ) + cntr = (self.frame // 2) % 16 + can_sends.append(self.tesla_can.create_steering_control(cntr, self.apply_angle_last, lat_active)) + can_sends.append(self.tesla_can.create_epas_control(cntr, 1)) + + if self.CP.openpilotLongitudinalControl and self.preap_long is not None: + can_sends.extend(self.preap_long.update(CC, CS, self.frame, self.tesla_can, CANBUS.party)) + + if self.stock_cc is not None: + can_sends.extend(self.stock_cc.update(CS, self.frame, self.tesla_can, CANBUS.party)) + + new_actuators = actuators.as_builder() + new_actuators.steeringAngleDeg = self.apply_angle_last + + self.frame += 1 + return new_actuators, can_sends diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 550edbe75..cfcd5d6c6 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -5,6 +5,10 @@ from opendbc.car import Bus, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD, CAR +from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap +from opendbc.car.tesla.preap.engagement import PreAPEngagement +from opendbc.car.tesla.preap.nap_conf import nap_conf +from opendbc.car.tesla.preap.pedal_feedback import PedalFeedback ButtonType = structs.CarState.ButtonEvent.Type @@ -13,7 +17,8 @@ class CarState(CarStateBase): def __init__(self, CP, FPCP): super().__init__(CP, FPCP) self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party]) - self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] + self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"] if CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP else \ + self.can_define.dv["DI_torque2"]["DI_gear"] self.autopark = False self.autopark_prev = False @@ -21,6 +26,23 @@ class CarState(CarStateBase): self.hands_on_level = 0 self.das_control = None + self.cruise_buttons = 0 + self.prev_cruise_buttons = 0 + self.msg_stw_actn_req = None + self.speed_units = "MPH" + + if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + self.engagement = PreAPEngagement(nap_conf.double_pull_enabled, nap_conf.double_pull_window_ms) + self.cruiseEnabled = False + self.enableLongControl = False + self.enableJustCC = False + self.pedal_speed_kph = 0.0 + self.preap_cc_cancel_needed = False + self.preap_cc_engage_needed = False + self.di_cruise_state = "OFF" + self.pedal = PedalFeedback() + self.pedal_interceptor_value = 0.0 + self.pedal_timeout = True def update_autopark_state(self, autopark_state: str, cruise_enabled: bool): autopark_now = autopark_state in ("ACTIVE", "COMPLETE", "SELFPARK_STARTED") @@ -32,6 +54,9 @@ class CarState(CarStateBase): self.cruise_enabled_prev = cruise_enabled def update(self, can_parsers, starpilot_toggles) -> structs.CarState: + if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + return update_preap(self, can_parsers) + cp_party = can_parsers[Bus.party] cp_ap_party = can_parsers[Bus.ap_party] ret = structs.CarState() @@ -124,6 +149,8 @@ class CarState(CarStateBase): @staticmethod def get_can_parsers(CP): + if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + return get_preap_can_parsers(CP) return { Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party), Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party) diff --git a/opendbc_repo/opendbc/car/tesla/fingerprints.py b/opendbc_repo/opendbc/car/tesla/fingerprints.py index 54705385a..5892c7519 100644 --- a/opendbc_repo/opendbc/car/tesla/fingerprints.py +++ b/opendbc_repo/opendbc/car/tesla/fingerprints.py @@ -45,3 +45,22 @@ FW_VERSIONS = { ], }, } + +FINGERPRINTS = { + CAR.TESLA_MODEL_S_PREAP: [ + { + 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 277: 6, 280: 6, 293: 4, 296: 4, + 309: 5, 325: 8, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, + 522: 8, 524: 8, 527: 8, 536: 8, 551: 4, 552: 2, 556: 8, 568: 8, 582: 5, 638: 8, 643: 8, + 693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8, + 780: 2, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 799: 8, 804: 8, 805: 8, + 807: 8, 808: 1, 812: 8, 815: 8, 820: 8, 823: 8, 824: 8, 831: 8, 836: 8, 840: 8, 856: 4, + 863: 8, 872: 8, 880: 8, 888: 8, 896: 8, 901: 6, 904: 3, 920: 8, 936: 8, 949: 8, 952: 8, + 953: 6, 968: 8, 984: 8, 1000: 8, 1006: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, + 1034: 8, 1048: 1, 1064: 8, 1080: 8, 1281: 8, 1285: 8, 1332: 8, 1335: 8, 1362: 6, 1368: 8, + 1412: 8, 1436: 8, 1456: 8, 1463: 8, 1476: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1617: 8, + 1621: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, + 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, + }, + ], +} diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py index baf159323..c284a08d4 100644 --- a/opendbc_repo/opendbc/car/tesla/interface.py +++ b/opendbc_repo/opendbc/car/tesla/interface.py @@ -2,17 +2,29 @@ from opendbc.car import get_safety_config, structs from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.carstate import CarState +from opendbc.car.tesla.radar_interface import RadarInterface from opendbc.car.tesla.values import TeslaSafetyFlags, CAR +from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params class CarInterface(CarInterfaceBase): CarState = CarState CarController = CarController + RadarInterface = RadarInterface + + @staticmethod + def get_pid_accel_limits(CP, current_speed, cruise_speed): + if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: + return get_preap_accel_limits(current_speed) + return CarInterfaceBase.get_pid_accel_limits(CP, current_speed, cruise_speed) @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "tesla" + if candidate == CAR.TESLA_MODEL_S_PREAP: + return get_preap_params(ret) + ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)] ret.steerLimitTimer = 0.4 diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py index 6fedfaac0..eb994fa5b 100644 --- a/opendbc_repo/opendbc/car/tesla/values.py +++ b/opendbc_repo/opendbc/car/tesla/values.py @@ -60,6 +60,16 @@ class CAR(Platforms): [TeslaCarDocsHW4("Tesla Model X (with HW4) 2024")], CarSpecs(mass=2495., wheelbase=2.960, steerRatio=12.0), ) + TESLA_MODEL_S_PREAP = TeslaPlatformConfig( + [CarDocs("Tesla Model S (Pre-AP) 2012-14", "All")], + CarSpecs(mass=2100., wheelbase=2.960, steerRatio=15.0), + { + Bus.party: 'tesla_can', + Bus.pt: 'tesla_can', + Bus.chassis: 'tesla_can', + Bus.radar: 'tesla_radar_bosch_generated', + }, + ) FW_QUERY_CONFIG = FwQueryConfig( @@ -75,7 +85,8 @@ FW_QUERY_CONFIG = FwQueryConfig( class CANBUS: party = 0 - vehicle = 1 + radar = 1 + vehicle = radar autopilot_party = 2 @@ -125,6 +136,25 @@ class TeslaFlags(IntFlag): LONG_CONTROL = 1 +class CruiseButtons: + IDLE = 0 + CANCEL = 1 + MAIN = 2 + RES_ACCEL_2ND = 4 + DECEL_2ND = 8 + SET_ACCEL = 16 + RES_ACCEL = 16 + DECEL_SET = 32 + + @classmethod + def is_accel(cls, btn: int) -> bool: + return btn in (cls.RES_ACCEL, cls.RES_ACCEL_2ND) + + @classmethod + def is_decel(cls, btn: int) -> bool: + return btn in (cls.DECEL_SET, cls.DECEL_2ND) + + DBC = CAR.create_dbc_map() STEER_THRESHOLD = 1 diff --git a/opendbc_repo/opendbc/car/torque_data/substitute.toml b/opendbc_repo/opendbc/car/torque_data/substitute.toml index 389bb07ca..e098037c8 100644 --- a/opendbc_repo/opendbc/car/torque_data/substitute.toml +++ b/opendbc_repo/opendbc/car/torque_data/substitute.toml @@ -89,6 +89,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] "SUBARU_FORESTER_HYBRID" = "SUBARU_IMPREZA_2020" "SUBARU_LEGACY" = "SUBARU_OUTBACK" +"TESLA_MODEL_S_PREAP" = "TESLA_MODEL_3" + # Old subarus don't have much data guessing it's like low torque impreza" "SUBARU_OUTBACK_PREGLOBAL_2018" = "SUBARU_IMPREZA" "SUBARU_OUTBACK_PREGLOBAL" = "SUBARU_IMPREZA" diff --git a/opendbc_repo/opendbc/dbc/tesla_can.dbc b/opendbc_repo/opendbc/dbc/tesla_can.dbc index 56624c3e5..dc394f36a 100644 --- a/opendbc_repo/opendbc/dbc/tesla_can.dbc +++ b/opendbc_repo/opendbc/dbc/tesla_can.dbc @@ -725,6 +725,15 @@ BO_ 780 DriverSeat: 8 XXX SG_ occupancyStatus : 16|3@1+ (1,0) [0|7] "" XXX SG_ buckleStatus : 19|2@1+ (1,0) [0|3] "" XXX +BO_ 1362 GAS_SENSOR: 6 INTERCEPTOR + SG_ INTERCEPTOR_GAS : 7|16@0+ (0.0507968128,-22.85856576) [0|1] "" EON + SG_ INTERCEPTOR_GAS2 : 23|16@0+ (0.1015936256,-22.85856576) [0|1] "" EON + SG_ STATE : 35|4@0+ (1,0) [0|15] "" EON + SG_ IDX : 39|4@0+ (1,0) [0|15] "" EON + SG_ CHECKSUM : 47|8@0+ (1,0) [0|255] "" EON + +VAL_ 1362 STATE 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + VAL_ 3 StW_Angl 16383 "SNA" ; VAL_ 3 StW_AnglSens_Id 2 "MUST" 0 "PSBL" 1 "SELF" ; VAL_ 3 StW_AnglSens_Stat 2 "ERR" 3 "ERR_INI" 1 "INI" 0 "OK" ; @@ -898,4 +907,3 @@ VAL_ 1160 DAS_steeringAngleRequest 16384 "ZERO_ANGLE" ; VAL_ 1160 DAS_steeringControlType 1 "ANGLE_CONTROL" 3 "DISABLED" 0 "NONE" 2 "RESERVED" ; VAL_ 1160 DAS_steeringHapticRequest 1 "ACTIVE" 0 "IDLE" ; - diff --git a/opendbc_repo/opendbc/safety/declarations.h b/opendbc_repo/opendbc/safety/declarations.h index 23798d7f0..1b6d436d2 100644 --- a/opendbc_repo/opendbc/safety/declarations.h +++ b/opendbc_repo/opendbc/safety/declarations.h @@ -33,6 +33,7 @@ #define SAFETY_PSA 31U #define SAFETY_RIVIAN 33U #define SAFETY_VOLKSWAGEN_MEB 34U +#define SAFETY_TESLA_PREAP 35U #define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)) #define GET_FLAG(value, mask) (((value) & (mask)) == (mask)) @@ -215,6 +216,7 @@ typedef bool (*fwd_hook)(int bus_num, int addr); // returns true if the mes typedef struct { safety_hook_init init; rx_hook rx; + rx_hook rx_all; tx_hook tx; fwd_hook fwd; get_checksum_t get_checksum; @@ -355,3 +357,4 @@ extern const safety_hooks volkswagen_mqb_hooks; extern const safety_hooks volkswagen_pq_hooks; extern const safety_hooks rivian_hooks; extern const safety_hooks psa_hooks; +extern const safety_hooks tesla_preap_hooks; diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 7ef230dfc..cd948e7dc 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/opendbc_repo/opendbc/safety/safety.h @@ -11,6 +11,7 @@ #include "opendbc/safety/modes/honda.h" #include "opendbc/safety/modes/toyota.h" #include "opendbc/safety/modes/tesla.h" +#include "opendbc/safety/modes/tesla_preap.h" #include "opendbc/safety/modes/gm.h" #include "opendbc/safety/modes/ford.h" #include "opendbc/safety/modes/hyundai.h" @@ -198,6 +199,10 @@ static bool rx_msg_safety_check(const CANPacket_t *msg, bool safety_rx_hook(const CANPacket_t *msg) { bool controls_allowed_prev = controls_allowed; + if (current_hooks->rx_all != NULL) { + current_hooks->rx_all(msg); + } + bool valid = rx_msg_safety_check(msg, ¤t_safety_config, current_hooks); bool whitelisted = get_addr_check_index(msg, current_safety_config.rx_checks, current_safety_config.rx_checks_len) != -1; bool gm_rx_passthrough = current_safety_mode == SAFETY_GM; @@ -417,6 +422,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { {SAFETY_FORD, &ford_hooks}, {SAFETY_RIVIAN, &rivian_hooks}, {SAFETY_TESLA, &tesla_hooks}, + {SAFETY_TESLA_PREAP, &tesla_preap_hooks}, #ifdef CANFD {SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks}, #endif diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla.py b/opendbc_repo/opendbc/safety/tests/test_tesla.py index bf03e50b9..c367a7ece 100755 --- a/opendbc_repo/opendbc/safety/tests/test_tesla.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla.py @@ -39,9 +39,9 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, # Tesla uses get_max_angle_delta_vm and get_max_angle_vm for real lateral accel and jerk limits # TODO: integrate this into AngleSteeringSafetyTest - ANGLE_RATE_BP = None - ANGLE_RATE_UP = None - ANGLE_RATE_DOWN = None + ANGLE_RATE_BP = [0.] + ANGLE_RATE_UP = [CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE] + ANGLE_RATE_DOWN = [CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE] # Real time limits LATERAL_FREQUENCY = 50 # Hz @@ -69,7 +69,9 @@ class TestTeslaSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest, self.steer_control_types = {d: v for v, d in self.define.dv["DAS_steeringControl"]["DAS_steeringControlType"].items()} - def _angle_cmd_msg(self, angle: float, state: bool | int, increment_timer: bool = True, bus: int = 0): + def _angle_cmd_msg(self, angle: float, state: bool | int = True, increment_timer: bool = True, bus: int = 0, enabled: bool | None = None): + if enabled is not None: + state = enabled values = {"DAS_steeringAngleRequest": angle, "DAS_steeringControlType": state} if increment_timer: self.safety.set_timer(self.cnt_angle_cmd * int(1e6 / self.LATERAL_FREQUENCY)) diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index ebe27109a..6faf6848d 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 331c89f75..5fe6d827d 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 9fc2b44b2..a87ec6f60 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 7a25a811d..b56c7b6ed 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 747530168..aa5a86d87 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 27644cd2a..66c355069 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index bcbf1ff4f..c07f9733b 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index bcbf1ff4f..c07f9733b 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index cb797ed3b..32f7a538b 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index 27644cd2a..66c355069 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index ddf591e5c..61184ab24 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-95fa880b-DEBUG"; +const uint8_t gitversion[19] = "DEV-7e481891-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index aef37ac6a..1938f9a00 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index d60395c98..05ffdb599 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 146a899c8..cbb3f0b2b 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index b9f75a2b5..2b57970ff 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index a0c8f2a1b..43d828038 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 7eae329e6..1ac003ea8 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index c27906d7b..90950f694 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index da484ac5f..74a0bb5a5 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 468b2460c..4c4af1a5e 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 8d5b34b44..866a27c1f 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index 0ff5af1a9..0a487e103 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 84854f38c..c519d4138 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 413e72145..6d6146f0c 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 84e8efd13..47b836018 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 6f9c777bb..7d5e817fe 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index 2b193323c..181229324 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index e617b2859..7a430cb6b 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 3a634b9bd..9519987bc 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index bf3e5b538..d9225d9fb 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 1312cfecc..30926ea6e 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index efe32d762..519305feb 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-95fa880b-DEBUG \ No newline at end of file +DEV-7e481891-DEBUG \ No newline at end of file diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index bb0ed3317..130235b89 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -184,6 +184,9 @@ class CarSpecificEvents: def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True, allow_button_cancel=True, suppress_low_speed_alert=False): events = Events() + preap_software_cruise = (self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and + self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise) + pcm_enable = pcm_enable or preap_software_cruise if CS.doorOpen: events.add(EventName.doorOpen) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index dea1e1120..edd5e8eda 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -220,13 +220,23 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - self.v_cruise_helper.update_v_cruise( - CS, - self.sm['carControl'].enabled, - self.is_metric, - self.sm['starpilotPlan'].speedLimitChanged, - self.starpilot_toggles, + preap_software_cruise = ( + self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and + self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise ) + if not preap_software_cruise: + self.v_cruise_helper.update_v_cruise( + CS, + self.sm['carControl'].enabled, + self.is_metric, + self.sm['starpilotPlan'].speedLimitChanged, + self.starpilot_toggles, + ) + else: + preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH) + self.v_cruise_helper.v_cruise_kph_last = self.v_cruise_helper.v_cruise_kph + self.v_cruise_helper.v_cruise_kph = preap_v_cruise_kph + self.v_cruise_helper.v_cruise_cluster_kph = preap_v_cruise_kph slc_force_speed = self.params_memory.get_float("SLCForceCruiseSpeed") if slc_force_speed > 0: if self.is_metric: @@ -237,7 +247,7 @@ class Car: self.v_cruise_helper.v_cruise_cluster_kph = self.v_cruise_helper.v_cruise_kph self.params_memory.remove("SLCForceCruiseSpeed") - if self.sm['carControl'].enabled and not self.CC_prev.enabled: + if self.sm['carControl'].enabled and not self.CC_prev.enabled and not preap_software_cruise: # Use CarState w/ buttons from the step selfdrived enables on desired_speed_limit = self.sm['starpilotPlan'].slcSpeedLimit + self.sm['starpilotPlan'].slcSpeedLimitOffset self.v_cruise_helper.initialize_v_cruise( diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 694e14bb7..e2d556df9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -64,6 +64,16 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.10 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.08 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.16 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.26 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.01 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.08 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.16 BOLT_2022_2023_CARS = ( GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, @@ -375,15 +385,30 @@ def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_la turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) low_speed_factor = _civic_bosch_modified_b_low_speed_factor(v_ego) + variant_active = civic_bosch_modified_lateral_testing_ground_active() + if variant_active: + base_reduction += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT) * onset * cutoff) turn_in_boost = 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT, CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT) * turn_in_weight * (0.40 + 0.60 * low_speed_factor)) + if variant_active: + turn_in_boost *= 1.0 + (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.40 + 0.60 * low_speed_factor)) unwind_taper = 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT, CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) + if variant_active: + unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0) @@ -398,16 +423,27 @@ def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_acce phase = _civic_bosch_modified_b_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) unwind_weight = max(-phase, 0.0) + variant_active = civic_bosch_modified_lateral_testing_ground_active() friction_scale = 1.0 friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT, CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT) * envelope * turn_in_weight) + if variant_active: + friction_scale += (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT) * + envelope * turn_in_weight) friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT, CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) * envelope * unwind_weight) + if variant_active: + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) return min(max(friction_scale, 0.82), 1.06) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9bc5135c0..51983dd95 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -108,6 +108,22 @@ UNCERT_MAG_TRIG = 0.50 _A_TOTAL_MAX_V = [3.5, 3.5, 3.2] _A_TOTAL_MAX_BP = [0., 20., 40.] +_preap_follow_cache = None + + +def get_preap_follow_limit(v_ego): + global _preap_follow_cache + if _preap_follow_cache is None: + try: + from opendbc.car.tesla.preap.constants import ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW + _preap_follow_cache = (ACCEL_PREAP_BP, ACCEL_PREAP_FOLLOW) + except ImportError: + _preap_follow_cache = (None, None) + bp, values = _preap_follow_cache + if bp is None: + return None + return float(np.interp(v_ego, bp, values)) + def get_longitudinal_personality(sm): return sm['selfdriveState'].personality @@ -220,6 +236,13 @@ class LongitudinalPlanner: self.model_allow_throttle = True self.allow_throttle = True self.mode = 'acc' + self.is_preap = ( + CP.brand == "tesla" and CP.carFingerprint == "TESLA_MODEL_S_PREAP" and + CP.openpilotLongitudinalControl and not CP.pcmCruise + ) + self.nap_adaptive_accel = False + self._preap_params = None + self._preap_param_frame = 0 self.generation = None @@ -251,6 +274,15 @@ class LongitudinalPlanner: self.effective_t_follow = None self.vision_low_speed_stop_hold_until = 0.0 + if self.is_preap: + try: + from openpilot.common.params import Params + self._preap_params = Params() + self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel") + except Exception: + self._preap_params = None + self.nap_adaptive_accel = False + @property def mlsim(self): return self.generation in ("v8", "v10", "v11", "v12", "v13") @@ -580,6 +612,11 @@ class LongitudinalPlanner: return d_rel < dynamic_distance and (ttc < RAW_LEAD_SAFETY_TTC or lead_braking) def update(self, sm, starpilot_toggles): + if self.is_preap: + self._preap_param_frame += 1 + if self._preap_params is not None and (self._preap_param_frame % 20) == 0: + self.nap_adaptive_accel = self._preap_params.get_bool("NAPAdaptiveAccel") + self.generation = getattr(starpilot_toggles, "model_version", None) self.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' self.mpc.mode = 'acc' @@ -661,6 +698,18 @@ class LongitudinalPlanner: lead_one_active = bool(self.lead_one.status and lead_control_active) effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego) + if self.is_preap and self.nap_adaptive_accel and lead_one_active: + follow_limit = get_preap_follow_limit(v_ego) + if follow_limit is not None: + safe_dist = get_safe_obstacle_distance(v_ego, effective_t_follow) + lead_dist_ratio = float(self.lead_one.dRel) / max(safe_dist, 1.0) + cap_strength = float(np.clip(1.0 - (lead_dist_ratio - 1.2) / 0.3, 0.0, 1.0)) + if cap_strength > 0.0: + accel_limits_turns[1] = min( + accel_limits_turns[1], + accel_limits_turns[1] * (1.0 - cap_strength) + follow_limit * cap_strength, + ) + lead_dist = self.lead_one.dRel if lead_one_active else 50.0 # Smooth lead distance (EMA) to avoid chatter in thresholds diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 258861cb9..81440988e 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -456,6 +456,24 @@ class TestLatControl: assert unwind_left < 1.0 assert unwind_right < unwind_left + def test_modified_civic_b_variant_extra_torque_shaping_curve(self, monkeypatch): + base_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0) + base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) + base_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + base_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) + + monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) + + variant_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0) + variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) + variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) + + assert variant_steady_right < base_steady_right + assert variant_turn_in_right >= base_turn_in_right + assert variant_unwind_right < base_unwind_right + assert variant_unwind_right_friction < base_unwind_right_friction + def test_kia_ev6_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6) monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 29cace37c..504b3a8ad 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_760253992355356602) { - out_760253992355356602[0] = delta_x[0] + nom_x[0]; - out_760253992355356602[1] = delta_x[1] + nom_x[1]; - out_760253992355356602[2] = delta_x[2] + nom_x[2]; - out_760253992355356602[3] = delta_x[3] + nom_x[3]; - out_760253992355356602[4] = delta_x[4] + nom_x[4]; - out_760253992355356602[5] = delta_x[5] + nom_x[5]; - out_760253992355356602[6] = delta_x[6] + nom_x[6]; - out_760253992355356602[7] = delta_x[7] + nom_x[7]; - out_760253992355356602[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353) { + out_1697783337874095353[0] = delta_x[0] + nom_x[0]; + out_1697783337874095353[1] = delta_x[1] + nom_x[1]; + out_1697783337874095353[2] = delta_x[2] + nom_x[2]; + out_1697783337874095353[3] = delta_x[3] + nom_x[3]; + out_1697783337874095353[4] = delta_x[4] + nom_x[4]; + out_1697783337874095353[5] = delta_x[5] + nom_x[5]; + out_1697783337874095353[6] = delta_x[6] + nom_x[6]; + out_1697783337874095353[7] = delta_x[7] + nom_x[7]; + out_1697783337874095353[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) { - out_4983548601721046404[0] = -nom_x[0] + true_x[0]; - out_4983548601721046404[1] = -nom_x[1] + true_x[1]; - out_4983548601721046404[2] = -nom_x[2] + true_x[2]; - out_4983548601721046404[3] = -nom_x[3] + true_x[3]; - out_4983548601721046404[4] = -nom_x[4] + true_x[4]; - out_4983548601721046404[5] = -nom_x[5] + true_x[5]; - out_4983548601721046404[6] = -nom_x[6] + true_x[6]; - out_4983548601721046404[7] = -nom_x[7] + true_x[7]; - out_4983548601721046404[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) { + out_3236445373879696441[0] = -nom_x[0] + true_x[0]; + out_3236445373879696441[1] = -nom_x[1] + true_x[1]; + out_3236445373879696441[2] = -nom_x[2] + true_x[2]; + out_3236445373879696441[3] = -nom_x[3] + true_x[3]; + out_3236445373879696441[4] = -nom_x[4] + true_x[4]; + out_3236445373879696441[5] = -nom_x[5] + true_x[5]; + out_3236445373879696441[6] = -nom_x[6] + true_x[6]; + out_3236445373879696441[7] = -nom_x[7] + true_x[7]; + out_3236445373879696441[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8995833010704521871) { - out_8995833010704521871[0] = 1.0; - out_8995833010704521871[1] = 0.0; - out_8995833010704521871[2] = 0.0; - out_8995833010704521871[3] = 0.0; - out_8995833010704521871[4] = 0.0; - out_8995833010704521871[5] = 0.0; - out_8995833010704521871[6] = 0.0; - out_8995833010704521871[7] = 0.0; - out_8995833010704521871[8] = 0.0; - out_8995833010704521871[9] = 0.0; - out_8995833010704521871[10] = 1.0; - out_8995833010704521871[11] = 0.0; - out_8995833010704521871[12] = 0.0; - out_8995833010704521871[13] = 0.0; - out_8995833010704521871[14] = 0.0; - out_8995833010704521871[15] = 0.0; - out_8995833010704521871[16] = 0.0; - out_8995833010704521871[17] = 0.0; - out_8995833010704521871[18] = 0.0; - out_8995833010704521871[19] = 0.0; - out_8995833010704521871[20] = 1.0; - out_8995833010704521871[21] = 0.0; - out_8995833010704521871[22] = 0.0; - out_8995833010704521871[23] = 0.0; - out_8995833010704521871[24] = 0.0; - out_8995833010704521871[25] = 0.0; - out_8995833010704521871[26] = 0.0; - out_8995833010704521871[27] = 0.0; - out_8995833010704521871[28] = 0.0; - out_8995833010704521871[29] = 0.0; - out_8995833010704521871[30] = 1.0; - out_8995833010704521871[31] = 0.0; - out_8995833010704521871[32] = 0.0; - out_8995833010704521871[33] = 0.0; - out_8995833010704521871[34] = 0.0; - out_8995833010704521871[35] = 0.0; - out_8995833010704521871[36] = 0.0; - out_8995833010704521871[37] = 0.0; - out_8995833010704521871[38] = 0.0; - out_8995833010704521871[39] = 0.0; - out_8995833010704521871[40] = 1.0; - out_8995833010704521871[41] = 0.0; - out_8995833010704521871[42] = 0.0; - out_8995833010704521871[43] = 0.0; - out_8995833010704521871[44] = 0.0; - out_8995833010704521871[45] = 0.0; - out_8995833010704521871[46] = 0.0; - out_8995833010704521871[47] = 0.0; - out_8995833010704521871[48] = 0.0; - out_8995833010704521871[49] = 0.0; - out_8995833010704521871[50] = 1.0; - out_8995833010704521871[51] = 0.0; - out_8995833010704521871[52] = 0.0; - out_8995833010704521871[53] = 0.0; - out_8995833010704521871[54] = 0.0; - out_8995833010704521871[55] = 0.0; - out_8995833010704521871[56] = 0.0; - out_8995833010704521871[57] = 0.0; - out_8995833010704521871[58] = 0.0; - out_8995833010704521871[59] = 0.0; - out_8995833010704521871[60] = 1.0; - out_8995833010704521871[61] = 0.0; - out_8995833010704521871[62] = 0.0; - out_8995833010704521871[63] = 0.0; - out_8995833010704521871[64] = 0.0; - out_8995833010704521871[65] = 0.0; - out_8995833010704521871[66] = 0.0; - out_8995833010704521871[67] = 0.0; - out_8995833010704521871[68] = 0.0; - out_8995833010704521871[69] = 0.0; - out_8995833010704521871[70] = 1.0; - out_8995833010704521871[71] = 0.0; - out_8995833010704521871[72] = 0.0; - out_8995833010704521871[73] = 0.0; - out_8995833010704521871[74] = 0.0; - out_8995833010704521871[75] = 0.0; - out_8995833010704521871[76] = 0.0; - out_8995833010704521871[77] = 0.0; - out_8995833010704521871[78] = 0.0; - out_8995833010704521871[79] = 0.0; - out_8995833010704521871[80] = 1.0; +void H_mod_fun(double *state, double *out_4178247856660158459) { + out_4178247856660158459[0] = 1.0; + out_4178247856660158459[1] = 0.0; + out_4178247856660158459[2] = 0.0; + out_4178247856660158459[3] = 0.0; + out_4178247856660158459[4] = 0.0; + out_4178247856660158459[5] = 0.0; + out_4178247856660158459[6] = 0.0; + out_4178247856660158459[7] = 0.0; + out_4178247856660158459[8] = 0.0; + out_4178247856660158459[9] = 0.0; + out_4178247856660158459[10] = 1.0; + out_4178247856660158459[11] = 0.0; + out_4178247856660158459[12] = 0.0; + out_4178247856660158459[13] = 0.0; + out_4178247856660158459[14] = 0.0; + out_4178247856660158459[15] = 0.0; + out_4178247856660158459[16] = 0.0; + out_4178247856660158459[17] = 0.0; + out_4178247856660158459[18] = 0.0; + out_4178247856660158459[19] = 0.0; + out_4178247856660158459[20] = 1.0; + out_4178247856660158459[21] = 0.0; + out_4178247856660158459[22] = 0.0; + out_4178247856660158459[23] = 0.0; + out_4178247856660158459[24] = 0.0; + out_4178247856660158459[25] = 0.0; + out_4178247856660158459[26] = 0.0; + out_4178247856660158459[27] = 0.0; + out_4178247856660158459[28] = 0.0; + out_4178247856660158459[29] = 0.0; + out_4178247856660158459[30] = 1.0; + out_4178247856660158459[31] = 0.0; + out_4178247856660158459[32] = 0.0; + out_4178247856660158459[33] = 0.0; + out_4178247856660158459[34] = 0.0; + out_4178247856660158459[35] = 0.0; + out_4178247856660158459[36] = 0.0; + out_4178247856660158459[37] = 0.0; + out_4178247856660158459[38] = 0.0; + out_4178247856660158459[39] = 0.0; + out_4178247856660158459[40] = 1.0; + out_4178247856660158459[41] = 0.0; + out_4178247856660158459[42] = 0.0; + out_4178247856660158459[43] = 0.0; + out_4178247856660158459[44] = 0.0; + out_4178247856660158459[45] = 0.0; + out_4178247856660158459[46] = 0.0; + out_4178247856660158459[47] = 0.0; + out_4178247856660158459[48] = 0.0; + out_4178247856660158459[49] = 0.0; + out_4178247856660158459[50] = 1.0; + out_4178247856660158459[51] = 0.0; + out_4178247856660158459[52] = 0.0; + out_4178247856660158459[53] = 0.0; + out_4178247856660158459[54] = 0.0; + out_4178247856660158459[55] = 0.0; + out_4178247856660158459[56] = 0.0; + out_4178247856660158459[57] = 0.0; + out_4178247856660158459[58] = 0.0; + out_4178247856660158459[59] = 0.0; + out_4178247856660158459[60] = 1.0; + out_4178247856660158459[61] = 0.0; + out_4178247856660158459[62] = 0.0; + out_4178247856660158459[63] = 0.0; + out_4178247856660158459[64] = 0.0; + out_4178247856660158459[65] = 0.0; + out_4178247856660158459[66] = 0.0; + out_4178247856660158459[67] = 0.0; + out_4178247856660158459[68] = 0.0; + out_4178247856660158459[69] = 0.0; + out_4178247856660158459[70] = 1.0; + out_4178247856660158459[71] = 0.0; + out_4178247856660158459[72] = 0.0; + out_4178247856660158459[73] = 0.0; + out_4178247856660158459[74] = 0.0; + out_4178247856660158459[75] = 0.0; + out_4178247856660158459[76] = 0.0; + out_4178247856660158459[77] = 0.0; + out_4178247856660158459[78] = 0.0; + out_4178247856660158459[79] = 0.0; + out_4178247856660158459[80] = 1.0; } -void f_fun(double *state, double dt, double *out_4562655195590537984) { - out_4562655195590537984[0] = state[0]; - out_4562655195590537984[1] = state[1]; - out_4562655195590537984[2] = state[2]; - out_4562655195590537984[3] = state[3]; - out_4562655195590537984[4] = state[4]; - out_4562655195590537984[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*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_4562655195590537984[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_4562655195590537984[7] = state[7]; - out_4562655195590537984[8] = state[8]; +void f_fun(double *state, double dt, double *out_6661857716321963649) { + out_6661857716321963649[0] = state[0]; + out_6661857716321963649[1] = state[1]; + out_6661857716321963649[2] = state[2]; + out_6661857716321963649[3] = state[3]; + out_6661857716321963649[4] = state[4]; + out_6661857716321963649[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*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_6661857716321963649[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_6661857716321963649[7] = state[7]; + out_6661857716321963649[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6423431565507769528) { - out_6423431565507769528[0] = 1; - out_6423431565507769528[1] = 0; - out_6423431565507769528[2] = 0; - out_6423431565507769528[3] = 0; - out_6423431565507769528[4] = 0; - out_6423431565507769528[5] = 0; - out_6423431565507769528[6] = 0; - out_6423431565507769528[7] = 0; - out_6423431565507769528[8] = 0; - out_6423431565507769528[9] = 0; - out_6423431565507769528[10] = 1; - out_6423431565507769528[11] = 0; - out_6423431565507769528[12] = 0; - out_6423431565507769528[13] = 0; - out_6423431565507769528[14] = 0; - out_6423431565507769528[15] = 0; - out_6423431565507769528[16] = 0; - out_6423431565507769528[17] = 0; - out_6423431565507769528[18] = 0; - out_6423431565507769528[19] = 0; - out_6423431565507769528[20] = 1; - out_6423431565507769528[21] = 0; - out_6423431565507769528[22] = 0; - out_6423431565507769528[23] = 0; - out_6423431565507769528[24] = 0; - out_6423431565507769528[25] = 0; - out_6423431565507769528[26] = 0; - out_6423431565507769528[27] = 0; - out_6423431565507769528[28] = 0; - out_6423431565507769528[29] = 0; - out_6423431565507769528[30] = 1; - out_6423431565507769528[31] = 0; - out_6423431565507769528[32] = 0; - out_6423431565507769528[33] = 0; - out_6423431565507769528[34] = 0; - out_6423431565507769528[35] = 0; - out_6423431565507769528[36] = 0; - out_6423431565507769528[37] = 0; - out_6423431565507769528[38] = 0; - out_6423431565507769528[39] = 0; - out_6423431565507769528[40] = 1; - out_6423431565507769528[41] = 0; - out_6423431565507769528[42] = 0; - out_6423431565507769528[43] = 0; - out_6423431565507769528[44] = 0; - out_6423431565507769528[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_6423431565507769528[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6423431565507769528[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6423431565507769528[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6423431565507769528[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_6423431565507769528[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6423431565507769528[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6423431565507769528[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6423431565507769528[53] = -9.8100000000000005*dt; - out_6423431565507769528[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_6423431565507769528[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6423431565507769528[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6423431565507769528[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6423431565507769528[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_6423431565507769528[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6423431565507769528[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_6423431565507769528[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6423431565507769528[62] = 0; - out_6423431565507769528[63] = 0; - out_6423431565507769528[64] = 0; - out_6423431565507769528[65] = 0; - out_6423431565507769528[66] = 0; - out_6423431565507769528[67] = 0; - out_6423431565507769528[68] = 0; - out_6423431565507769528[69] = 0; - out_6423431565507769528[70] = 1; - out_6423431565507769528[71] = 0; - out_6423431565507769528[72] = 0; - out_6423431565507769528[73] = 0; - out_6423431565507769528[74] = 0; - out_6423431565507769528[75] = 0; - out_6423431565507769528[76] = 0; - out_6423431565507769528[77] = 0; - out_6423431565507769528[78] = 0; - out_6423431565507769528[79] = 0; - out_6423431565507769528[80] = 1; +void F_fun(double *state, double dt, double *out_4395736809983768071) { + out_4395736809983768071[0] = 1; + out_4395736809983768071[1] = 0; + out_4395736809983768071[2] = 0; + out_4395736809983768071[3] = 0; + out_4395736809983768071[4] = 0; + out_4395736809983768071[5] = 0; + out_4395736809983768071[6] = 0; + out_4395736809983768071[7] = 0; + out_4395736809983768071[8] = 0; + out_4395736809983768071[9] = 0; + out_4395736809983768071[10] = 1; + out_4395736809983768071[11] = 0; + out_4395736809983768071[12] = 0; + out_4395736809983768071[13] = 0; + out_4395736809983768071[14] = 0; + out_4395736809983768071[15] = 0; + out_4395736809983768071[16] = 0; + out_4395736809983768071[17] = 0; + out_4395736809983768071[18] = 0; + out_4395736809983768071[19] = 0; + out_4395736809983768071[20] = 1; + out_4395736809983768071[21] = 0; + out_4395736809983768071[22] = 0; + out_4395736809983768071[23] = 0; + out_4395736809983768071[24] = 0; + out_4395736809983768071[25] = 0; + out_4395736809983768071[26] = 0; + out_4395736809983768071[27] = 0; + out_4395736809983768071[28] = 0; + out_4395736809983768071[29] = 0; + out_4395736809983768071[30] = 1; + out_4395736809983768071[31] = 0; + out_4395736809983768071[32] = 0; + out_4395736809983768071[33] = 0; + out_4395736809983768071[34] = 0; + out_4395736809983768071[35] = 0; + out_4395736809983768071[36] = 0; + out_4395736809983768071[37] = 0; + out_4395736809983768071[38] = 0; + out_4395736809983768071[39] = 0; + out_4395736809983768071[40] = 1; + out_4395736809983768071[41] = 0; + out_4395736809983768071[42] = 0; + out_4395736809983768071[43] = 0; + out_4395736809983768071[44] = 0; + out_4395736809983768071[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_4395736809983768071[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_4395736809983768071[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4395736809983768071[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_4395736809983768071[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_4395736809983768071[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_4395736809983768071[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_4395736809983768071[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_4395736809983768071[53] = -9.8100000000000005*dt; + out_4395736809983768071[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_4395736809983768071[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_4395736809983768071[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4395736809983768071[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4395736809983768071[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_4395736809983768071[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_4395736809983768071[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_4395736809983768071[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_4395736809983768071[62] = 0; + out_4395736809983768071[63] = 0; + out_4395736809983768071[64] = 0; + out_4395736809983768071[65] = 0; + out_4395736809983768071[66] = 0; + out_4395736809983768071[67] = 0; + out_4395736809983768071[68] = 0; + out_4395736809983768071[69] = 0; + out_4395736809983768071[70] = 1; + out_4395736809983768071[71] = 0; + out_4395736809983768071[72] = 0; + out_4395736809983768071[73] = 0; + out_4395736809983768071[74] = 0; + out_4395736809983768071[75] = 0; + out_4395736809983768071[76] = 0; + out_4395736809983768071[77] = 0; + out_4395736809983768071[78] = 0; + out_4395736809983768071[79] = 0; + out_4395736809983768071[80] = 1; } -void h_25(double *state, double *unused, double *out_8281489855210528984) { - out_8281489855210528984[0] = state[6]; +void h_25(double *state, double *unused, double *out_2333658162275422577) { + out_2333658162275422577[0] = state[6]; } -void H_25(double *state, double *unused, double *out_257722837092792508) { - out_257722837092792508[0] = 0; - out_257722837092792508[1] = 0; - out_257722837092792508[2] = 0; - out_257722837092792508[3] = 0; - out_257722837092792508[4] = 0; - out_257722837092792508[5] = 0; - out_257722837092792508[6] = 1; - out_257722837092792508[7] = 0; - out_257722837092792508[8] = 0; +void H_25(double *state, double *unused, double *out_725443968524506381) { + out_725443968524506381[0] = 0; + out_725443968524506381[1] = 0; + out_725443968524506381[2] = 0; + out_725443968524506381[3] = 0; + out_725443968524506381[4] = 0; + out_725443968524506381[5] = 0; + out_725443968524506381[6] = 1; + out_725443968524506381[7] = 0; + out_725443968524506381[8] = 0; } -void h_24(double *state, double *unused, double *out_5654195456743599601) { - out_5654195456743599601[0] = state[4]; - out_5654195456743599601[1] = state[5]; +void h_24(double *state, double *unused, double *out_7377061376079036481) { + out_7377061376079036481[0] = state[4]; + out_7377061376079036481[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2483430621071661070) { - out_2483430621071661070[0] = 0; - out_2483430621071661070[1] = 0; - out_2483430621071661070[2] = 0; - out_2483430621071661070[3] = 0; - out_2483430621071661070[4] = 1; - out_2483430621071661070[5] = 0; - out_2483430621071661070[6] = 0; - out_2483430621071661070[7] = 0; - out_2483430621071661070[8] = 0; - out_2483430621071661070[9] = 0; - out_2483430621071661070[10] = 0; - out_2483430621071661070[11] = 0; - out_2483430621071661070[12] = 0; - out_2483430621071661070[13] = 0; - out_2483430621071661070[14] = 1; - out_2483430621071661070[15] = 0; - out_2483430621071661070[16] = 0; - out_2483430621071661070[17] = 0; +void H_24(double *state, double *unused, double *out_1570440577308201780) { + out_1570440577308201780[0] = 0; + out_1570440577308201780[1] = 0; + out_1570440577308201780[2] = 0; + out_1570440577308201780[3] = 0; + out_1570440577308201780[4] = 1; + out_1570440577308201780[5] = 0; + out_1570440577308201780[6] = 0; + out_1570440577308201780[7] = 0; + out_1570440577308201780[8] = 0; + out_1570440577308201780[9] = 0; + out_1570440577308201780[10] = 0; + out_1570440577308201780[11] = 0; + out_1570440577308201780[12] = 0; + out_1570440577308201780[13] = 0; + out_1570440577308201780[14] = 1; + out_1570440577308201780[15] = 0; + out_1570440577308201780[16] = 0; + out_1570440577308201780[17] = 0; } -void h_30(double *state, double *unused, double *out_8438676523561658129) { - out_8438676523561658129[0] = state[4]; +void h_30(double *state, double *unused, double *out_2058464099990916688) { + out_2058464099990916688[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7174413178584409263) { - out_7174413178584409263[0] = 0; - out_7174413178584409263[1] = 0; - out_7174413178584409263[2] = 0; - out_7174413178584409263[3] = 0; - out_7174413178584409263[4] = 1; - out_7174413178584409263[5] = 0; - out_7174413178584409263[6] = 0; - out_7174413178584409263[7] = 0; - out_7174413178584409263[8] = 0; +void H_30(double *state, double *unused, double *out_3243776927031755008) { + out_3243776927031755008[0] = 0; + out_3243776927031755008[1] = 0; + out_3243776927031755008[2] = 0; + out_3243776927031755008[3] = 0; + out_3243776927031755008[4] = 1; + out_3243776927031755008[5] = 0; + out_3243776927031755008[6] = 0; + out_3243776927031755008[7] = 0; + out_3243776927031755008[8] = 0; } -void h_26(double *state, double *unused, double *out_1419904868343052079) { - out_1419904868343052079[0] = state[7]; +void h_26(double *state, double *unused, double *out_8738934800407300586) { + out_8738934800407300586[0] = state[7]; } -void H_26(double *state, double *unused, double *out_3562248806853593109) { - out_3562248806853593109[0] = 0; - out_3562248806853593109[1] = 0; - out_3562248806853593109[2] = 0; - out_3562248806853593109[3] = 0; - out_3562248806853593109[4] = 0; - out_3562248806853593109[5] = 0; - out_3562248806853593109[6] = 0; - out_3562248806853593109[7] = 1; - out_3562248806853593109[8] = 0; +void H_26(double *state, double *unused, double *out_3016059350349549843) { + out_3016059350349549843[0] = 0; + out_3016059350349549843[1] = 0; + out_3016059350349549843[2] = 0; + out_3016059350349549843[3] = 0; + out_3016059350349549843[4] = 0; + out_3016059350349549843[5] = 0; + out_3016059350349549843[6] = 0; + out_3016059350349549843[7] = 1; + out_3016059350349549843[8] = 0; } -void h_27(double *state, double *unused, double *out_1905364559686504598) { - out_1905364559686504598[0] = state[3]; +void h_27(double *state, double *unused, double *out_3277168015177085012) { + out_3277168015177085012[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4999649866783984352) { - out_4999649866783984352[0] = 0; - out_4999649866783984352[1] = 0; - out_4999649866783984352[2] = 0; - out_4999649866783984352[3] = 1; - out_4999649866783984352[4] = 0; - out_4999649866783984352[5] = 0; - out_4999649866783984352[6] = 0; - out_4999649866783984352[7] = 0; - out_4999649866783984352[8] = 0; +void H_27(double *state, double *unused, double *out_5467370998215698225) { + out_5467370998215698225[0] = 0; + out_5467370998215698225[1] = 0; + out_5467370998215698225[2] = 0; + out_5467370998215698225[3] = 1; + out_5467370998215698225[4] = 0; + out_5467370998215698225[5] = 0; + out_5467370998215698225[6] = 0; + out_5467370998215698225[7] = 0; + out_5467370998215698225[8] = 0; } -void h_29(double *state, double *unused, double *out_3005304846354827708) { - out_3005304846354827708[0] = state[1]; +void h_29(double *state, double *unused, double *out_4679132473327541980) { + out_4679132473327541980[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7684644522898801447) { - out_7684644522898801447[0] = 0; - out_7684644522898801447[1] = 1; - out_7684644522898801447[2] = 0; - out_7684644522898801447[3] = 0; - out_7684644522898801447[4] = 0; - out_7684644522898801447[5] = 0; - out_7684644522898801447[6] = 0; - out_7684644522898801447[7] = 0; - out_7684644522898801447[8] = 0; +void H_29(double *state, double *unused, double *out_3754008271346147192) { + out_3754008271346147192[0] = 0; + out_3754008271346147192[1] = 1; + out_3754008271346147192[2] = 0; + out_3754008271346147192[3] = 0; + out_3754008271346147192[4] = 0; + out_3754008271346147192[5] = 0; + out_3754008271346147192[6] = 0; + out_3754008271346147192[7] = 0; + out_3754008271346147192[8] = 0; } -void h_28(double *state, double *unused, double *out_8832630219803516209) { - out_8832630219803516209[0] = state[0]; +void h_28(double *state, double *unused, double *out_1724505252483760633) { + out_1724505252483760633[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1796111877155097255) { - out_1796111877155097255[0] = 1; - out_1796111877155097255[1] = 0; - out_1796111877155097255[2] = 0; - out_1796111877155097255[3] = 0; - out_1796111877155097255[4] = 0; - out_1796111877155097255[5] = 0; - out_1796111877155097255[6] = 0; - out_1796111877155097255[7] = 0; - out_1796111877155097255[8] = 0; +void H_28(double *state, double *unused, double *out_1328390745723383382) { + out_1328390745723383382[0] = 1; + out_1328390745723383382[1] = 0; + out_1328390745723383382[2] = 0; + out_1328390745723383382[3] = 0; + out_1328390745723383382[4] = 0; + out_1328390745723383382[5] = 0; + out_1328390745723383382[6] = 0; + out_1328390745723383382[7] = 0; + out_1328390745723383382[8] = 0; } -void h_31(double *state, double *unused, double *out_6225098197673216166) { - out_6225098197673216166[0] = state[8]; +void h_31(double *state, double *unused, double *out_6530453262617598609) { + out_6530453262617598609[0] = state[8]; } -void H_31(double *state, double *unused, double *out_7334398087604609761) { - out_7334398087604609761[0] = 0; - out_7334398087604609761[1] = 0; - out_7334398087604609761[2] = 0; - out_7334398087604609761[3] = 0; - out_7334398087604609761[4] = 0; - out_7334398087604609761[5] = 0; - out_7334398087604609761[6] = 0; - out_7334398087604609761[7] = 0; - out_7334398087604609761[8] = 1; +void H_31(double *state, double *unused, double *out_756089930401466809) { + out_756089930401466809[0] = 0; + out_756089930401466809[1] = 0; + out_756089930401466809[2] = 0; + out_756089930401466809[3] = 0; + out_756089930401466809[4] = 0; + out_756089930401466809[5] = 0; + out_756089930401466809[6] = 0; + out_756089930401466809[7] = 0; + out_756089930401466809[8] = 1; } #include #include @@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31); } -void car_err_fun(double *nom_x, double *delta_x, double *out_760253992355356602) { - err_fun(nom_x, delta_x, out_760253992355356602); +void car_err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353) { + err_fun(nom_x, delta_x, out_1697783337874095353); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404) { - inv_err_fun(nom_x, true_x, out_4983548601721046404); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441) { + inv_err_fun(nom_x, true_x, out_3236445373879696441); } -void car_H_mod_fun(double *state, double *out_8995833010704521871) { - H_mod_fun(state, out_8995833010704521871); +void car_H_mod_fun(double *state, double *out_4178247856660158459) { + H_mod_fun(state, out_4178247856660158459); } -void car_f_fun(double *state, double dt, double *out_4562655195590537984) { - f_fun(state, dt, out_4562655195590537984); +void car_f_fun(double *state, double dt, double *out_6661857716321963649) { + f_fun(state, dt, out_6661857716321963649); } -void car_F_fun(double *state, double dt, double *out_6423431565507769528) { - F_fun(state, dt, out_6423431565507769528); +void car_F_fun(double *state, double dt, double *out_4395736809983768071) { + F_fun(state, dt, out_4395736809983768071); } -void car_h_25(double *state, double *unused, double *out_8281489855210528984) { - h_25(state, unused, out_8281489855210528984); +void car_h_25(double *state, double *unused, double *out_2333658162275422577) { + h_25(state, unused, out_2333658162275422577); } -void car_H_25(double *state, double *unused, double *out_257722837092792508) { - H_25(state, unused, out_257722837092792508); +void car_H_25(double *state, double *unused, double *out_725443968524506381) { + H_25(state, unused, out_725443968524506381); } -void car_h_24(double *state, double *unused, double *out_5654195456743599601) { - h_24(state, unused, out_5654195456743599601); +void car_h_24(double *state, double *unused, double *out_7377061376079036481) { + h_24(state, unused, out_7377061376079036481); } -void car_H_24(double *state, double *unused, double *out_2483430621071661070) { - H_24(state, unused, out_2483430621071661070); +void car_H_24(double *state, double *unused, double *out_1570440577308201780) { + H_24(state, unused, out_1570440577308201780); } -void car_h_30(double *state, double *unused, double *out_8438676523561658129) { - h_30(state, unused, out_8438676523561658129); +void car_h_30(double *state, double *unused, double *out_2058464099990916688) { + h_30(state, unused, out_2058464099990916688); } -void car_H_30(double *state, double *unused, double *out_7174413178584409263) { - H_30(state, unused, out_7174413178584409263); +void car_H_30(double *state, double *unused, double *out_3243776927031755008) { + H_30(state, unused, out_3243776927031755008); } -void car_h_26(double *state, double *unused, double *out_1419904868343052079) { - h_26(state, unused, out_1419904868343052079); +void car_h_26(double *state, double *unused, double *out_8738934800407300586) { + h_26(state, unused, out_8738934800407300586); } -void car_H_26(double *state, double *unused, double *out_3562248806853593109) { - H_26(state, unused, out_3562248806853593109); +void car_H_26(double *state, double *unused, double *out_3016059350349549843) { + H_26(state, unused, out_3016059350349549843); } -void car_h_27(double *state, double *unused, double *out_1905364559686504598) { - h_27(state, unused, out_1905364559686504598); +void car_h_27(double *state, double *unused, double *out_3277168015177085012) { + h_27(state, unused, out_3277168015177085012); } -void car_H_27(double *state, double *unused, double *out_4999649866783984352) { - H_27(state, unused, out_4999649866783984352); +void car_H_27(double *state, double *unused, double *out_5467370998215698225) { + H_27(state, unused, out_5467370998215698225); } -void car_h_29(double *state, double *unused, double *out_3005304846354827708) { - h_29(state, unused, out_3005304846354827708); +void car_h_29(double *state, double *unused, double *out_4679132473327541980) { + h_29(state, unused, out_4679132473327541980); } -void car_H_29(double *state, double *unused, double *out_7684644522898801447) { - H_29(state, unused, out_7684644522898801447); +void car_H_29(double *state, double *unused, double *out_3754008271346147192) { + H_29(state, unused, out_3754008271346147192); } -void car_h_28(double *state, double *unused, double *out_8832630219803516209) { - h_28(state, unused, out_8832630219803516209); +void car_h_28(double *state, double *unused, double *out_1724505252483760633) { + h_28(state, unused, out_1724505252483760633); } -void car_H_28(double *state, double *unused, double *out_1796111877155097255) { - H_28(state, unused, out_1796111877155097255); +void car_H_28(double *state, double *unused, double *out_1328390745723383382) { + H_28(state, unused, out_1328390745723383382); } -void car_h_31(double *state, double *unused, double *out_6225098197673216166) { - h_31(state, unused, out_6225098197673216166); +void car_h_31(double *state, double *unused, double *out_6530453262617598609) { + h_31(state, unused, out_6530453262617598609); } -void car_H_31(double *state, double *unused, double *out_7334398087604609761) { - H_31(state, unused, out_7334398087604609761); +void car_H_31(double *state, double *unused, double *out_756089930401466809) { + H_31(state, unused, out_756089930401466809); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 877bd06b3..c50cf50d8 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_760253992355356602); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_4983548601721046404); -void car_H_mod_fun(double *state, double *out_8995833010704521871); -void car_f_fun(double *state, double dt, double *out_4562655195590537984); -void car_F_fun(double *state, double dt, double *out_6423431565507769528); -void car_h_25(double *state, double *unused, double *out_8281489855210528984); -void car_H_25(double *state, double *unused, double *out_257722837092792508); -void car_h_24(double *state, double *unused, double *out_5654195456743599601); -void car_H_24(double *state, double *unused, double *out_2483430621071661070); -void car_h_30(double *state, double *unused, double *out_8438676523561658129); -void car_H_30(double *state, double *unused, double *out_7174413178584409263); -void car_h_26(double *state, double *unused, double *out_1419904868343052079); -void car_H_26(double *state, double *unused, double *out_3562248806853593109); -void car_h_27(double *state, double *unused, double *out_1905364559686504598); -void car_H_27(double *state, double *unused, double *out_4999649866783984352); -void car_h_29(double *state, double *unused, double *out_3005304846354827708); -void car_H_29(double *state, double *unused, double *out_7684644522898801447); -void car_h_28(double *state, double *unused, double *out_8832630219803516209); -void car_H_28(double *state, double *unused, double *out_1796111877155097255); -void car_h_31(double *state, double *unused, double *out_6225098197673216166); -void car_H_31(double *state, double *unused, double *out_7334398087604609761); +void car_err_fun(double *nom_x, double *delta_x, double *out_1697783337874095353); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3236445373879696441); +void car_H_mod_fun(double *state, double *out_4178247856660158459); +void car_f_fun(double *state, double dt, double *out_6661857716321963649); +void car_F_fun(double *state, double dt, double *out_4395736809983768071); +void car_h_25(double *state, double *unused, double *out_2333658162275422577); +void car_H_25(double *state, double *unused, double *out_725443968524506381); +void car_h_24(double *state, double *unused, double *out_7377061376079036481); +void car_H_24(double *state, double *unused, double *out_1570440577308201780); +void car_h_30(double *state, double *unused, double *out_2058464099990916688); +void car_H_30(double *state, double *unused, double *out_3243776927031755008); +void car_h_26(double *state, double *unused, double *out_8738934800407300586); +void car_H_26(double *state, double *unused, double *out_3016059350349549843); +void car_h_27(double *state, double *unused, double *out_3277168015177085012); +void car_H_27(double *state, double *unused, double *out_5467370998215698225); +void car_h_29(double *state, double *unused, double *out_4679132473327541980); +void car_H_29(double *state, double *unused, double *out_3754008271346147192); +void car_h_28(double *state, double *unused, double *out_1724505252483760633); +void car_H_28(double *state, double *unused, double *out_1328390745723383382); +void car_h_31(double *state, double *unused, double *out_6530453262617598609); +void car_H_31(double *state, double *unused, double *out_756089930401466809); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index e73bbd6a8..7725b0c2c 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071) { - out_7113141565155944071[0] = delta_x[0] + nom_x[0]; - out_7113141565155944071[1] = delta_x[1] + nom_x[1]; - out_7113141565155944071[2] = delta_x[2] + nom_x[2]; - out_7113141565155944071[3] = delta_x[3] + nom_x[3]; - out_7113141565155944071[4] = delta_x[4] + nom_x[4]; - out_7113141565155944071[5] = delta_x[5] + nom_x[5]; - out_7113141565155944071[6] = delta_x[6] + nom_x[6]; - out_7113141565155944071[7] = delta_x[7] + nom_x[7]; - out_7113141565155944071[8] = delta_x[8] + nom_x[8]; - out_7113141565155944071[9] = delta_x[9] + nom_x[9]; - out_7113141565155944071[10] = delta_x[10] + nom_x[10]; - out_7113141565155944071[11] = delta_x[11] + nom_x[11]; - out_7113141565155944071[12] = delta_x[12] + nom_x[12]; - out_7113141565155944071[13] = delta_x[13] + nom_x[13]; - out_7113141565155944071[14] = delta_x[14] + nom_x[14]; - out_7113141565155944071[15] = delta_x[15] + nom_x[15]; - out_7113141565155944071[16] = delta_x[16] + nom_x[16]; - out_7113141565155944071[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4940663026929446750) { + out_4940663026929446750[0] = delta_x[0] + nom_x[0]; + out_4940663026929446750[1] = delta_x[1] + nom_x[1]; + out_4940663026929446750[2] = delta_x[2] + nom_x[2]; + out_4940663026929446750[3] = delta_x[3] + nom_x[3]; + out_4940663026929446750[4] = delta_x[4] + nom_x[4]; + out_4940663026929446750[5] = delta_x[5] + nom_x[5]; + out_4940663026929446750[6] = delta_x[6] + nom_x[6]; + out_4940663026929446750[7] = delta_x[7] + nom_x[7]; + out_4940663026929446750[8] = delta_x[8] + nom_x[8]; + out_4940663026929446750[9] = delta_x[9] + nom_x[9]; + out_4940663026929446750[10] = delta_x[10] + nom_x[10]; + out_4940663026929446750[11] = delta_x[11] + nom_x[11]; + out_4940663026929446750[12] = delta_x[12] + nom_x[12]; + out_4940663026929446750[13] = delta_x[13] + nom_x[13]; + out_4940663026929446750[14] = delta_x[14] + nom_x[14]; + out_4940663026929446750[15] = delta_x[15] + nom_x[15]; + out_4940663026929446750[16] = delta_x[16] + nom_x[16]; + out_4940663026929446750[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861) { - out_4273806753012627861[0] = -nom_x[0] + true_x[0]; - out_4273806753012627861[1] = -nom_x[1] + true_x[1]; - out_4273806753012627861[2] = -nom_x[2] + true_x[2]; - out_4273806753012627861[3] = -nom_x[3] + true_x[3]; - out_4273806753012627861[4] = -nom_x[4] + true_x[4]; - out_4273806753012627861[5] = -nom_x[5] + true_x[5]; - out_4273806753012627861[6] = -nom_x[6] + true_x[6]; - out_4273806753012627861[7] = -nom_x[7] + true_x[7]; - out_4273806753012627861[8] = -nom_x[8] + true_x[8]; - out_4273806753012627861[9] = -nom_x[9] + true_x[9]; - out_4273806753012627861[10] = -nom_x[10] + true_x[10]; - out_4273806753012627861[11] = -nom_x[11] + true_x[11]; - out_4273806753012627861[12] = -nom_x[12] + true_x[12]; - out_4273806753012627861[13] = -nom_x[13] + true_x[13]; - out_4273806753012627861[14] = -nom_x[14] + true_x[14]; - out_4273806753012627861[15] = -nom_x[15] + true_x[15]; - out_4273806753012627861[16] = -nom_x[16] + true_x[16]; - out_4273806753012627861[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442) { + out_1207218016783825442[0] = -nom_x[0] + true_x[0]; + out_1207218016783825442[1] = -nom_x[1] + true_x[1]; + out_1207218016783825442[2] = -nom_x[2] + true_x[2]; + out_1207218016783825442[3] = -nom_x[3] + true_x[3]; + out_1207218016783825442[4] = -nom_x[4] + true_x[4]; + out_1207218016783825442[5] = -nom_x[5] + true_x[5]; + out_1207218016783825442[6] = -nom_x[6] + true_x[6]; + out_1207218016783825442[7] = -nom_x[7] + true_x[7]; + out_1207218016783825442[8] = -nom_x[8] + true_x[8]; + out_1207218016783825442[9] = -nom_x[9] + true_x[9]; + out_1207218016783825442[10] = -nom_x[10] + true_x[10]; + out_1207218016783825442[11] = -nom_x[11] + true_x[11]; + out_1207218016783825442[12] = -nom_x[12] + true_x[12]; + out_1207218016783825442[13] = -nom_x[13] + true_x[13]; + out_1207218016783825442[14] = -nom_x[14] + true_x[14]; + out_1207218016783825442[15] = -nom_x[15] + true_x[15]; + out_1207218016783825442[16] = -nom_x[16] + true_x[16]; + out_1207218016783825442[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4327706851715669235) { - out_4327706851715669235[0] = 1.0; - out_4327706851715669235[1] = 0.0; - out_4327706851715669235[2] = 0.0; - out_4327706851715669235[3] = 0.0; - out_4327706851715669235[4] = 0.0; - out_4327706851715669235[5] = 0.0; - out_4327706851715669235[6] = 0.0; - out_4327706851715669235[7] = 0.0; - out_4327706851715669235[8] = 0.0; - out_4327706851715669235[9] = 0.0; - out_4327706851715669235[10] = 0.0; - out_4327706851715669235[11] = 0.0; - out_4327706851715669235[12] = 0.0; - out_4327706851715669235[13] = 0.0; - out_4327706851715669235[14] = 0.0; - out_4327706851715669235[15] = 0.0; - out_4327706851715669235[16] = 0.0; - out_4327706851715669235[17] = 0.0; - out_4327706851715669235[18] = 0.0; - out_4327706851715669235[19] = 1.0; - out_4327706851715669235[20] = 0.0; - out_4327706851715669235[21] = 0.0; - out_4327706851715669235[22] = 0.0; - out_4327706851715669235[23] = 0.0; - out_4327706851715669235[24] = 0.0; - out_4327706851715669235[25] = 0.0; - out_4327706851715669235[26] = 0.0; - out_4327706851715669235[27] = 0.0; - out_4327706851715669235[28] = 0.0; - out_4327706851715669235[29] = 0.0; - out_4327706851715669235[30] = 0.0; - out_4327706851715669235[31] = 0.0; - out_4327706851715669235[32] = 0.0; - out_4327706851715669235[33] = 0.0; - out_4327706851715669235[34] = 0.0; - out_4327706851715669235[35] = 0.0; - out_4327706851715669235[36] = 0.0; - out_4327706851715669235[37] = 0.0; - out_4327706851715669235[38] = 1.0; - out_4327706851715669235[39] = 0.0; - out_4327706851715669235[40] = 0.0; - out_4327706851715669235[41] = 0.0; - out_4327706851715669235[42] = 0.0; - out_4327706851715669235[43] = 0.0; - out_4327706851715669235[44] = 0.0; - out_4327706851715669235[45] = 0.0; - out_4327706851715669235[46] = 0.0; - out_4327706851715669235[47] = 0.0; - out_4327706851715669235[48] = 0.0; - out_4327706851715669235[49] = 0.0; - out_4327706851715669235[50] = 0.0; - out_4327706851715669235[51] = 0.0; - out_4327706851715669235[52] = 0.0; - out_4327706851715669235[53] = 0.0; - out_4327706851715669235[54] = 0.0; - out_4327706851715669235[55] = 0.0; - out_4327706851715669235[56] = 0.0; - out_4327706851715669235[57] = 1.0; - out_4327706851715669235[58] = 0.0; - out_4327706851715669235[59] = 0.0; - out_4327706851715669235[60] = 0.0; - out_4327706851715669235[61] = 0.0; - out_4327706851715669235[62] = 0.0; - out_4327706851715669235[63] = 0.0; - out_4327706851715669235[64] = 0.0; - out_4327706851715669235[65] = 0.0; - out_4327706851715669235[66] = 0.0; - out_4327706851715669235[67] = 0.0; - out_4327706851715669235[68] = 0.0; - out_4327706851715669235[69] = 0.0; - out_4327706851715669235[70] = 0.0; - out_4327706851715669235[71] = 0.0; - out_4327706851715669235[72] = 0.0; - out_4327706851715669235[73] = 0.0; - out_4327706851715669235[74] = 0.0; - out_4327706851715669235[75] = 0.0; - out_4327706851715669235[76] = 1.0; - out_4327706851715669235[77] = 0.0; - out_4327706851715669235[78] = 0.0; - out_4327706851715669235[79] = 0.0; - out_4327706851715669235[80] = 0.0; - out_4327706851715669235[81] = 0.0; - out_4327706851715669235[82] = 0.0; - out_4327706851715669235[83] = 0.0; - out_4327706851715669235[84] = 0.0; - out_4327706851715669235[85] = 0.0; - out_4327706851715669235[86] = 0.0; - out_4327706851715669235[87] = 0.0; - out_4327706851715669235[88] = 0.0; - out_4327706851715669235[89] = 0.0; - out_4327706851715669235[90] = 0.0; - out_4327706851715669235[91] = 0.0; - out_4327706851715669235[92] = 0.0; - out_4327706851715669235[93] = 0.0; - out_4327706851715669235[94] = 0.0; - out_4327706851715669235[95] = 1.0; - out_4327706851715669235[96] = 0.0; - out_4327706851715669235[97] = 0.0; - out_4327706851715669235[98] = 0.0; - out_4327706851715669235[99] = 0.0; - out_4327706851715669235[100] = 0.0; - out_4327706851715669235[101] = 0.0; - out_4327706851715669235[102] = 0.0; - out_4327706851715669235[103] = 0.0; - out_4327706851715669235[104] = 0.0; - out_4327706851715669235[105] = 0.0; - out_4327706851715669235[106] = 0.0; - out_4327706851715669235[107] = 0.0; - out_4327706851715669235[108] = 0.0; - out_4327706851715669235[109] = 0.0; - out_4327706851715669235[110] = 0.0; - out_4327706851715669235[111] = 0.0; - out_4327706851715669235[112] = 0.0; - out_4327706851715669235[113] = 0.0; - out_4327706851715669235[114] = 1.0; - out_4327706851715669235[115] = 0.0; - out_4327706851715669235[116] = 0.0; - out_4327706851715669235[117] = 0.0; - out_4327706851715669235[118] = 0.0; - out_4327706851715669235[119] = 0.0; - out_4327706851715669235[120] = 0.0; - out_4327706851715669235[121] = 0.0; - out_4327706851715669235[122] = 0.0; - out_4327706851715669235[123] = 0.0; - out_4327706851715669235[124] = 0.0; - out_4327706851715669235[125] = 0.0; - out_4327706851715669235[126] = 0.0; - out_4327706851715669235[127] = 0.0; - out_4327706851715669235[128] = 0.0; - out_4327706851715669235[129] = 0.0; - out_4327706851715669235[130] = 0.0; - out_4327706851715669235[131] = 0.0; - out_4327706851715669235[132] = 0.0; - out_4327706851715669235[133] = 1.0; - out_4327706851715669235[134] = 0.0; - out_4327706851715669235[135] = 0.0; - out_4327706851715669235[136] = 0.0; - out_4327706851715669235[137] = 0.0; - out_4327706851715669235[138] = 0.0; - out_4327706851715669235[139] = 0.0; - out_4327706851715669235[140] = 0.0; - out_4327706851715669235[141] = 0.0; - out_4327706851715669235[142] = 0.0; - out_4327706851715669235[143] = 0.0; - out_4327706851715669235[144] = 0.0; - out_4327706851715669235[145] = 0.0; - out_4327706851715669235[146] = 0.0; - out_4327706851715669235[147] = 0.0; - out_4327706851715669235[148] = 0.0; - out_4327706851715669235[149] = 0.0; - out_4327706851715669235[150] = 0.0; - out_4327706851715669235[151] = 0.0; - out_4327706851715669235[152] = 1.0; - out_4327706851715669235[153] = 0.0; - out_4327706851715669235[154] = 0.0; - out_4327706851715669235[155] = 0.0; - out_4327706851715669235[156] = 0.0; - out_4327706851715669235[157] = 0.0; - out_4327706851715669235[158] = 0.0; - out_4327706851715669235[159] = 0.0; - out_4327706851715669235[160] = 0.0; - out_4327706851715669235[161] = 0.0; - out_4327706851715669235[162] = 0.0; - out_4327706851715669235[163] = 0.0; - out_4327706851715669235[164] = 0.0; - out_4327706851715669235[165] = 0.0; - out_4327706851715669235[166] = 0.0; - out_4327706851715669235[167] = 0.0; - out_4327706851715669235[168] = 0.0; - out_4327706851715669235[169] = 0.0; - out_4327706851715669235[170] = 0.0; - out_4327706851715669235[171] = 1.0; - out_4327706851715669235[172] = 0.0; - out_4327706851715669235[173] = 0.0; - out_4327706851715669235[174] = 0.0; - out_4327706851715669235[175] = 0.0; - out_4327706851715669235[176] = 0.0; - out_4327706851715669235[177] = 0.0; - out_4327706851715669235[178] = 0.0; - out_4327706851715669235[179] = 0.0; - out_4327706851715669235[180] = 0.0; - out_4327706851715669235[181] = 0.0; - out_4327706851715669235[182] = 0.0; - out_4327706851715669235[183] = 0.0; - out_4327706851715669235[184] = 0.0; - out_4327706851715669235[185] = 0.0; - out_4327706851715669235[186] = 0.0; - out_4327706851715669235[187] = 0.0; - out_4327706851715669235[188] = 0.0; - out_4327706851715669235[189] = 0.0; - out_4327706851715669235[190] = 1.0; - out_4327706851715669235[191] = 0.0; - out_4327706851715669235[192] = 0.0; - out_4327706851715669235[193] = 0.0; - out_4327706851715669235[194] = 0.0; - out_4327706851715669235[195] = 0.0; - out_4327706851715669235[196] = 0.0; - out_4327706851715669235[197] = 0.0; - out_4327706851715669235[198] = 0.0; - out_4327706851715669235[199] = 0.0; - out_4327706851715669235[200] = 0.0; - out_4327706851715669235[201] = 0.0; - out_4327706851715669235[202] = 0.0; - out_4327706851715669235[203] = 0.0; - out_4327706851715669235[204] = 0.0; - out_4327706851715669235[205] = 0.0; - out_4327706851715669235[206] = 0.0; - out_4327706851715669235[207] = 0.0; - out_4327706851715669235[208] = 0.0; - out_4327706851715669235[209] = 1.0; - out_4327706851715669235[210] = 0.0; - out_4327706851715669235[211] = 0.0; - out_4327706851715669235[212] = 0.0; - out_4327706851715669235[213] = 0.0; - out_4327706851715669235[214] = 0.0; - out_4327706851715669235[215] = 0.0; - out_4327706851715669235[216] = 0.0; - out_4327706851715669235[217] = 0.0; - out_4327706851715669235[218] = 0.0; - out_4327706851715669235[219] = 0.0; - out_4327706851715669235[220] = 0.0; - out_4327706851715669235[221] = 0.0; - out_4327706851715669235[222] = 0.0; - out_4327706851715669235[223] = 0.0; - out_4327706851715669235[224] = 0.0; - out_4327706851715669235[225] = 0.0; - out_4327706851715669235[226] = 0.0; - out_4327706851715669235[227] = 0.0; - out_4327706851715669235[228] = 1.0; - out_4327706851715669235[229] = 0.0; - out_4327706851715669235[230] = 0.0; - out_4327706851715669235[231] = 0.0; - out_4327706851715669235[232] = 0.0; - out_4327706851715669235[233] = 0.0; - out_4327706851715669235[234] = 0.0; - out_4327706851715669235[235] = 0.0; - out_4327706851715669235[236] = 0.0; - out_4327706851715669235[237] = 0.0; - out_4327706851715669235[238] = 0.0; - out_4327706851715669235[239] = 0.0; - out_4327706851715669235[240] = 0.0; - out_4327706851715669235[241] = 0.0; - out_4327706851715669235[242] = 0.0; - out_4327706851715669235[243] = 0.0; - out_4327706851715669235[244] = 0.0; - out_4327706851715669235[245] = 0.0; - out_4327706851715669235[246] = 0.0; - out_4327706851715669235[247] = 1.0; - out_4327706851715669235[248] = 0.0; - out_4327706851715669235[249] = 0.0; - out_4327706851715669235[250] = 0.0; - out_4327706851715669235[251] = 0.0; - out_4327706851715669235[252] = 0.0; - out_4327706851715669235[253] = 0.0; - out_4327706851715669235[254] = 0.0; - out_4327706851715669235[255] = 0.0; - out_4327706851715669235[256] = 0.0; - out_4327706851715669235[257] = 0.0; - out_4327706851715669235[258] = 0.0; - out_4327706851715669235[259] = 0.0; - out_4327706851715669235[260] = 0.0; - out_4327706851715669235[261] = 0.0; - out_4327706851715669235[262] = 0.0; - out_4327706851715669235[263] = 0.0; - out_4327706851715669235[264] = 0.0; - out_4327706851715669235[265] = 0.0; - out_4327706851715669235[266] = 1.0; - out_4327706851715669235[267] = 0.0; - out_4327706851715669235[268] = 0.0; - out_4327706851715669235[269] = 0.0; - out_4327706851715669235[270] = 0.0; - out_4327706851715669235[271] = 0.0; - out_4327706851715669235[272] = 0.0; - out_4327706851715669235[273] = 0.0; - out_4327706851715669235[274] = 0.0; - out_4327706851715669235[275] = 0.0; - out_4327706851715669235[276] = 0.0; - out_4327706851715669235[277] = 0.0; - out_4327706851715669235[278] = 0.0; - out_4327706851715669235[279] = 0.0; - out_4327706851715669235[280] = 0.0; - out_4327706851715669235[281] = 0.0; - out_4327706851715669235[282] = 0.0; - out_4327706851715669235[283] = 0.0; - out_4327706851715669235[284] = 0.0; - out_4327706851715669235[285] = 1.0; - out_4327706851715669235[286] = 0.0; - out_4327706851715669235[287] = 0.0; - out_4327706851715669235[288] = 0.0; - out_4327706851715669235[289] = 0.0; - out_4327706851715669235[290] = 0.0; - out_4327706851715669235[291] = 0.0; - out_4327706851715669235[292] = 0.0; - out_4327706851715669235[293] = 0.0; - out_4327706851715669235[294] = 0.0; - out_4327706851715669235[295] = 0.0; - out_4327706851715669235[296] = 0.0; - out_4327706851715669235[297] = 0.0; - out_4327706851715669235[298] = 0.0; - out_4327706851715669235[299] = 0.0; - out_4327706851715669235[300] = 0.0; - out_4327706851715669235[301] = 0.0; - out_4327706851715669235[302] = 0.0; - out_4327706851715669235[303] = 0.0; - out_4327706851715669235[304] = 1.0; - out_4327706851715669235[305] = 0.0; - out_4327706851715669235[306] = 0.0; - out_4327706851715669235[307] = 0.0; - out_4327706851715669235[308] = 0.0; - out_4327706851715669235[309] = 0.0; - out_4327706851715669235[310] = 0.0; - out_4327706851715669235[311] = 0.0; - out_4327706851715669235[312] = 0.0; - out_4327706851715669235[313] = 0.0; - out_4327706851715669235[314] = 0.0; - out_4327706851715669235[315] = 0.0; - out_4327706851715669235[316] = 0.0; - out_4327706851715669235[317] = 0.0; - out_4327706851715669235[318] = 0.0; - out_4327706851715669235[319] = 0.0; - out_4327706851715669235[320] = 0.0; - out_4327706851715669235[321] = 0.0; - out_4327706851715669235[322] = 0.0; - out_4327706851715669235[323] = 1.0; +void H_mod_fun(double *state, double *out_2820660951987973699) { + out_2820660951987973699[0] = 1.0; + out_2820660951987973699[1] = 0.0; + out_2820660951987973699[2] = 0.0; + out_2820660951987973699[3] = 0.0; + out_2820660951987973699[4] = 0.0; + out_2820660951987973699[5] = 0.0; + out_2820660951987973699[6] = 0.0; + out_2820660951987973699[7] = 0.0; + out_2820660951987973699[8] = 0.0; + out_2820660951987973699[9] = 0.0; + out_2820660951987973699[10] = 0.0; + out_2820660951987973699[11] = 0.0; + out_2820660951987973699[12] = 0.0; + out_2820660951987973699[13] = 0.0; + out_2820660951987973699[14] = 0.0; + out_2820660951987973699[15] = 0.0; + out_2820660951987973699[16] = 0.0; + out_2820660951987973699[17] = 0.0; + out_2820660951987973699[18] = 0.0; + out_2820660951987973699[19] = 1.0; + out_2820660951987973699[20] = 0.0; + out_2820660951987973699[21] = 0.0; + out_2820660951987973699[22] = 0.0; + out_2820660951987973699[23] = 0.0; + out_2820660951987973699[24] = 0.0; + out_2820660951987973699[25] = 0.0; + out_2820660951987973699[26] = 0.0; + out_2820660951987973699[27] = 0.0; + out_2820660951987973699[28] = 0.0; + out_2820660951987973699[29] = 0.0; + out_2820660951987973699[30] = 0.0; + out_2820660951987973699[31] = 0.0; + out_2820660951987973699[32] = 0.0; + out_2820660951987973699[33] = 0.0; + out_2820660951987973699[34] = 0.0; + out_2820660951987973699[35] = 0.0; + out_2820660951987973699[36] = 0.0; + out_2820660951987973699[37] = 0.0; + out_2820660951987973699[38] = 1.0; + out_2820660951987973699[39] = 0.0; + out_2820660951987973699[40] = 0.0; + out_2820660951987973699[41] = 0.0; + out_2820660951987973699[42] = 0.0; + out_2820660951987973699[43] = 0.0; + out_2820660951987973699[44] = 0.0; + out_2820660951987973699[45] = 0.0; + out_2820660951987973699[46] = 0.0; + out_2820660951987973699[47] = 0.0; + out_2820660951987973699[48] = 0.0; + out_2820660951987973699[49] = 0.0; + out_2820660951987973699[50] = 0.0; + out_2820660951987973699[51] = 0.0; + out_2820660951987973699[52] = 0.0; + out_2820660951987973699[53] = 0.0; + out_2820660951987973699[54] = 0.0; + out_2820660951987973699[55] = 0.0; + out_2820660951987973699[56] = 0.0; + out_2820660951987973699[57] = 1.0; + out_2820660951987973699[58] = 0.0; + out_2820660951987973699[59] = 0.0; + out_2820660951987973699[60] = 0.0; + out_2820660951987973699[61] = 0.0; + out_2820660951987973699[62] = 0.0; + out_2820660951987973699[63] = 0.0; + out_2820660951987973699[64] = 0.0; + out_2820660951987973699[65] = 0.0; + out_2820660951987973699[66] = 0.0; + out_2820660951987973699[67] = 0.0; + out_2820660951987973699[68] = 0.0; + out_2820660951987973699[69] = 0.0; + out_2820660951987973699[70] = 0.0; + out_2820660951987973699[71] = 0.0; + out_2820660951987973699[72] = 0.0; + out_2820660951987973699[73] = 0.0; + out_2820660951987973699[74] = 0.0; + out_2820660951987973699[75] = 0.0; + out_2820660951987973699[76] = 1.0; + out_2820660951987973699[77] = 0.0; + out_2820660951987973699[78] = 0.0; + out_2820660951987973699[79] = 0.0; + out_2820660951987973699[80] = 0.0; + out_2820660951987973699[81] = 0.0; + out_2820660951987973699[82] = 0.0; + out_2820660951987973699[83] = 0.0; + out_2820660951987973699[84] = 0.0; + out_2820660951987973699[85] = 0.0; + out_2820660951987973699[86] = 0.0; + out_2820660951987973699[87] = 0.0; + out_2820660951987973699[88] = 0.0; + out_2820660951987973699[89] = 0.0; + out_2820660951987973699[90] = 0.0; + out_2820660951987973699[91] = 0.0; + out_2820660951987973699[92] = 0.0; + out_2820660951987973699[93] = 0.0; + out_2820660951987973699[94] = 0.0; + out_2820660951987973699[95] = 1.0; + out_2820660951987973699[96] = 0.0; + out_2820660951987973699[97] = 0.0; + out_2820660951987973699[98] = 0.0; + out_2820660951987973699[99] = 0.0; + out_2820660951987973699[100] = 0.0; + out_2820660951987973699[101] = 0.0; + out_2820660951987973699[102] = 0.0; + out_2820660951987973699[103] = 0.0; + out_2820660951987973699[104] = 0.0; + out_2820660951987973699[105] = 0.0; + out_2820660951987973699[106] = 0.0; + out_2820660951987973699[107] = 0.0; + out_2820660951987973699[108] = 0.0; + out_2820660951987973699[109] = 0.0; + out_2820660951987973699[110] = 0.0; + out_2820660951987973699[111] = 0.0; + out_2820660951987973699[112] = 0.0; + out_2820660951987973699[113] = 0.0; + out_2820660951987973699[114] = 1.0; + out_2820660951987973699[115] = 0.0; + out_2820660951987973699[116] = 0.0; + out_2820660951987973699[117] = 0.0; + out_2820660951987973699[118] = 0.0; + out_2820660951987973699[119] = 0.0; + out_2820660951987973699[120] = 0.0; + out_2820660951987973699[121] = 0.0; + out_2820660951987973699[122] = 0.0; + out_2820660951987973699[123] = 0.0; + out_2820660951987973699[124] = 0.0; + out_2820660951987973699[125] = 0.0; + out_2820660951987973699[126] = 0.0; + out_2820660951987973699[127] = 0.0; + out_2820660951987973699[128] = 0.0; + out_2820660951987973699[129] = 0.0; + out_2820660951987973699[130] = 0.0; + out_2820660951987973699[131] = 0.0; + out_2820660951987973699[132] = 0.0; + out_2820660951987973699[133] = 1.0; + out_2820660951987973699[134] = 0.0; + out_2820660951987973699[135] = 0.0; + out_2820660951987973699[136] = 0.0; + out_2820660951987973699[137] = 0.0; + out_2820660951987973699[138] = 0.0; + out_2820660951987973699[139] = 0.0; + out_2820660951987973699[140] = 0.0; + out_2820660951987973699[141] = 0.0; + out_2820660951987973699[142] = 0.0; + out_2820660951987973699[143] = 0.0; + out_2820660951987973699[144] = 0.0; + out_2820660951987973699[145] = 0.0; + out_2820660951987973699[146] = 0.0; + out_2820660951987973699[147] = 0.0; + out_2820660951987973699[148] = 0.0; + out_2820660951987973699[149] = 0.0; + out_2820660951987973699[150] = 0.0; + out_2820660951987973699[151] = 0.0; + out_2820660951987973699[152] = 1.0; + out_2820660951987973699[153] = 0.0; + out_2820660951987973699[154] = 0.0; + out_2820660951987973699[155] = 0.0; + out_2820660951987973699[156] = 0.0; + out_2820660951987973699[157] = 0.0; + out_2820660951987973699[158] = 0.0; + out_2820660951987973699[159] = 0.0; + out_2820660951987973699[160] = 0.0; + out_2820660951987973699[161] = 0.0; + out_2820660951987973699[162] = 0.0; + out_2820660951987973699[163] = 0.0; + out_2820660951987973699[164] = 0.0; + out_2820660951987973699[165] = 0.0; + out_2820660951987973699[166] = 0.0; + out_2820660951987973699[167] = 0.0; + out_2820660951987973699[168] = 0.0; + out_2820660951987973699[169] = 0.0; + out_2820660951987973699[170] = 0.0; + out_2820660951987973699[171] = 1.0; + out_2820660951987973699[172] = 0.0; + out_2820660951987973699[173] = 0.0; + out_2820660951987973699[174] = 0.0; + out_2820660951987973699[175] = 0.0; + out_2820660951987973699[176] = 0.0; + out_2820660951987973699[177] = 0.0; + out_2820660951987973699[178] = 0.0; + out_2820660951987973699[179] = 0.0; + out_2820660951987973699[180] = 0.0; + out_2820660951987973699[181] = 0.0; + out_2820660951987973699[182] = 0.0; + out_2820660951987973699[183] = 0.0; + out_2820660951987973699[184] = 0.0; + out_2820660951987973699[185] = 0.0; + out_2820660951987973699[186] = 0.0; + out_2820660951987973699[187] = 0.0; + out_2820660951987973699[188] = 0.0; + out_2820660951987973699[189] = 0.0; + out_2820660951987973699[190] = 1.0; + out_2820660951987973699[191] = 0.0; + out_2820660951987973699[192] = 0.0; + out_2820660951987973699[193] = 0.0; + out_2820660951987973699[194] = 0.0; + out_2820660951987973699[195] = 0.0; + out_2820660951987973699[196] = 0.0; + out_2820660951987973699[197] = 0.0; + out_2820660951987973699[198] = 0.0; + out_2820660951987973699[199] = 0.0; + out_2820660951987973699[200] = 0.0; + out_2820660951987973699[201] = 0.0; + out_2820660951987973699[202] = 0.0; + out_2820660951987973699[203] = 0.0; + out_2820660951987973699[204] = 0.0; + out_2820660951987973699[205] = 0.0; + out_2820660951987973699[206] = 0.0; + out_2820660951987973699[207] = 0.0; + out_2820660951987973699[208] = 0.0; + out_2820660951987973699[209] = 1.0; + out_2820660951987973699[210] = 0.0; + out_2820660951987973699[211] = 0.0; + out_2820660951987973699[212] = 0.0; + out_2820660951987973699[213] = 0.0; + out_2820660951987973699[214] = 0.0; + out_2820660951987973699[215] = 0.0; + out_2820660951987973699[216] = 0.0; + out_2820660951987973699[217] = 0.0; + out_2820660951987973699[218] = 0.0; + out_2820660951987973699[219] = 0.0; + out_2820660951987973699[220] = 0.0; + out_2820660951987973699[221] = 0.0; + out_2820660951987973699[222] = 0.0; + out_2820660951987973699[223] = 0.0; + out_2820660951987973699[224] = 0.0; + out_2820660951987973699[225] = 0.0; + out_2820660951987973699[226] = 0.0; + out_2820660951987973699[227] = 0.0; + out_2820660951987973699[228] = 1.0; + out_2820660951987973699[229] = 0.0; + out_2820660951987973699[230] = 0.0; + out_2820660951987973699[231] = 0.0; + out_2820660951987973699[232] = 0.0; + out_2820660951987973699[233] = 0.0; + out_2820660951987973699[234] = 0.0; + out_2820660951987973699[235] = 0.0; + out_2820660951987973699[236] = 0.0; + out_2820660951987973699[237] = 0.0; + out_2820660951987973699[238] = 0.0; + out_2820660951987973699[239] = 0.0; + out_2820660951987973699[240] = 0.0; + out_2820660951987973699[241] = 0.0; + out_2820660951987973699[242] = 0.0; + out_2820660951987973699[243] = 0.0; + out_2820660951987973699[244] = 0.0; + out_2820660951987973699[245] = 0.0; + out_2820660951987973699[246] = 0.0; + out_2820660951987973699[247] = 1.0; + out_2820660951987973699[248] = 0.0; + out_2820660951987973699[249] = 0.0; + out_2820660951987973699[250] = 0.0; + out_2820660951987973699[251] = 0.0; + out_2820660951987973699[252] = 0.0; + out_2820660951987973699[253] = 0.0; + out_2820660951987973699[254] = 0.0; + out_2820660951987973699[255] = 0.0; + out_2820660951987973699[256] = 0.0; + out_2820660951987973699[257] = 0.0; + out_2820660951987973699[258] = 0.0; + out_2820660951987973699[259] = 0.0; + out_2820660951987973699[260] = 0.0; + out_2820660951987973699[261] = 0.0; + out_2820660951987973699[262] = 0.0; + out_2820660951987973699[263] = 0.0; + out_2820660951987973699[264] = 0.0; + out_2820660951987973699[265] = 0.0; + out_2820660951987973699[266] = 1.0; + out_2820660951987973699[267] = 0.0; + out_2820660951987973699[268] = 0.0; + out_2820660951987973699[269] = 0.0; + out_2820660951987973699[270] = 0.0; + out_2820660951987973699[271] = 0.0; + out_2820660951987973699[272] = 0.0; + out_2820660951987973699[273] = 0.0; + out_2820660951987973699[274] = 0.0; + out_2820660951987973699[275] = 0.0; + out_2820660951987973699[276] = 0.0; + out_2820660951987973699[277] = 0.0; + out_2820660951987973699[278] = 0.0; + out_2820660951987973699[279] = 0.0; + out_2820660951987973699[280] = 0.0; + out_2820660951987973699[281] = 0.0; + out_2820660951987973699[282] = 0.0; + out_2820660951987973699[283] = 0.0; + out_2820660951987973699[284] = 0.0; + out_2820660951987973699[285] = 1.0; + out_2820660951987973699[286] = 0.0; + out_2820660951987973699[287] = 0.0; + out_2820660951987973699[288] = 0.0; + out_2820660951987973699[289] = 0.0; + out_2820660951987973699[290] = 0.0; + out_2820660951987973699[291] = 0.0; + out_2820660951987973699[292] = 0.0; + out_2820660951987973699[293] = 0.0; + out_2820660951987973699[294] = 0.0; + out_2820660951987973699[295] = 0.0; + out_2820660951987973699[296] = 0.0; + out_2820660951987973699[297] = 0.0; + out_2820660951987973699[298] = 0.0; + out_2820660951987973699[299] = 0.0; + out_2820660951987973699[300] = 0.0; + out_2820660951987973699[301] = 0.0; + out_2820660951987973699[302] = 0.0; + out_2820660951987973699[303] = 0.0; + out_2820660951987973699[304] = 1.0; + out_2820660951987973699[305] = 0.0; + out_2820660951987973699[306] = 0.0; + out_2820660951987973699[307] = 0.0; + out_2820660951987973699[308] = 0.0; + out_2820660951987973699[309] = 0.0; + out_2820660951987973699[310] = 0.0; + out_2820660951987973699[311] = 0.0; + out_2820660951987973699[312] = 0.0; + out_2820660951987973699[313] = 0.0; + out_2820660951987973699[314] = 0.0; + out_2820660951987973699[315] = 0.0; + out_2820660951987973699[316] = 0.0; + out_2820660951987973699[317] = 0.0; + out_2820660951987973699[318] = 0.0; + out_2820660951987973699[319] = 0.0; + out_2820660951987973699[320] = 0.0; + out_2820660951987973699[321] = 0.0; + out_2820660951987973699[322] = 0.0; + out_2820660951987973699[323] = 1.0; } -void f_fun(double *state, double dt, double *out_497730658810754078) { - out_497730658810754078[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_497730658810754078[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_497730658810754078[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_497730658810754078[3] = dt*state[12] + state[3]; - out_497730658810754078[4] = dt*state[13] + state[4]; - out_497730658810754078[5] = dt*state[14] + state[5]; - out_497730658810754078[6] = state[6]; - out_497730658810754078[7] = state[7]; - out_497730658810754078[8] = state[8]; - out_497730658810754078[9] = state[9]; - out_497730658810754078[10] = state[10]; - out_497730658810754078[11] = state[11]; - out_497730658810754078[12] = state[12]; - out_497730658810754078[13] = state[13]; - out_497730658810754078[14] = state[14]; - out_497730658810754078[15] = state[15]; - out_497730658810754078[16] = state[16]; - out_497730658810754078[17] = state[17]; +void f_fun(double *state, double dt, double *out_2310584025056258416) { + out_2310584025056258416[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); + out_2310584025056258416[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); + out_2310584025056258416[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); + out_2310584025056258416[3] = dt*state[12] + state[3]; + out_2310584025056258416[4] = dt*state[13] + state[4]; + out_2310584025056258416[5] = dt*state[14] + state[5]; + out_2310584025056258416[6] = state[6]; + out_2310584025056258416[7] = state[7]; + out_2310584025056258416[8] = state[8]; + out_2310584025056258416[9] = state[9]; + out_2310584025056258416[10] = state[10]; + out_2310584025056258416[11] = state[11]; + out_2310584025056258416[12] = state[12]; + out_2310584025056258416[13] = state[13]; + out_2310584025056258416[14] = state[14]; + out_2310584025056258416[15] = state[15]; + out_2310584025056258416[16] = state[16]; + out_2310584025056258416[17] = state[17]; } -void F_fun(double *state, double dt, double *out_6088738551748760158) { - out_6088738551748760158[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_6088738551748760158[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_6088738551748760158[2] = 0; - out_6088738551748760158[3] = 0; - out_6088738551748760158[4] = 0; - out_6088738551748760158[5] = 0; - out_6088738551748760158[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_6088738551748760158[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_6088738551748760158[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_6088738551748760158[9] = 0; - out_6088738551748760158[10] = 0; - out_6088738551748760158[11] = 0; - out_6088738551748760158[12] = 0; - out_6088738551748760158[13] = 0; - out_6088738551748760158[14] = 0; - out_6088738551748760158[15] = 0; - out_6088738551748760158[16] = 0; - out_6088738551748760158[17] = 0; - out_6088738551748760158[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_6088738551748760158[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_6088738551748760158[20] = 0; - out_6088738551748760158[21] = 0; - out_6088738551748760158[22] = 0; - out_6088738551748760158[23] = 0; - out_6088738551748760158[24] = 0; - out_6088738551748760158[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_6088738551748760158[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_6088738551748760158[27] = 0; - out_6088738551748760158[28] = 0; - out_6088738551748760158[29] = 0; - out_6088738551748760158[30] = 0; - out_6088738551748760158[31] = 0; - out_6088738551748760158[32] = 0; - out_6088738551748760158[33] = 0; - out_6088738551748760158[34] = 0; - out_6088738551748760158[35] = 0; - out_6088738551748760158[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_6088738551748760158[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_6088738551748760158[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_6088738551748760158[39] = 0; - out_6088738551748760158[40] = 0; - out_6088738551748760158[41] = 0; - out_6088738551748760158[42] = 0; - out_6088738551748760158[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_6088738551748760158[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_6088738551748760158[45] = 0; - out_6088738551748760158[46] = 0; - out_6088738551748760158[47] = 0; - out_6088738551748760158[48] = 0; - out_6088738551748760158[49] = 0; - out_6088738551748760158[50] = 0; - out_6088738551748760158[51] = 0; - out_6088738551748760158[52] = 0; - out_6088738551748760158[53] = 0; - out_6088738551748760158[54] = 0; - out_6088738551748760158[55] = 0; - out_6088738551748760158[56] = 0; - out_6088738551748760158[57] = 1; - out_6088738551748760158[58] = 0; - out_6088738551748760158[59] = 0; - out_6088738551748760158[60] = 0; - out_6088738551748760158[61] = 0; - out_6088738551748760158[62] = 0; - out_6088738551748760158[63] = 0; - out_6088738551748760158[64] = 0; - out_6088738551748760158[65] = 0; - out_6088738551748760158[66] = dt; - out_6088738551748760158[67] = 0; - out_6088738551748760158[68] = 0; - out_6088738551748760158[69] = 0; - out_6088738551748760158[70] = 0; - out_6088738551748760158[71] = 0; - out_6088738551748760158[72] = 0; - out_6088738551748760158[73] = 0; - out_6088738551748760158[74] = 0; - out_6088738551748760158[75] = 0; - out_6088738551748760158[76] = 1; - out_6088738551748760158[77] = 0; - out_6088738551748760158[78] = 0; - out_6088738551748760158[79] = 0; - out_6088738551748760158[80] = 0; - out_6088738551748760158[81] = 0; - out_6088738551748760158[82] = 0; - out_6088738551748760158[83] = 0; - out_6088738551748760158[84] = 0; - out_6088738551748760158[85] = dt; - out_6088738551748760158[86] = 0; - out_6088738551748760158[87] = 0; - out_6088738551748760158[88] = 0; - out_6088738551748760158[89] = 0; - out_6088738551748760158[90] = 0; - out_6088738551748760158[91] = 0; - out_6088738551748760158[92] = 0; - out_6088738551748760158[93] = 0; - out_6088738551748760158[94] = 0; - out_6088738551748760158[95] = 1; - out_6088738551748760158[96] = 0; - out_6088738551748760158[97] = 0; - out_6088738551748760158[98] = 0; - out_6088738551748760158[99] = 0; - out_6088738551748760158[100] = 0; - out_6088738551748760158[101] = 0; - out_6088738551748760158[102] = 0; - out_6088738551748760158[103] = 0; - out_6088738551748760158[104] = dt; - out_6088738551748760158[105] = 0; - out_6088738551748760158[106] = 0; - out_6088738551748760158[107] = 0; - out_6088738551748760158[108] = 0; - out_6088738551748760158[109] = 0; - out_6088738551748760158[110] = 0; - out_6088738551748760158[111] = 0; - out_6088738551748760158[112] = 0; - out_6088738551748760158[113] = 0; - out_6088738551748760158[114] = 1; - out_6088738551748760158[115] = 0; - out_6088738551748760158[116] = 0; - out_6088738551748760158[117] = 0; - out_6088738551748760158[118] = 0; - out_6088738551748760158[119] = 0; - out_6088738551748760158[120] = 0; - out_6088738551748760158[121] = 0; - out_6088738551748760158[122] = 0; - out_6088738551748760158[123] = 0; - out_6088738551748760158[124] = 0; - out_6088738551748760158[125] = 0; - out_6088738551748760158[126] = 0; - out_6088738551748760158[127] = 0; - out_6088738551748760158[128] = 0; - out_6088738551748760158[129] = 0; - out_6088738551748760158[130] = 0; - out_6088738551748760158[131] = 0; - out_6088738551748760158[132] = 0; - out_6088738551748760158[133] = 1; - out_6088738551748760158[134] = 0; - out_6088738551748760158[135] = 0; - out_6088738551748760158[136] = 0; - out_6088738551748760158[137] = 0; - out_6088738551748760158[138] = 0; - out_6088738551748760158[139] = 0; - out_6088738551748760158[140] = 0; - out_6088738551748760158[141] = 0; - out_6088738551748760158[142] = 0; - out_6088738551748760158[143] = 0; - out_6088738551748760158[144] = 0; - out_6088738551748760158[145] = 0; - out_6088738551748760158[146] = 0; - out_6088738551748760158[147] = 0; - out_6088738551748760158[148] = 0; - out_6088738551748760158[149] = 0; - out_6088738551748760158[150] = 0; - out_6088738551748760158[151] = 0; - out_6088738551748760158[152] = 1; - out_6088738551748760158[153] = 0; - out_6088738551748760158[154] = 0; - out_6088738551748760158[155] = 0; - out_6088738551748760158[156] = 0; - out_6088738551748760158[157] = 0; - out_6088738551748760158[158] = 0; - out_6088738551748760158[159] = 0; - out_6088738551748760158[160] = 0; - out_6088738551748760158[161] = 0; - out_6088738551748760158[162] = 0; - out_6088738551748760158[163] = 0; - out_6088738551748760158[164] = 0; - out_6088738551748760158[165] = 0; - out_6088738551748760158[166] = 0; - out_6088738551748760158[167] = 0; - out_6088738551748760158[168] = 0; - out_6088738551748760158[169] = 0; - out_6088738551748760158[170] = 0; - out_6088738551748760158[171] = 1; - out_6088738551748760158[172] = 0; - out_6088738551748760158[173] = 0; - out_6088738551748760158[174] = 0; - out_6088738551748760158[175] = 0; - out_6088738551748760158[176] = 0; - out_6088738551748760158[177] = 0; - out_6088738551748760158[178] = 0; - out_6088738551748760158[179] = 0; - out_6088738551748760158[180] = 0; - out_6088738551748760158[181] = 0; - out_6088738551748760158[182] = 0; - out_6088738551748760158[183] = 0; - out_6088738551748760158[184] = 0; - out_6088738551748760158[185] = 0; - out_6088738551748760158[186] = 0; - out_6088738551748760158[187] = 0; - out_6088738551748760158[188] = 0; - out_6088738551748760158[189] = 0; - out_6088738551748760158[190] = 1; - out_6088738551748760158[191] = 0; - out_6088738551748760158[192] = 0; - out_6088738551748760158[193] = 0; - out_6088738551748760158[194] = 0; - out_6088738551748760158[195] = 0; - out_6088738551748760158[196] = 0; - out_6088738551748760158[197] = 0; - out_6088738551748760158[198] = 0; - out_6088738551748760158[199] = 0; - out_6088738551748760158[200] = 0; - out_6088738551748760158[201] = 0; - out_6088738551748760158[202] = 0; - out_6088738551748760158[203] = 0; - out_6088738551748760158[204] = 0; - out_6088738551748760158[205] = 0; - out_6088738551748760158[206] = 0; - out_6088738551748760158[207] = 0; - out_6088738551748760158[208] = 0; - out_6088738551748760158[209] = 1; - out_6088738551748760158[210] = 0; - out_6088738551748760158[211] = 0; - out_6088738551748760158[212] = 0; - out_6088738551748760158[213] = 0; - out_6088738551748760158[214] = 0; - out_6088738551748760158[215] = 0; - out_6088738551748760158[216] = 0; - out_6088738551748760158[217] = 0; - out_6088738551748760158[218] = 0; - out_6088738551748760158[219] = 0; - out_6088738551748760158[220] = 0; - out_6088738551748760158[221] = 0; - out_6088738551748760158[222] = 0; - out_6088738551748760158[223] = 0; - out_6088738551748760158[224] = 0; - out_6088738551748760158[225] = 0; - out_6088738551748760158[226] = 0; - out_6088738551748760158[227] = 0; - out_6088738551748760158[228] = 1; - out_6088738551748760158[229] = 0; - out_6088738551748760158[230] = 0; - out_6088738551748760158[231] = 0; - out_6088738551748760158[232] = 0; - out_6088738551748760158[233] = 0; - out_6088738551748760158[234] = 0; - out_6088738551748760158[235] = 0; - out_6088738551748760158[236] = 0; - out_6088738551748760158[237] = 0; - out_6088738551748760158[238] = 0; - out_6088738551748760158[239] = 0; - out_6088738551748760158[240] = 0; - out_6088738551748760158[241] = 0; - out_6088738551748760158[242] = 0; - out_6088738551748760158[243] = 0; - out_6088738551748760158[244] = 0; - out_6088738551748760158[245] = 0; - out_6088738551748760158[246] = 0; - out_6088738551748760158[247] = 1; - out_6088738551748760158[248] = 0; - out_6088738551748760158[249] = 0; - out_6088738551748760158[250] = 0; - out_6088738551748760158[251] = 0; - out_6088738551748760158[252] = 0; - out_6088738551748760158[253] = 0; - out_6088738551748760158[254] = 0; - out_6088738551748760158[255] = 0; - out_6088738551748760158[256] = 0; - out_6088738551748760158[257] = 0; - out_6088738551748760158[258] = 0; - out_6088738551748760158[259] = 0; - out_6088738551748760158[260] = 0; - out_6088738551748760158[261] = 0; - out_6088738551748760158[262] = 0; - out_6088738551748760158[263] = 0; - out_6088738551748760158[264] = 0; - out_6088738551748760158[265] = 0; - out_6088738551748760158[266] = 1; - out_6088738551748760158[267] = 0; - out_6088738551748760158[268] = 0; - out_6088738551748760158[269] = 0; - out_6088738551748760158[270] = 0; - out_6088738551748760158[271] = 0; - out_6088738551748760158[272] = 0; - out_6088738551748760158[273] = 0; - out_6088738551748760158[274] = 0; - out_6088738551748760158[275] = 0; - out_6088738551748760158[276] = 0; - out_6088738551748760158[277] = 0; - out_6088738551748760158[278] = 0; - out_6088738551748760158[279] = 0; - out_6088738551748760158[280] = 0; - out_6088738551748760158[281] = 0; - out_6088738551748760158[282] = 0; - out_6088738551748760158[283] = 0; - out_6088738551748760158[284] = 0; - out_6088738551748760158[285] = 1; - out_6088738551748760158[286] = 0; - out_6088738551748760158[287] = 0; - out_6088738551748760158[288] = 0; - out_6088738551748760158[289] = 0; - out_6088738551748760158[290] = 0; - out_6088738551748760158[291] = 0; - out_6088738551748760158[292] = 0; - out_6088738551748760158[293] = 0; - out_6088738551748760158[294] = 0; - out_6088738551748760158[295] = 0; - out_6088738551748760158[296] = 0; - out_6088738551748760158[297] = 0; - out_6088738551748760158[298] = 0; - out_6088738551748760158[299] = 0; - out_6088738551748760158[300] = 0; - out_6088738551748760158[301] = 0; - out_6088738551748760158[302] = 0; - out_6088738551748760158[303] = 0; - out_6088738551748760158[304] = 1; - out_6088738551748760158[305] = 0; - out_6088738551748760158[306] = 0; - out_6088738551748760158[307] = 0; - out_6088738551748760158[308] = 0; - out_6088738551748760158[309] = 0; - out_6088738551748760158[310] = 0; - out_6088738551748760158[311] = 0; - out_6088738551748760158[312] = 0; - out_6088738551748760158[313] = 0; - out_6088738551748760158[314] = 0; - out_6088738551748760158[315] = 0; - out_6088738551748760158[316] = 0; - out_6088738551748760158[317] = 0; - out_6088738551748760158[318] = 0; - out_6088738551748760158[319] = 0; - out_6088738551748760158[320] = 0; - out_6088738551748760158[321] = 0; - out_6088738551748760158[322] = 0; - out_6088738551748760158[323] = 1; +void F_fun(double *state, double dt, double *out_6557190266781458971) { + out_6557190266781458971[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6557190266781458971[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6557190266781458971[2] = 0; + out_6557190266781458971[3] = 0; + out_6557190266781458971[4] = 0; + out_6557190266781458971[5] = 0; + out_6557190266781458971[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6557190266781458971[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6557190266781458971[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6557190266781458971[9] = 0; + out_6557190266781458971[10] = 0; + out_6557190266781458971[11] = 0; + out_6557190266781458971[12] = 0; + out_6557190266781458971[13] = 0; + out_6557190266781458971[14] = 0; + out_6557190266781458971[15] = 0; + out_6557190266781458971[16] = 0; + out_6557190266781458971[17] = 0; + out_6557190266781458971[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6557190266781458971[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6557190266781458971[20] = 0; + out_6557190266781458971[21] = 0; + out_6557190266781458971[22] = 0; + out_6557190266781458971[23] = 0; + out_6557190266781458971[24] = 0; + out_6557190266781458971[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6557190266781458971[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6557190266781458971[27] = 0; + out_6557190266781458971[28] = 0; + out_6557190266781458971[29] = 0; + out_6557190266781458971[30] = 0; + out_6557190266781458971[31] = 0; + out_6557190266781458971[32] = 0; + out_6557190266781458971[33] = 0; + out_6557190266781458971[34] = 0; + out_6557190266781458971[35] = 0; + out_6557190266781458971[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6557190266781458971[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6557190266781458971[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6557190266781458971[39] = 0; + out_6557190266781458971[40] = 0; + out_6557190266781458971[41] = 0; + out_6557190266781458971[42] = 0; + out_6557190266781458971[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6557190266781458971[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6557190266781458971[45] = 0; + out_6557190266781458971[46] = 0; + out_6557190266781458971[47] = 0; + out_6557190266781458971[48] = 0; + out_6557190266781458971[49] = 0; + out_6557190266781458971[50] = 0; + out_6557190266781458971[51] = 0; + out_6557190266781458971[52] = 0; + out_6557190266781458971[53] = 0; + out_6557190266781458971[54] = 0; + out_6557190266781458971[55] = 0; + out_6557190266781458971[56] = 0; + out_6557190266781458971[57] = 1; + out_6557190266781458971[58] = 0; + out_6557190266781458971[59] = 0; + out_6557190266781458971[60] = 0; + out_6557190266781458971[61] = 0; + out_6557190266781458971[62] = 0; + out_6557190266781458971[63] = 0; + out_6557190266781458971[64] = 0; + out_6557190266781458971[65] = 0; + out_6557190266781458971[66] = dt; + out_6557190266781458971[67] = 0; + out_6557190266781458971[68] = 0; + out_6557190266781458971[69] = 0; + out_6557190266781458971[70] = 0; + out_6557190266781458971[71] = 0; + out_6557190266781458971[72] = 0; + out_6557190266781458971[73] = 0; + out_6557190266781458971[74] = 0; + out_6557190266781458971[75] = 0; + out_6557190266781458971[76] = 1; + out_6557190266781458971[77] = 0; + out_6557190266781458971[78] = 0; + out_6557190266781458971[79] = 0; + out_6557190266781458971[80] = 0; + out_6557190266781458971[81] = 0; + out_6557190266781458971[82] = 0; + out_6557190266781458971[83] = 0; + out_6557190266781458971[84] = 0; + out_6557190266781458971[85] = dt; + out_6557190266781458971[86] = 0; + out_6557190266781458971[87] = 0; + out_6557190266781458971[88] = 0; + out_6557190266781458971[89] = 0; + out_6557190266781458971[90] = 0; + out_6557190266781458971[91] = 0; + out_6557190266781458971[92] = 0; + out_6557190266781458971[93] = 0; + out_6557190266781458971[94] = 0; + out_6557190266781458971[95] = 1; + out_6557190266781458971[96] = 0; + out_6557190266781458971[97] = 0; + out_6557190266781458971[98] = 0; + out_6557190266781458971[99] = 0; + out_6557190266781458971[100] = 0; + out_6557190266781458971[101] = 0; + out_6557190266781458971[102] = 0; + out_6557190266781458971[103] = 0; + out_6557190266781458971[104] = dt; + out_6557190266781458971[105] = 0; + out_6557190266781458971[106] = 0; + out_6557190266781458971[107] = 0; + out_6557190266781458971[108] = 0; + out_6557190266781458971[109] = 0; + out_6557190266781458971[110] = 0; + out_6557190266781458971[111] = 0; + out_6557190266781458971[112] = 0; + out_6557190266781458971[113] = 0; + out_6557190266781458971[114] = 1; + out_6557190266781458971[115] = 0; + out_6557190266781458971[116] = 0; + out_6557190266781458971[117] = 0; + out_6557190266781458971[118] = 0; + out_6557190266781458971[119] = 0; + out_6557190266781458971[120] = 0; + out_6557190266781458971[121] = 0; + out_6557190266781458971[122] = 0; + out_6557190266781458971[123] = 0; + out_6557190266781458971[124] = 0; + out_6557190266781458971[125] = 0; + out_6557190266781458971[126] = 0; + out_6557190266781458971[127] = 0; + out_6557190266781458971[128] = 0; + out_6557190266781458971[129] = 0; + out_6557190266781458971[130] = 0; + out_6557190266781458971[131] = 0; + out_6557190266781458971[132] = 0; + out_6557190266781458971[133] = 1; + out_6557190266781458971[134] = 0; + out_6557190266781458971[135] = 0; + out_6557190266781458971[136] = 0; + out_6557190266781458971[137] = 0; + out_6557190266781458971[138] = 0; + out_6557190266781458971[139] = 0; + out_6557190266781458971[140] = 0; + out_6557190266781458971[141] = 0; + out_6557190266781458971[142] = 0; + out_6557190266781458971[143] = 0; + out_6557190266781458971[144] = 0; + out_6557190266781458971[145] = 0; + out_6557190266781458971[146] = 0; + out_6557190266781458971[147] = 0; + out_6557190266781458971[148] = 0; + out_6557190266781458971[149] = 0; + out_6557190266781458971[150] = 0; + out_6557190266781458971[151] = 0; + out_6557190266781458971[152] = 1; + out_6557190266781458971[153] = 0; + out_6557190266781458971[154] = 0; + out_6557190266781458971[155] = 0; + out_6557190266781458971[156] = 0; + out_6557190266781458971[157] = 0; + out_6557190266781458971[158] = 0; + out_6557190266781458971[159] = 0; + out_6557190266781458971[160] = 0; + out_6557190266781458971[161] = 0; + out_6557190266781458971[162] = 0; + out_6557190266781458971[163] = 0; + out_6557190266781458971[164] = 0; + out_6557190266781458971[165] = 0; + out_6557190266781458971[166] = 0; + out_6557190266781458971[167] = 0; + out_6557190266781458971[168] = 0; + out_6557190266781458971[169] = 0; + out_6557190266781458971[170] = 0; + out_6557190266781458971[171] = 1; + out_6557190266781458971[172] = 0; + out_6557190266781458971[173] = 0; + out_6557190266781458971[174] = 0; + out_6557190266781458971[175] = 0; + out_6557190266781458971[176] = 0; + out_6557190266781458971[177] = 0; + out_6557190266781458971[178] = 0; + out_6557190266781458971[179] = 0; + out_6557190266781458971[180] = 0; + out_6557190266781458971[181] = 0; + out_6557190266781458971[182] = 0; + out_6557190266781458971[183] = 0; + out_6557190266781458971[184] = 0; + out_6557190266781458971[185] = 0; + out_6557190266781458971[186] = 0; + out_6557190266781458971[187] = 0; + out_6557190266781458971[188] = 0; + out_6557190266781458971[189] = 0; + out_6557190266781458971[190] = 1; + out_6557190266781458971[191] = 0; + out_6557190266781458971[192] = 0; + out_6557190266781458971[193] = 0; + out_6557190266781458971[194] = 0; + out_6557190266781458971[195] = 0; + out_6557190266781458971[196] = 0; + out_6557190266781458971[197] = 0; + out_6557190266781458971[198] = 0; + out_6557190266781458971[199] = 0; + out_6557190266781458971[200] = 0; + out_6557190266781458971[201] = 0; + out_6557190266781458971[202] = 0; + out_6557190266781458971[203] = 0; + out_6557190266781458971[204] = 0; + out_6557190266781458971[205] = 0; + out_6557190266781458971[206] = 0; + out_6557190266781458971[207] = 0; + out_6557190266781458971[208] = 0; + out_6557190266781458971[209] = 1; + out_6557190266781458971[210] = 0; + out_6557190266781458971[211] = 0; + out_6557190266781458971[212] = 0; + out_6557190266781458971[213] = 0; + out_6557190266781458971[214] = 0; + out_6557190266781458971[215] = 0; + out_6557190266781458971[216] = 0; + out_6557190266781458971[217] = 0; + out_6557190266781458971[218] = 0; + out_6557190266781458971[219] = 0; + out_6557190266781458971[220] = 0; + out_6557190266781458971[221] = 0; + out_6557190266781458971[222] = 0; + out_6557190266781458971[223] = 0; + out_6557190266781458971[224] = 0; + out_6557190266781458971[225] = 0; + out_6557190266781458971[226] = 0; + out_6557190266781458971[227] = 0; + out_6557190266781458971[228] = 1; + out_6557190266781458971[229] = 0; + out_6557190266781458971[230] = 0; + out_6557190266781458971[231] = 0; + out_6557190266781458971[232] = 0; + out_6557190266781458971[233] = 0; + out_6557190266781458971[234] = 0; + out_6557190266781458971[235] = 0; + out_6557190266781458971[236] = 0; + out_6557190266781458971[237] = 0; + out_6557190266781458971[238] = 0; + out_6557190266781458971[239] = 0; + out_6557190266781458971[240] = 0; + out_6557190266781458971[241] = 0; + out_6557190266781458971[242] = 0; + out_6557190266781458971[243] = 0; + out_6557190266781458971[244] = 0; + out_6557190266781458971[245] = 0; + out_6557190266781458971[246] = 0; + out_6557190266781458971[247] = 1; + out_6557190266781458971[248] = 0; + out_6557190266781458971[249] = 0; + out_6557190266781458971[250] = 0; + out_6557190266781458971[251] = 0; + out_6557190266781458971[252] = 0; + out_6557190266781458971[253] = 0; + out_6557190266781458971[254] = 0; + out_6557190266781458971[255] = 0; + out_6557190266781458971[256] = 0; + out_6557190266781458971[257] = 0; + out_6557190266781458971[258] = 0; + out_6557190266781458971[259] = 0; + out_6557190266781458971[260] = 0; + out_6557190266781458971[261] = 0; + out_6557190266781458971[262] = 0; + out_6557190266781458971[263] = 0; + out_6557190266781458971[264] = 0; + out_6557190266781458971[265] = 0; + out_6557190266781458971[266] = 1; + out_6557190266781458971[267] = 0; + out_6557190266781458971[268] = 0; + out_6557190266781458971[269] = 0; + out_6557190266781458971[270] = 0; + out_6557190266781458971[271] = 0; + out_6557190266781458971[272] = 0; + out_6557190266781458971[273] = 0; + out_6557190266781458971[274] = 0; + out_6557190266781458971[275] = 0; + out_6557190266781458971[276] = 0; + out_6557190266781458971[277] = 0; + out_6557190266781458971[278] = 0; + out_6557190266781458971[279] = 0; + out_6557190266781458971[280] = 0; + out_6557190266781458971[281] = 0; + out_6557190266781458971[282] = 0; + out_6557190266781458971[283] = 0; + out_6557190266781458971[284] = 0; + out_6557190266781458971[285] = 1; + out_6557190266781458971[286] = 0; + out_6557190266781458971[287] = 0; + out_6557190266781458971[288] = 0; + out_6557190266781458971[289] = 0; + out_6557190266781458971[290] = 0; + out_6557190266781458971[291] = 0; + out_6557190266781458971[292] = 0; + out_6557190266781458971[293] = 0; + out_6557190266781458971[294] = 0; + out_6557190266781458971[295] = 0; + out_6557190266781458971[296] = 0; + out_6557190266781458971[297] = 0; + out_6557190266781458971[298] = 0; + out_6557190266781458971[299] = 0; + out_6557190266781458971[300] = 0; + out_6557190266781458971[301] = 0; + out_6557190266781458971[302] = 0; + out_6557190266781458971[303] = 0; + out_6557190266781458971[304] = 1; + out_6557190266781458971[305] = 0; + out_6557190266781458971[306] = 0; + out_6557190266781458971[307] = 0; + out_6557190266781458971[308] = 0; + out_6557190266781458971[309] = 0; + out_6557190266781458971[310] = 0; + out_6557190266781458971[311] = 0; + out_6557190266781458971[312] = 0; + out_6557190266781458971[313] = 0; + out_6557190266781458971[314] = 0; + out_6557190266781458971[315] = 0; + out_6557190266781458971[316] = 0; + out_6557190266781458971[317] = 0; + out_6557190266781458971[318] = 0; + out_6557190266781458971[319] = 0; + out_6557190266781458971[320] = 0; + out_6557190266781458971[321] = 0; + out_6557190266781458971[322] = 0; + out_6557190266781458971[323] = 1; } -void h_4(double *state, double *unused, double *out_2742355909746826294) { - out_2742355909746826294[0] = state[6] + state[9]; - out_2742355909746826294[1] = state[7] + state[10]; - out_2742355909746826294[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_8177424571477984765) { + out_8177424571477984765[0] = state[6] + state[9]; + out_8177424571477984765[1] = state[7] + state[10]; + out_8177424571477984765[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3203571547522250360) { - out_3203571547522250360[0] = 0; - out_3203571547522250360[1] = 0; - out_3203571547522250360[2] = 0; - out_3203571547522250360[3] = 0; - out_3203571547522250360[4] = 0; - out_3203571547522250360[5] = 0; - out_3203571547522250360[6] = 1; - out_3203571547522250360[7] = 0; - out_3203571547522250360[8] = 0; - out_3203571547522250360[9] = 1; - out_3203571547522250360[10] = 0; - out_3203571547522250360[11] = 0; - out_3203571547522250360[12] = 0; - out_3203571547522250360[13] = 0; - out_3203571547522250360[14] = 0; - out_3203571547522250360[15] = 0; - out_3203571547522250360[16] = 0; - out_3203571547522250360[17] = 0; - out_3203571547522250360[18] = 0; - out_3203571547522250360[19] = 0; - out_3203571547522250360[20] = 0; - out_3203571547522250360[21] = 0; - out_3203571547522250360[22] = 0; - out_3203571547522250360[23] = 0; - out_3203571547522250360[24] = 0; - out_3203571547522250360[25] = 1; - out_3203571547522250360[26] = 0; - out_3203571547522250360[27] = 0; - out_3203571547522250360[28] = 1; - out_3203571547522250360[29] = 0; - out_3203571547522250360[30] = 0; - out_3203571547522250360[31] = 0; - out_3203571547522250360[32] = 0; - out_3203571547522250360[33] = 0; - out_3203571547522250360[34] = 0; - out_3203571547522250360[35] = 0; - out_3203571547522250360[36] = 0; - out_3203571547522250360[37] = 0; - out_3203571547522250360[38] = 0; - out_3203571547522250360[39] = 0; - out_3203571547522250360[40] = 0; - out_3203571547522250360[41] = 0; - out_3203571547522250360[42] = 0; - out_3203571547522250360[43] = 0; - out_3203571547522250360[44] = 1; - out_3203571547522250360[45] = 0; - out_3203571547522250360[46] = 0; - out_3203571547522250360[47] = 1; - out_3203571547522250360[48] = 0; - out_3203571547522250360[49] = 0; - out_3203571547522250360[50] = 0; - out_3203571547522250360[51] = 0; - out_3203571547522250360[52] = 0; - out_3203571547522250360[53] = 0; +void H_4(double *state, double *unused, double *out_6592468161831881271) { + out_6592468161831881271[0] = 0; + out_6592468161831881271[1] = 0; + out_6592468161831881271[2] = 0; + out_6592468161831881271[3] = 0; + out_6592468161831881271[4] = 0; + out_6592468161831881271[5] = 0; + out_6592468161831881271[6] = 1; + out_6592468161831881271[7] = 0; + out_6592468161831881271[8] = 0; + out_6592468161831881271[9] = 1; + out_6592468161831881271[10] = 0; + out_6592468161831881271[11] = 0; + out_6592468161831881271[12] = 0; + out_6592468161831881271[13] = 0; + out_6592468161831881271[14] = 0; + out_6592468161831881271[15] = 0; + out_6592468161831881271[16] = 0; + out_6592468161831881271[17] = 0; + out_6592468161831881271[18] = 0; + out_6592468161831881271[19] = 0; + out_6592468161831881271[20] = 0; + out_6592468161831881271[21] = 0; + out_6592468161831881271[22] = 0; + out_6592468161831881271[23] = 0; + out_6592468161831881271[24] = 0; + out_6592468161831881271[25] = 1; + out_6592468161831881271[26] = 0; + out_6592468161831881271[27] = 0; + out_6592468161831881271[28] = 1; + out_6592468161831881271[29] = 0; + out_6592468161831881271[30] = 0; + out_6592468161831881271[31] = 0; + out_6592468161831881271[32] = 0; + out_6592468161831881271[33] = 0; + out_6592468161831881271[34] = 0; + out_6592468161831881271[35] = 0; + out_6592468161831881271[36] = 0; + out_6592468161831881271[37] = 0; + out_6592468161831881271[38] = 0; + out_6592468161831881271[39] = 0; + out_6592468161831881271[40] = 0; + out_6592468161831881271[41] = 0; + out_6592468161831881271[42] = 0; + out_6592468161831881271[43] = 0; + out_6592468161831881271[44] = 1; + out_6592468161831881271[45] = 0; + out_6592468161831881271[46] = 0; + out_6592468161831881271[47] = 1; + out_6592468161831881271[48] = 0; + out_6592468161831881271[49] = 0; + out_6592468161831881271[50] = 0; + out_6592468161831881271[51] = 0; + out_6592468161831881271[52] = 0; + out_6592468161831881271[53] = 0; } -void h_10(double *state, double *unused, double *out_5392750946095808839) { - out_5392750946095808839[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5392750946095808839[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5392750946095808839[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_4220186743788310408) { + out_4220186743788310408[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4220186743788310408[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4220186743788310408[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_6832887606116687430) { - out_6832887606116687430[0] = 0; - out_6832887606116687430[1] = 9.8100000000000005*cos(state[1]); - out_6832887606116687430[2] = 0; - out_6832887606116687430[3] = 0; - out_6832887606116687430[4] = -state[8]; - out_6832887606116687430[5] = state[7]; - out_6832887606116687430[6] = 0; - out_6832887606116687430[7] = state[5]; - out_6832887606116687430[8] = -state[4]; - out_6832887606116687430[9] = 0; - out_6832887606116687430[10] = 0; - out_6832887606116687430[11] = 0; - out_6832887606116687430[12] = 1; - out_6832887606116687430[13] = 0; - out_6832887606116687430[14] = 0; - out_6832887606116687430[15] = 1; - out_6832887606116687430[16] = 0; - out_6832887606116687430[17] = 0; - out_6832887606116687430[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6832887606116687430[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6832887606116687430[20] = 0; - out_6832887606116687430[21] = state[8]; - out_6832887606116687430[22] = 0; - out_6832887606116687430[23] = -state[6]; - out_6832887606116687430[24] = -state[5]; - out_6832887606116687430[25] = 0; - out_6832887606116687430[26] = state[3]; - out_6832887606116687430[27] = 0; - out_6832887606116687430[28] = 0; - out_6832887606116687430[29] = 0; - out_6832887606116687430[30] = 0; - out_6832887606116687430[31] = 1; - out_6832887606116687430[32] = 0; - out_6832887606116687430[33] = 0; - out_6832887606116687430[34] = 1; - out_6832887606116687430[35] = 0; - out_6832887606116687430[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6832887606116687430[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6832887606116687430[38] = 0; - out_6832887606116687430[39] = -state[7]; - out_6832887606116687430[40] = state[6]; - out_6832887606116687430[41] = 0; - out_6832887606116687430[42] = state[4]; - out_6832887606116687430[43] = -state[3]; - out_6832887606116687430[44] = 0; - out_6832887606116687430[45] = 0; - out_6832887606116687430[46] = 0; - out_6832887606116687430[47] = 0; - out_6832887606116687430[48] = 0; - out_6832887606116687430[49] = 0; - out_6832887606116687430[50] = 1; - out_6832887606116687430[51] = 0; - out_6832887606116687430[52] = 0; - out_6832887606116687430[53] = 1; +void H_10(double *state, double *unused, double *out_2494414403690571241) { + out_2494414403690571241[0] = 0; + out_2494414403690571241[1] = 9.8100000000000005*cos(state[1]); + out_2494414403690571241[2] = 0; + out_2494414403690571241[3] = 0; + out_2494414403690571241[4] = -state[8]; + out_2494414403690571241[5] = state[7]; + out_2494414403690571241[6] = 0; + out_2494414403690571241[7] = state[5]; + out_2494414403690571241[8] = -state[4]; + out_2494414403690571241[9] = 0; + out_2494414403690571241[10] = 0; + out_2494414403690571241[11] = 0; + out_2494414403690571241[12] = 1; + out_2494414403690571241[13] = 0; + out_2494414403690571241[14] = 0; + out_2494414403690571241[15] = 1; + out_2494414403690571241[16] = 0; + out_2494414403690571241[17] = 0; + out_2494414403690571241[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_2494414403690571241[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_2494414403690571241[20] = 0; + out_2494414403690571241[21] = state[8]; + out_2494414403690571241[22] = 0; + out_2494414403690571241[23] = -state[6]; + out_2494414403690571241[24] = -state[5]; + out_2494414403690571241[25] = 0; + out_2494414403690571241[26] = state[3]; + out_2494414403690571241[27] = 0; + out_2494414403690571241[28] = 0; + out_2494414403690571241[29] = 0; + out_2494414403690571241[30] = 0; + out_2494414403690571241[31] = 1; + out_2494414403690571241[32] = 0; + out_2494414403690571241[33] = 0; + out_2494414403690571241[34] = 1; + out_2494414403690571241[35] = 0; + out_2494414403690571241[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_2494414403690571241[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_2494414403690571241[38] = 0; + out_2494414403690571241[39] = -state[7]; + out_2494414403690571241[40] = state[6]; + out_2494414403690571241[41] = 0; + out_2494414403690571241[42] = state[4]; + out_2494414403690571241[43] = -state[3]; + out_2494414403690571241[44] = 0; + out_2494414403690571241[45] = 0; + out_2494414403690571241[46] = 0; + out_2494414403690571241[47] = 0; + out_2494414403690571241[48] = 0; + out_2494414403690571241[49] = 0; + out_2494414403690571241[50] = 1; + out_2494414403690571241[51] = 0; + out_2494414403690571241[52] = 0; + out_2494414403690571241[53] = 1; } -void h_13(double *state, double *unused, double *out_7827281880185644605) { - out_7827281880185644605[0] = state[3]; - out_7827281880185644605[1] = state[4]; - out_7827281880185644605[2] = state[5]; +void h_13(double *state, double *unused, double *out_5780074361933493057) { + out_5780074361933493057[0] = state[3]; + out_5780074361933493057[1] = state[4]; + out_5780074361933493057[2] = state[5]; } -void H_13(double *state, double *unused, double *out_8702277810082441) { - out_8702277810082441[0] = 0; - out_8702277810082441[1] = 0; - out_8702277810082441[2] = 0; - out_8702277810082441[3] = 1; - out_8702277810082441[4] = 0; - out_8702277810082441[5] = 0; - out_8702277810082441[6] = 0; - out_8702277810082441[7] = 0; - out_8702277810082441[8] = 0; - out_8702277810082441[9] = 0; - out_8702277810082441[10] = 0; - out_8702277810082441[11] = 0; - out_8702277810082441[12] = 0; - out_8702277810082441[13] = 0; - out_8702277810082441[14] = 0; - out_8702277810082441[15] = 0; - out_8702277810082441[16] = 0; - out_8702277810082441[17] = 0; - out_8702277810082441[18] = 0; - out_8702277810082441[19] = 0; - out_8702277810082441[20] = 0; - out_8702277810082441[21] = 0; - out_8702277810082441[22] = 1; - out_8702277810082441[23] = 0; - out_8702277810082441[24] = 0; - out_8702277810082441[25] = 0; - out_8702277810082441[26] = 0; - out_8702277810082441[27] = 0; - out_8702277810082441[28] = 0; - out_8702277810082441[29] = 0; - out_8702277810082441[30] = 0; - out_8702277810082441[31] = 0; - out_8702277810082441[32] = 0; - out_8702277810082441[33] = 0; - out_8702277810082441[34] = 0; - out_8702277810082441[35] = 0; - out_8702277810082441[36] = 0; - out_8702277810082441[37] = 0; - out_8702277810082441[38] = 0; - out_8702277810082441[39] = 0; - out_8702277810082441[40] = 0; - out_8702277810082441[41] = 1; - out_8702277810082441[42] = 0; - out_8702277810082441[43] = 0; - out_8702277810082441[44] = 0; - out_8702277810082441[45] = 0; - out_8702277810082441[46] = 0; - out_8702277810082441[47] = 0; - out_8702277810082441[48] = 0; - out_8702277810082441[49] = 0; - out_8702277810082441[50] = 0; - out_8702277810082441[51] = 0; - out_8702277810082441[52] = 0; - out_8702277810082441[53] = 0; +void H_13(double *state, double *unused, double *out_8642002086545337544) { + out_8642002086545337544[0] = 0; + out_8642002086545337544[1] = 0; + out_8642002086545337544[2] = 0; + out_8642002086545337544[3] = 1; + out_8642002086545337544[4] = 0; + out_8642002086545337544[5] = 0; + out_8642002086545337544[6] = 0; + out_8642002086545337544[7] = 0; + out_8642002086545337544[8] = 0; + out_8642002086545337544[9] = 0; + out_8642002086545337544[10] = 0; + out_8642002086545337544[11] = 0; + out_8642002086545337544[12] = 0; + out_8642002086545337544[13] = 0; + out_8642002086545337544[14] = 0; + out_8642002086545337544[15] = 0; + out_8642002086545337544[16] = 0; + out_8642002086545337544[17] = 0; + out_8642002086545337544[18] = 0; + out_8642002086545337544[19] = 0; + out_8642002086545337544[20] = 0; + out_8642002086545337544[21] = 0; + out_8642002086545337544[22] = 1; + out_8642002086545337544[23] = 0; + out_8642002086545337544[24] = 0; + out_8642002086545337544[25] = 0; + out_8642002086545337544[26] = 0; + out_8642002086545337544[27] = 0; + out_8642002086545337544[28] = 0; + out_8642002086545337544[29] = 0; + out_8642002086545337544[30] = 0; + out_8642002086545337544[31] = 0; + out_8642002086545337544[32] = 0; + out_8642002086545337544[33] = 0; + out_8642002086545337544[34] = 0; + out_8642002086545337544[35] = 0; + out_8642002086545337544[36] = 0; + out_8642002086545337544[37] = 0; + out_8642002086545337544[38] = 0; + out_8642002086545337544[39] = 0; + out_8642002086545337544[40] = 0; + out_8642002086545337544[41] = 1; + out_8642002086545337544[42] = 0; + out_8642002086545337544[43] = 0; + out_8642002086545337544[44] = 0; + out_8642002086545337544[45] = 0; + out_8642002086545337544[46] = 0; + out_8642002086545337544[47] = 0; + out_8642002086545337544[48] = 0; + out_8642002086545337544[49] = 0; + out_8642002086545337544[50] = 0; + out_8642002086545337544[51] = 0; + out_8642002086545337544[52] = 0; + out_8642002086545337544[53] = 0; } -void h_14(double *state, double *unused, double *out_3917682265727873259) { - out_3917682265727873259[0] = state[6]; - out_3917682265727873259[1] = state[7]; - out_3917682265727873259[2] = state[8]; +void h_14(double *state, double *unused, double *out_3473232613379301184) { + out_3473232613379301184[0] = state[6]; + out_3473232613379301184[1] = state[7]; + out_3473232613379301184[2] = state[8]; } -void H_14(double *state, double *unused, double *out_759669308817234169) { - out_759669308817234169[0] = 0; - out_759669308817234169[1] = 0; - out_759669308817234169[2] = 0; - out_759669308817234169[3] = 0; - out_759669308817234169[4] = 0; - out_759669308817234169[5] = 0; - out_759669308817234169[6] = 1; - out_759669308817234169[7] = 0; - out_759669308817234169[8] = 0; - out_759669308817234169[9] = 0; - out_759669308817234169[10] = 0; - out_759669308817234169[11] = 0; - out_759669308817234169[12] = 0; - out_759669308817234169[13] = 0; - out_759669308817234169[14] = 0; - out_759669308817234169[15] = 0; - out_759669308817234169[16] = 0; - out_759669308817234169[17] = 0; - out_759669308817234169[18] = 0; - out_759669308817234169[19] = 0; - out_759669308817234169[20] = 0; - out_759669308817234169[21] = 0; - out_759669308817234169[22] = 0; - out_759669308817234169[23] = 0; - out_759669308817234169[24] = 0; - out_759669308817234169[25] = 1; - out_759669308817234169[26] = 0; - out_759669308817234169[27] = 0; - out_759669308817234169[28] = 0; - out_759669308817234169[29] = 0; - out_759669308817234169[30] = 0; - out_759669308817234169[31] = 0; - out_759669308817234169[32] = 0; - out_759669308817234169[33] = 0; - out_759669308817234169[34] = 0; - out_759669308817234169[35] = 0; - out_759669308817234169[36] = 0; - out_759669308817234169[37] = 0; - out_759669308817234169[38] = 0; - out_759669308817234169[39] = 0; - out_759669308817234169[40] = 0; - out_759669308817234169[41] = 0; - out_759669308817234169[42] = 0; - out_759669308817234169[43] = 0; - out_759669308817234169[44] = 1; - out_759669308817234169[45] = 0; - out_759669308817234169[46] = 0; - out_759669308817234169[47] = 0; - out_759669308817234169[48] = 0; - out_759669308817234169[49] = 0; - out_759669308817234169[50] = 0; - out_759669308817234169[51] = 0; - out_759669308817234169[52] = 0; - out_759669308817234169[53] = 0; +void H_14(double *state, double *unused, double *out_3509679729536508975) { + out_3509679729536508975[0] = 0; + out_3509679729536508975[1] = 0; + out_3509679729536508975[2] = 0; + out_3509679729536508975[3] = 0; + out_3509679729536508975[4] = 0; + out_3509679729536508975[5] = 0; + out_3509679729536508975[6] = 1; + out_3509679729536508975[7] = 0; + out_3509679729536508975[8] = 0; + out_3509679729536508975[9] = 0; + out_3509679729536508975[10] = 0; + out_3509679729536508975[11] = 0; + out_3509679729536508975[12] = 0; + out_3509679729536508975[13] = 0; + out_3509679729536508975[14] = 0; + out_3509679729536508975[15] = 0; + out_3509679729536508975[16] = 0; + out_3509679729536508975[17] = 0; + out_3509679729536508975[18] = 0; + out_3509679729536508975[19] = 0; + out_3509679729536508975[20] = 0; + out_3509679729536508975[21] = 0; + out_3509679729536508975[22] = 0; + out_3509679729536508975[23] = 0; + out_3509679729536508975[24] = 0; + out_3509679729536508975[25] = 1; + out_3509679729536508975[26] = 0; + out_3509679729536508975[27] = 0; + out_3509679729536508975[28] = 0; + out_3509679729536508975[29] = 0; + out_3509679729536508975[30] = 0; + out_3509679729536508975[31] = 0; + out_3509679729536508975[32] = 0; + out_3509679729536508975[33] = 0; + out_3509679729536508975[34] = 0; + out_3509679729536508975[35] = 0; + out_3509679729536508975[36] = 0; + out_3509679729536508975[37] = 0; + out_3509679729536508975[38] = 0; + out_3509679729536508975[39] = 0; + out_3509679729536508975[40] = 0; + out_3509679729536508975[41] = 0; + out_3509679729536508975[42] = 0; + out_3509679729536508975[43] = 0; + out_3509679729536508975[44] = 1; + out_3509679729536508975[45] = 0; + out_3509679729536508975[46] = 0; + out_3509679729536508975[47] = 0; + out_3509679729536508975[48] = 0; + out_3509679729536508975[49] = 0; + out_3509679729536508975[50] = 0; + out_3509679729536508975[51] = 0; + out_3509679729536508975[52] = 0; + out_3509679729536508975[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071) { - err_fun(nom_x, delta_x, out_7113141565155944071); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4940663026929446750) { + err_fun(nom_x, delta_x, out_4940663026929446750); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861) { - inv_err_fun(nom_x, true_x, out_4273806753012627861); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442) { + inv_err_fun(nom_x, true_x, out_1207218016783825442); } -void pose_H_mod_fun(double *state, double *out_4327706851715669235) { - H_mod_fun(state, out_4327706851715669235); +void pose_H_mod_fun(double *state, double *out_2820660951987973699) { + H_mod_fun(state, out_2820660951987973699); } -void pose_f_fun(double *state, double dt, double *out_497730658810754078) { - f_fun(state, dt, out_497730658810754078); +void pose_f_fun(double *state, double dt, double *out_2310584025056258416) { + f_fun(state, dt, out_2310584025056258416); } -void pose_F_fun(double *state, double dt, double *out_6088738551748760158) { - F_fun(state, dt, out_6088738551748760158); +void pose_F_fun(double *state, double dt, double *out_6557190266781458971) { + F_fun(state, dt, out_6557190266781458971); } -void pose_h_4(double *state, double *unused, double *out_2742355909746826294) { - h_4(state, unused, out_2742355909746826294); +void pose_h_4(double *state, double *unused, double *out_8177424571477984765) { + h_4(state, unused, out_8177424571477984765); } -void pose_H_4(double *state, double *unused, double *out_3203571547522250360) { - H_4(state, unused, out_3203571547522250360); +void pose_H_4(double *state, double *unused, double *out_6592468161831881271) { + H_4(state, unused, out_6592468161831881271); } -void pose_h_10(double *state, double *unused, double *out_5392750946095808839) { - h_10(state, unused, out_5392750946095808839); +void pose_h_10(double *state, double *unused, double *out_4220186743788310408) { + h_10(state, unused, out_4220186743788310408); } -void pose_H_10(double *state, double *unused, double *out_6832887606116687430) { - H_10(state, unused, out_6832887606116687430); +void pose_H_10(double *state, double *unused, double *out_2494414403690571241) { + H_10(state, unused, out_2494414403690571241); } -void pose_h_13(double *state, double *unused, double *out_7827281880185644605) { - h_13(state, unused, out_7827281880185644605); +void pose_h_13(double *state, double *unused, double *out_5780074361933493057) { + h_13(state, unused, out_5780074361933493057); } -void pose_H_13(double *state, double *unused, double *out_8702277810082441) { - H_13(state, unused, out_8702277810082441); +void pose_H_13(double *state, double *unused, double *out_8642002086545337544) { + H_13(state, unused, out_8642002086545337544); } -void pose_h_14(double *state, double *unused, double *out_3917682265727873259) { - h_14(state, unused, out_3917682265727873259); +void pose_h_14(double *state, double *unused, double *out_3473232613379301184) { + h_14(state, unused, out_3473232613379301184); } -void pose_H_14(double *state, double *unused, double *out_759669308817234169) { - H_14(state, unused, out_759669308817234169); +void pose_H_14(double *state, double *unused, double *out_3509679729536508975) { + H_14(state, unused, out_3509679729536508975); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index f82030be7..7637a03f3 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_7113141565155944071); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4273806753012627861); -void pose_H_mod_fun(double *state, double *out_4327706851715669235); -void pose_f_fun(double *state, double dt, double *out_497730658810754078); -void pose_F_fun(double *state, double dt, double *out_6088738551748760158); -void pose_h_4(double *state, double *unused, double *out_2742355909746826294); -void pose_H_4(double *state, double *unused, double *out_3203571547522250360); -void pose_h_10(double *state, double *unused, double *out_5392750946095808839); -void pose_H_10(double *state, double *unused, double *out_6832887606116687430); -void pose_h_13(double *state, double *unused, double *out_7827281880185644605); -void pose_H_13(double *state, double *unused, double *out_8702277810082441); -void pose_h_14(double *state, double *unused, double *out_3917682265727873259); -void pose_H_14(double *state, double *unused, double *out_759669308817234169); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4940663026929446750); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1207218016783825442); +void pose_H_mod_fun(double *state, double *out_2820660951987973699); +void pose_f_fun(double *state, double dt, double *out_2310584025056258416); +void pose_F_fun(double *state, double dt, double *out_6557190266781458971); +void pose_h_4(double *state, double *unused, double *out_8177424571477984765); +void pose_H_4(double *state, double *unused, double *out_6592468161831881271); +void pose_h_10(double *state, double *unused, double *out_4220186743788310408); +void pose_H_10(double *state, double *unused, double *out_2494414403690571241); +void pose_h_13(double *state, double *unused, double *out_5780074361933493057); +void pose_H_13(double *state, double *unused, double *out_8642002086545337544); +void pose_h_14(double *state, double *unused, double *out_3473232613379301184); +void pose_H_14(double *state, double *unused, double *out_3509679729536508975); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 690189863..d03843660 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 98f7e0720..d1a99aad4 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -253,11 +253,22 @@ class SelfdriveD: # body always wants to enable self.events.add(EventName.pcmEnable) - # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0. - if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ - (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): + gas_disable = CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator + brake_or_regen_disable = ( + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or + (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)) + ) + preap_steering_only_brake = ( + self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and + self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise + ) + if gas_disable: self.events.add(EventName.pedalPressed) + elif brake_or_regen_disable: + if preap_steering_only_brake: + self.events.add(EventName.gasPressedOverride) + else: + self.events.add(EventName.pedalPressed) # Create events for temperature, disk space, and memory if self.sm['deviceState'].thermalStatus >= ThermalStatus.red: @@ -432,7 +443,12 @@ class SelfdriveD: self.enabled, self.sm['starpilotCarState'].alwaysOnLateralEnabled, ) - cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) and not pacifica_hybrid_aol + preap_software_cruise = ( + self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and + self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise + ) + effective_pcm_cruise = self.CP.pcmCruise or preap_software_cruise + cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not effective_pcm_cruise) and not pacifica_hybrid_aol self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0 if self.cruise_mismatch_counter > int(6. / DT_CTRL): self.events.add(EventName.cruiseMismatch) diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 65c9fa3b0..7a0c9ccd5 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 7d3123a90..5dcf8c4c7 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -77,6 +77,13 @@ LEGACY_STARPILOT_STATS_KEY_RENAMES = { "FrogPilotSeconds": "StarPilotSeconds", } +LEGACY_VOLT_STOCK_ACC_CARS = { + GM_CAR.CHEVROLET_VOLT, + GM_CAR.CHEVROLET_VOLT_2019, + GM_CAR.CHEVROLET_VOLT_ASCM, + GM_CAR.CHEVROLET_VOLT_CAMERA, +} + RESOURCES_REPO = os.getenv("STARPILOT_RESOURCES_REPO", "firestar5683/StarPilot-Resources") ACTIVE_THEME_PATH = Path(BASEDIR) / "starpilot/assets/active_theme" @@ -1193,15 +1200,10 @@ class StarPilotVariables: condition=toggle.car_make == "gm" and toggle.has_pedal and "BOLT" in toggle.car_model, ) - gm_auto_hold_supported = toggle.car_model in { - GM_CAR.CHEVROLET_VOLT, - GM_CAR.CHEVROLET_VOLT_2019, - GM_CAR.CHEVROLET_VOLT_ASCM, - GM_CAR.CHEVROLET_VOLT_CAMERA, - } + gm_auto_hold_supported = toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS toggle.gm_auto_hold = self.get_value("GMAutoHold", condition=gm_auto_hold_supported) - toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model == "CHEVROLET_VOLT") + toggle.volt_sng = self.get_value("VoltSNG", condition=toggle.car_model in LEGACY_VOLT_STOCK_ACC_CARS) process_starpilot_toggles.cache_clear() self.params_memory.remove("StarPilotTogglesUpdated") diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index 1d564177b..35694ec68 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -3,6 +3,15 @@ from types import SimpleNamespace from openpilot.starpilot.common import starpilot_variables as spv +def test_legacy_volt_stock_acc_models_share_sng_and_auto_hold_scope(): + assert spv.LEGACY_VOLT_STOCK_ACC_CARS == { + "CHEVROLET_VOLT", + "CHEVROLET_VOLT_2019", + "CHEVROLET_VOLT_ASCM", + "CHEVROLET_VOLT_CAMERA", + } + + def test_get_starpilot_toggles_uses_last_non_empty_broadcast(monkeypatch): params = SimpleNamespace(get_bool=lambda _key: False) monkeypatch.setattr(spv.get_starpilot_toggles, "_params", params, raising=False) diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index cf5763ad9..3a4ab4e19 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -2143,6 +2143,100 @@ "data_type": "bool", "ui_type": "toggle" }, + { + "key": "NAPRadarEnabled", + "label": "Tesla Pre-AP Bosch Radar", + "description": "Use the stock Bosch radar path on pre-Autopilot Teslas.", + "data_type": "bool", + "ui_type": "toggle", + "is_parent_toggle": true + }, + { + "key": "NAPRadarBehindNosecone", + "label": "Radar Behind Nosecone", + "description": "Enable this if the Bosch radar is mounted behind the nosecone instead of being exposed.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "NAPRadarEnabled" + }, + { + "key": "NAPRadarOffset", + "label": "Radar Offset", + "description": "Adjust the radar position offset in meters if the radar alignment needs a small correction.", + "data_type": "float", + "ui_type": "numeric", + "min": -2.0, + "max": 2.0, + "step": 0.01, + "precision": 2, + "parent_key": "NAPRadarEnabled" + }, + { + "key": "NAPPedalEnabled", + "label": "Tesla Pre-AP comma Pedal Longitudinal", + "description": "Use a comma pedal interceptor for acceleration and regenerative braking on pre-Autopilot Teslas.", + "data_type": "bool", + "ui_type": "toggle", + "is_parent_toggle": true + }, + { + "key": "NAPPedalCanBus", + "label": "Pedal CAN Bus", + "description": "Select the CAN bus used by the Tesla pedal interceptor.", + "data_type": "int", + "ui_type": "dropdown", + "parent_key": "NAPPedalEnabled", + "options": [ + { + "value": 0, + "label": "CAN 0" + }, + { + "value": 2, + "label": "CAN 2" + } + ] + }, + { + "key": "NAPAdaptiveAccel", + "label": "Adaptive Acceleration Limit", + "description": "Reduce maximum acceleration as you close in on a lead vehicle when Tesla pedal-long is active.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "NAPPedalEnabled" + }, + { + "key": "NAPPedalCalibDone", + "label": "Pedal Calibration Complete", + "description": "Enable only after entering valid Tesla pedal calibration values. Pedal-long stays inactive until this is on.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "NAPPedalEnabled" + }, + { + "key": "NAPPedalCalibFactor", + "label": "Pedal Calibration Factor", + "description": "Scaling factor used to convert Tesla pedal interceptor output into the internal DI range.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.01, + "max": 10.0, + "step": 0.01, + "precision": 2, + "parent_key": "NAPPedalEnabled" + }, + { + "key": "NAPPedalCalibZero", + "label": "Pedal Calibration Zero", + "description": "Zero point used to convert Tesla pedal interceptor output into the internal DI range.", + "data_type": "float", + "ui_type": "numeric", + "min": -50.0, + "max": 50.0, + "step": 0.01, + "precision": 2, + "parent_key": "NAPPedalEnabled" + }, { "key": "GMPedalLongitudinal", "label": "Use Pedal For Longitudinal", diff --git a/system/camerad/camerad b/system/camerad/camerad index 88f1368d5..3cf9b0c43 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 9121ea97f..90da6be9d 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 610d5bb34..a047bdbb3 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index d65bc3371..2c11d3ec2 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ