mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-07-26 20:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

neeneeneeneeneenee

Browse Source
This commit is contained in:
firestar5683
2026-05-05 16:20:53 -05:00
parent 9b97124f80
commit e421eb271e
46 changed files with 1495 additions and 1443 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+10 -4
View File
@@ -251,6 +251,7 @@ class CarController(CarControllerBase):
self._ioniq_6_last_ipedal_regen_state_2 = 0
self._ioniq_6_last_buttons_counter = 0
self._ioniq_6_last_regen_control_counter = -1
self._ioniq_6_regen_request_sent = False
self._ioniq_6_last_gear = structs.CarState.GearShifter.unknown
self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState()
@@ -258,11 +259,13 @@ class CarController(CarControllerBase):
self._ioniq_6_always_ipedal_pending = False
self._ioniq_6_always_ipedal_press_remaining = 0
self._ioniq_6_always_ipedal_retry_frame = 0
self._ioniq_6_regen_request_sent = False
def _arm_ioniq_6_always_ipedal(self) -> None:
self._ioniq_6_always_ipedal_pending = True
self._ioniq_6_always_ipedal_press_remaining = 0
self._ioniq_6_always_ipedal_retry_frame = self.frame
self._ioniq_6_regen_request_sent = False
def _update_ioniq_6_always_ipedal(self, CC, CS, starpilot_toggles):
can_sends = []
@@ -313,6 +316,7 @@ class CarController(CarControllerBase):
if self._ioniq_6_always_ipedal_press_remaining == 0 and self.frame >= self._ioniq_6_always_ipedal_retry_frame:
self._ioniq_6_always_ipedal_press_remaining = IONIQ_6_IPEDAL_LATCH_PRESS_SEND_COUNT if (max_regen_state or ipedal_latch_pending) \
else IONIQ_6_IPEDAL_PRESS_SEND_COUNT
self._ioniq_6_regen_request_sent = False
# Mirror the current stock counter after seeing the real CRUISE_BUTTONS frame land on the bus.
# Sending the next counter early creates a paddle frame that gets followed by the stock no-paddle
@@ -327,10 +331,12 @@ class CarController(CarControllerBase):
self._ioniq_6_always_ipedal_retry_frame = self.frame + retry_wait_frames
# The drivetrain latch uses a second HKG CAN-FD request path in addition to the left paddle bit.
# Once the cluster-facing regen state reaches max regen, mirror the live stock frame and only
# flip the request bytes that the physical paddle toggles for the final i-Pedal latch step.
if max_regen_state and has_regen_control_msg and regen_control_counter_changed:
can_sends.append(hyundaicanfd.create_ioniq_6_regen_control(self.packer, self.CP, self.CAN, regen_control_msg))
# Mirror the next stock 0x25A frame once per retry burst instead of spamming it continuously.
if max_regen_state and has_regen_control_msg and regen_control_counter_changed and not self._ioniq_6_regen_request_sent:
request_tail = hyundaicanfd.get_ioniq_6_regen_control_request_tail(regen_control_msg)
if request_tail is not None:
can_sends.append(hyundaicanfd.create_ioniq_6_regen_control(self.packer, self.CP, self.CAN, regen_control_msg))
self._ioniq_6_regen_request_sent = True
self._ioniq_6_last_ipedal_regen_state = regen_state
self._ioniq_6_last_ipedal_regen_state_2 = regen_state_2
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
+17 -7
View File
@@ -164,8 +164,12 @@ IONIQ_6_CRUISE_BUTTONS_BASE_CHECKSUMS = (
)
IONIQ_6_CRUISE_BUTTONS_LEFT_PADDLE_CHECKSUM_XOR = 0x77
IONIQ_6_CRUISE_BUTTONS_RIGHT_PADDLE_CHECKSUM_XOR = 0xD4
IONIQ_6_REGEN_CONTROL_REQ_BYTE24 = 0xC0
IONIQ_6_REGEN_CONTROL_REQ_BYTE27 = 0x00
IONIQ_6_REGEN_CONTROL_REQUEST_TAILS = {
# Stock manual left-paddle latch request seen on route 0000025e.
(0xA8, 0x0C, 0x12, 0x0E): (0xC0, 0x0C, 0x12, 0x00),
# Stock manual left-paddle latch request seen on route 00000260.
(0xA8, 0x0E, 0x07, 0x0E): (0x85, 0x0E, 0x07, 0x0C),
}
def get_ioniq_6_cruise_buttons_next_counter(counter: int) -> int:
@@ -202,14 +206,20 @@ def create_ioniq_6_paddle_buttons(packer, CP, CAN, cnt, left_paddle=False, right
return address, bytes(dat), bus
def create_ioniq_6_regen_control(packer, CP, CAN, base_values,
req_byte24=IONIQ_6_REGEN_CONTROL_REQ_BYTE24,
req_byte27=IONIQ_6_REGEN_CONTROL_REQ_BYTE27):
def get_ioniq_6_regen_control_request_tail(base_values):
base_tail = tuple(int(base_values.get(f"BYTE{i}", 0)) for i in range(24, 28))
return IONIQ_6_REGEN_CONTROL_REQUEST_TAILS.get(base_tail)
def create_ioniq_6_regen_control(packer, CP, CAN, base_values):
request_tail = get_ioniq_6_regen_control_request_tail(base_values)
if request_tail is None:
raise ValueError(f"Unsupported Ioniq 6 regen control tail: {tuple(int(base_values.get(f'BYTE{i}', 0)) for i in range(24, 28))}")
values = {k: v for k, v in base_values.items() if k != "CHECKSUM"}
address = packer.dbc.name_to_msg["IONIQ_6_REGEN_CONTROL"].address
dat = bytearray(packer.pack(address, values))
dat[24] = req_byte24
dat[27] = req_byte27
dat[24:28] = bytes(request_tail)
checksum = hkg_can_fd_checksum(address, None, dat)
dat[0] = checksum & 0xFF
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+16 -7
View File
@@ -343,7 +343,11 @@ class TestHyundaiFingerprint:
assert hyundaicanfd.get_ioniq_6_cruise_buttons_next_counter(13) == 14
assert hyundaicanfd.get_ioniq_6_cruise_buttons_next_counter(14) == 0
def test_ioniq_6_regen_control_message_preserves_stock_frame_and_flips_only_ipedal_request_bytes(self):
@pytest.mark.parametrize(("stock_hex", "expected_tail"), [
("45421440801f000000000000a865170000a000000080c071a80c120e00000000", bytes.fromhex("c00c1200")),
("47745840801f0000000005006e5e0e0000a000000080c071a80e070e00000000", bytes.fromhex("850e070c")),
])
def test_ioniq_6_regen_control_message_preserves_stock_frame_and_flips_only_ipedal_request_bytes(self, stock_hex, expected_tail):
CP = CarParams.new_message()
CP.carFingerprint = CAR.HYUNDAI_IONIQ_6
CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING)
@@ -351,14 +355,12 @@ class TestHyundaiFingerprint:
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
can_bus = CanBus(CP)
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("IONIQ_6_REGEN_CONTROL", 0)], can_bus.ECAN)
stock_dat = bytes.fromhex("45421440801f000000000000a865170000a000000080c071a80c120e00000000")
stock_dat = bytes.fromhex(stock_hex)
parser.update([(1, [(0x25A, stock_dat, can_bus.ECAN)])])
msg = hyundaicanfd.create_ioniq_6_regen_control(packer, CP, can_bus, parser.vl["IONIQ_6_REGEN_CONTROL"])
assert msg[1][2:24] == stock_dat[2:24]
assert msg[1][24] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE24
assert msg[1][25:27] == stock_dat[25:27]
assert msg[1][27] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE27
assert msg[1][24:28] == expected_tail
assert msg[1][28:] == stock_dat[28:]
checksum = hyundaicanfd.hkg_can_fd_checksum(msg[0], None, bytearray(msg[1]))
assert msg[1][0] | (msg[1][1] << 8) == checksum
@@ -525,11 +527,18 @@ class TestHyundaiFingerprint:
assert any(msg[0] == 0x1CF for msg in sends)
regen_cmd = next(msg for msg in sends if msg[0] == 0x25A)
assert regen_cmd[1][24] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE24
assert regen_cmd[1][27] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE27
assert regen_cmd[1][24:28] == bytes.fromhex("c00c1200")
checksum = hyundaicanfd.hkg_can_fd_checksum(regen_cmd[0], None, bytearray(regen_cmd[1]))
assert regen_cmd[1][0] | (regen_cmd[1][1] << 8) == checksum
controller.frame = 1
cs.buttons_counter = 6
cs.ioniq_6_regen_control_msg = dict(cs.ioniq_6_regen_control_msg, COUNTER=0x15)
sends = controller._update_ioniq_6_always_ipedal(cc, cs, toggles)
assert any(msg[0] == 0x1CF for msg in sends)
assert not any(msg[0] == 0x25A for msg in sends)
def test_ioniq_6_longitudinal_params_match_canfd_tune(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles)
opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc
-35
View File
@@ -774,39 +774,6 @@ BO_ 593 RADAR_0x251: 16 FRONT_RADAR
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
BO_ 602 IONIQ_6_REGEN_CONTROL: 32 XXX
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE3 : 24|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE4 : 32|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE5 : 40|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE6 : 48|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE7 : 56|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE8 : 64|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE9 : 72|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE10 : 80|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE11 : 88|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE12 : 96|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE13 : 104|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE14 : 112|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE15 : 120|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE16 : 128|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE17 : 136|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE18 : 144|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE19 : 152|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE20 : 160|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE21 : 168|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE22 : 176|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE23 : 184|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE24 : 192|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE25 : 200|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE26 : 208|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE27 : 216|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE28 : 224|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE29 : 232|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE30 : 240|8@1+ (1,0) [0|255] "" XXX
SG_ BYTE31 : 248|8@1+ (1,0) [0|255] "" XXX
BO_ 687 HOD_FD_01_100ms: 8 XXX
SG_ HOD_Dir_Status : 18|3@0+ (1,0) [0|7] "" XXX
SG_ NEW_SIGNAL_2 : 32|8@1+ (1,0) [0|255] "" XXX
@@ -856,9 +823,7 @@ BO_ 736 MANUAL_SPEED_LIMIT_ASSIST: 32 XXX
SG_ MSLA_STATUS : 26|2@1+ (1,0) [0|3] "" XXX
SG_ MSLA_ENABLED : 38|1@1+ (1,0) [0|1] "" XXX
SG_ MAX_SPEED : 55|8@0+ (1,0) [0|255] "" XXX
SG_ EV_REGEN_STATE : 104|8@1+ (1,0) [0|255] "" XXX
SG_ MAX_SPEED_COPY : 144|8@1+ (1,0) [0|255] "" XXX
SG_ EV_REGEN_STATE_2 : 184|8@1+ (1,0) [0|255] "" XXX
BO_ 837 ADRV_0x345: 8 ADRV
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+25 -5
View File
@@ -82,6 +82,25 @@ static uint32_t hyundai_canfd_get_checksum(const CANPacket_t *msg) {
return chksum;
}
static bool hyundai_canfd_ioniq_6_regen_tail_allowed(const CANPacket_t *msg) {
const uint8_t stock24 = hyundai_canfd_last_regen_control[24];
const uint8_t stock25 = hyundai_canfd_last_regen_control[25];
const uint8_t stock26 = hyundai_canfd_last_regen_control[26];
const uint8_t stock27 = hyundai_canfd_last_regen_control[27];
if ((stock24 == 0xA8U) && (stock25 == 0x0CU) && (stock26 == 0x12U) && (stock27 == 0x0EU)) {
return (msg->data[24] == 0xC0U) && (msg->data[25] == 0x0CU) &&
(msg->data[26] == 0x12U) && (msg->data[27] == 0x00U);
}
if ((stock24 == 0xA8U) && (stock25 == 0x0EU) && (stock26 == 0x07U) && (stock27 == 0x0EU)) {
return (msg->data[24] == 0x85U) && (msg->data[25] == 0x0EU) &&
(msg->data[26] == 0x07U) && (msg->data[27] == 0x0CU);
}
return false;
}
static void hyundai_canfd_rx_all_hook(const CANPacket_t *msg) {
const unsigned pt_bus = hyundai_canfd_lka_steering ? 1U : 0U;
@@ -273,14 +292,15 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
(GET_LEN(msg) == 32U) &&
hyundai_canfd_last_regen_control_seen &&
(hyundai_canfd_get_checksum(msg) == hyundai_common_canfd_compute_checksum(msg)) &&
(msg->data[24] == 0xC0U) &&
(msg->data[27] == 0x00U);
hyundai_canfd_ioniq_6_regen_tail_allowed(msg);
if (allowed) {
for (int i = 2; i < 32; i++) {
if ((i != 24) && (i != 27) && (msg->data[i] != hyundai_canfd_last_regen_control[i])) {
allowed = false;
break;
if ((i < 24) || (i > 27)) {
if (msg->data[i] != hyundai_canfd_last_regen_control[i]) {
allowed = false;
break;
}
}
}
}
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+9 -3
View File
@@ -440,7 +440,7 @@ class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase):
}
return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values)
def _regen_control_msg(self, byte24=0xA8, byte27=0x0E, counter=0x14, bus=None):
def _regen_control_msg(self, byte24=0xA8, byte25=0x0C, byte26=0x12, byte27=0x0E, counter=0x14, bus=None):
if bus is None:
bus = self.PT_BUS
values = {
@@ -467,8 +467,8 @@ class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase):
"BYTE22": 0xC0,
"BYTE23": 0x71,
"BYTE24": byte24,
"BYTE25": 0x0C,
"BYTE26": 0x12,
"BYTE25": byte25,
"BYTE26": byte26,
"BYTE27": byte27,
"BYTE28": 0x00,
"BYTE29": 0x00,
@@ -522,10 +522,16 @@ class TestHyundaiCanfdLKASteeringEVAlwaysIPedal(TestHyundaiCanfdLKASteeringEV):
self.safety.set_controls_allowed(False)
self.assertTrue(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x14)))
self.assertFalse(self._tx(self._regen_control_msg(byte24=0x85, byte25=0x0E, byte26=0x07, byte27=0x0C, counter=0x14)))
self.assertFalse(self._tx(self._regen_control_msg(byte24=0xA8, byte27=0x00, counter=0x14)))
self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x0E, counter=0x14)))
self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x15)))
stock_msg = self._regen_control_msg(byte24=0xA8, byte25=0x0E, byte26=0x07, byte27=0x0E, counter=0x58)
self._rx(stock_msg)
self.assertTrue(self._tx(self._regen_control_msg(byte24=0x85, byte25=0x0E, byte26=0x07, byte27=0x0C, counter=0x58)))
self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte25=0x0E, byte26=0x07, byte27=0x00, counter=0x58)))
self.safety.set_controls_allowed(True)
self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x14)))
panda/board/drivers/can_common.h
+16
View File
@@ -216,6 +216,22 @@ void ignition_can_hook(CANPacket_t *msg) {
prev_counter_tesla = counter;
}
// Tesla Model S pre-AP exception
if ((msg->addr == 0x101U) && (len == 3)) {
// Validate Tesla checksum/counter to avoid false positives on overlapping frames.
int counter = msg->data[1] & 0xFU;
int checksum = (((msg->addr & 0xFFU) + ((msg->addr >> 8U) & 0xFFU) + msg->data[0] + msg->data[1]) & 0xFFU);
static int prev_counter_tesla_preap = -1;
if ((msg->data[2] == checksum) && (counter == ((prev_counter_tesla_preap + 1) % 16)) && (prev_counter_tesla_preap != -1)) {
// GTW_epasPowerMode=1 is DRIVE_ON, which is the only ignition source on pre-AP cars.
int power_mode = (msg->data[0] >> 3U) & 0xFU;
ignition_can = power_mode == 0x1U;
ignition_can_cnt = 0U;
}
prev_counter_tesla_preap = counter;
}
// Mazda exception
if ((msg->addr == 0x9EU) && (len == 8)) {
ignition_can = (msg->data[0] >> 5) == 0x6U;
panda/board/obj/body_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/body_h7/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/body_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/body_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.body_h7.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda_h7.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda_h7_remote.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda_jungle_h7.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda_remote.bin
BIN
View File
Binary file not shown.
panda/board/obj/gitversion.h
+1 -1
View File
@@ -1,2 +1,2 @@
extern const uint8_t gitversion[19];
const uint8_t gitversion[19] = "DEV-2f74f933-DEBUG";
const uint8_t gitversion[19] = "DEV-9b97124f-DEBUG";
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_jungle_h7/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote/bootstub.elf
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote/main.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda_remote/main.elf
BIN
View File
Binary file not shown.
panda/board/obj/version
+1 -1
View File
@@ -1 +1 @@
DEV-2f74f933-DEBUG
DEV-9b97124f-DEBUG
selfdrive/controls/lib/latcontrol_torque.py
+40 -26
View File
@@ -48,8 +48,8 @@ UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3
MIN_LATERAL_CONTROL_SPEED = 0.3
CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 0.98
CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.14
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.18
CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0
CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.10
CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18
@@ -66,22 +66,26 @@ 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_A_VARIANT_FF_RESTORE_LEFT = 0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.01
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.02
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.14
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.22
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.06
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.14
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.04
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.10
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.18
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.28
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.38
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.48
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.50
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.62
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.01
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.24
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.34
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.32
CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.46
BOLT_2022_2023_CARS = (
GM_CAR.CHEVROLET_BOLT_ACC_2022_2023,
@@ -248,21 +252,21 @@ IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.28
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.36
IONIQ_6_UNWIND_TAPER_LEFT = 2.18
IONIQ_6_UNWIND_TAPER_RIGHT = 5.05
IONIQ_6_TURN_IN_BOOST_LEFT = 1.40
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.50
IONIQ_6_UNWIND_TAPER_LEFT = 2.40
IONIQ_6_UNWIND_TAPER_RIGHT = 5.40
IONIQ_6_FRICTION_MULT = 0.965
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.50
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.72
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.55
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.20
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.26
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.44
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.30
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 5.70
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.56
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.82
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.80
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.70
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.30
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.50
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.55
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.10
IONIQ_6_CENTER_TAPER_MAX = 0.056
IONIQ_6_CENTER_TAPER_LAT = 0.22
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
@@ -426,6 +430,11 @@ def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_la
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT,
CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
if a_variant_active:
unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_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,
@@ -467,6 +476,11 @@ def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_acce
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) *
envelope * unwind_weight)
if a_variant_active:
friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel,
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT,
CIVIC_BOSCH_MODIFIED_A_VARIANT_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,
selfdrive/controls/tests/test_latcontrol.py
+15 -9
View File
@@ -432,13 +432,13 @@ class TestLatControl:
monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: True)
a_variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
assert a_variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 0.98)
assert a_variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20)
monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: False)
monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)
variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL)
assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.14)
assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.18)
def test_modified_civic_b_torque_ff_scale_curve(self):
steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0)
@@ -490,7 +490,9 @@ class TestLatControl:
base_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0)
base_turn_in_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.8, 12.0)
base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
base_turn_in_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, 0.8)
base_unwind_left = 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_a_lateral_testing_ground_active", lambda: True)
@@ -498,13 +500,17 @@ class TestLatControl:
a_variant_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0)
a_variant_turn_in_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.8, 12.0)
a_variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0)
a_variant_turn_in_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, 0.8)
a_variant_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0)
a_variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0)
a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8)
assert a_variant_steady_left > base_steady_left
assert a_variant_steady_right > base_steady_right
assert a_variant_turn_in_left > base_turn_in_left
assert a_variant_turn_in_right > base_turn_in_right
assert a_variant_turn_in_left_friction > base_turn_in_left_friction
assert a_variant_steady_left == pytest.approx(base_steady_left)
assert a_variant_steady_right == pytest.approx(base_steady_right)
assert a_variant_turn_in_left == pytest.approx(base_turn_in_left)
assert a_variant_turn_in_right == pytest.approx(base_turn_in_right)
assert a_variant_unwind_left < base_unwind_left
assert a_variant_unwind_right < base_unwind_right
assert a_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)
selfdrive/locationd/models/generated/car.cpp
+342 -342
View File
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) {
out_1461273597009064329[0] = delta_x[0] + nom_x[0];
out_1461273597009064329[1] = delta_x[1] + nom_x[1];
out_1461273597009064329[2] = delta_x[2] + nom_x[2];
out_1461273597009064329[3] = delta_x[3] + nom_x[3];
out_1461273597009064329[4] = delta_x[4] + nom_x[4];
out_1461273597009064329[5] = delta_x[5] + nom_x[5];
out_1461273597009064329[6] = delta_x[6] + nom_x[6];
out_1461273597009064329[7] = delta_x[7] + nom_x[7];
out_1461273597009064329[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701) {
out_4611641411917091701[0] = delta_x[0] + nom_x[0];
out_4611641411917091701[1] = delta_x[1] + nom_x[1];
out_4611641411917091701[2] = delta_x[2] + nom_x[2];
out_4611641411917091701[3] = delta_x[3] + nom_x[3];
out_4611641411917091701[4] = delta_x[4] + nom_x[4];
out_4611641411917091701[5] = delta_x[5] + nom_x[5];
out_4611641411917091701[6] = delta_x[6] + nom_x[6];
out_4611641411917091701[7] = delta_x[7] + nom_x[7];
out_4611641411917091701[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) {
out_7289765730580897738[0] = -nom_x[0] + true_x[0];
out_7289765730580897738[1] = -nom_x[1] + true_x[1];
out_7289765730580897738[2] = -nom_x[2] + true_x[2];
out_7289765730580897738[3] = -nom_x[3] + true_x[3];
out_7289765730580897738[4] = -nom_x[4] + true_x[4];
out_7289765730580897738[5] = -nom_x[5] + true_x[5];
out_7289765730580897738[6] = -nom_x[6] + true_x[6];
out_7289765730580897738[7] = -nom_x[7] + true_x[7];
out_7289765730580897738[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) {
out_3968532722392907058[0] = -nom_x[0] + true_x[0];
out_3968532722392907058[1] = -nom_x[1] + true_x[1];
out_3968532722392907058[2] = -nom_x[2] + true_x[2];
out_3968532722392907058[3] = -nom_x[3] + true_x[3];
out_3968532722392907058[4] = -nom_x[4] + true_x[4];
out_3968532722392907058[5] = -nom_x[5] + true_x[5];
out_3968532722392907058[6] = -nom_x[6] + true_x[6];
out_3968532722392907058[7] = -nom_x[7] + true_x[7];
out_3968532722392907058[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_8304527178040913025) {
out_8304527178040913025[0] = 1.0;
out_8304527178040913025[1] = 0.0;
out_8304527178040913025[2] = 0.0;
out_8304527178040913025[3] = 0.0;
out_8304527178040913025[4] = 0.0;
out_8304527178040913025[5] = 0.0;
out_8304527178040913025[6] = 0.0;
out_8304527178040913025[7] = 0.0;
out_8304527178040913025[8] = 0.0;
out_8304527178040913025[9] = 0.0;
out_8304527178040913025[10] = 1.0;
out_8304527178040913025[11] = 0.0;
out_8304527178040913025[12] = 0.0;
out_8304527178040913025[13] = 0.0;
out_8304527178040913025[14] = 0.0;
out_8304527178040913025[15] = 0.0;
out_8304527178040913025[16] = 0.0;
out_8304527178040913025[17] = 0.0;
out_8304527178040913025[18] = 0.0;
out_8304527178040913025[19] = 0.0;
out_8304527178040913025[20] = 1.0;
out_8304527178040913025[21] = 0.0;
out_8304527178040913025[22] = 0.0;
out_8304527178040913025[23] = 0.0;
out_8304527178040913025[24] = 0.0;
out_8304527178040913025[25] = 0.0;
out_8304527178040913025[26] = 0.0;
out_8304527178040913025[27] = 0.0;
out_8304527178040913025[28] = 0.0;
out_8304527178040913025[29] = 0.0;
out_8304527178040913025[30] = 1.0;
out_8304527178040913025[31] = 0.0;
out_8304527178040913025[32] = 0.0;
out_8304527178040913025[33] = 0.0;
out_8304527178040913025[34] = 0.0;
out_8304527178040913025[35] = 0.0;
out_8304527178040913025[36] = 0.0;
out_8304527178040913025[37] = 0.0;
out_8304527178040913025[38] = 0.0;
out_8304527178040913025[39] = 0.0;
out_8304527178040913025[40] = 1.0;
out_8304527178040913025[41] = 0.0;
out_8304527178040913025[42] = 0.0;
out_8304527178040913025[43] = 0.0;
out_8304527178040913025[44] = 0.0;
out_8304527178040913025[45] = 0.0;
out_8304527178040913025[46] = 0.0;
out_8304527178040913025[47] = 0.0;
out_8304527178040913025[48] = 0.0;
out_8304527178040913025[49] = 0.0;
out_8304527178040913025[50] = 1.0;
out_8304527178040913025[51] = 0.0;
out_8304527178040913025[52] = 0.0;
out_8304527178040913025[53] = 0.0;
out_8304527178040913025[54] = 0.0;
out_8304527178040913025[55] = 0.0;
out_8304527178040913025[56] = 0.0;
out_8304527178040913025[57] = 0.0;
out_8304527178040913025[58] = 0.0;
out_8304527178040913025[59] = 0.0;
out_8304527178040913025[60] = 1.0;
out_8304527178040913025[61] = 0.0;
out_8304527178040913025[62] = 0.0;
out_8304527178040913025[63] = 0.0;
out_8304527178040913025[64] = 0.0;
out_8304527178040913025[65] = 0.0;
out_8304527178040913025[66] = 0.0;
out_8304527178040913025[67] = 0.0;
out_8304527178040913025[68] = 0.0;
out_8304527178040913025[69] = 0.0;
out_8304527178040913025[70] = 1.0;
out_8304527178040913025[71] = 0.0;
out_8304527178040913025[72] = 0.0;
out_8304527178040913025[73] = 0.0;
out_8304527178040913025[74] = 0.0;
out_8304527178040913025[75] = 0.0;
out_8304527178040913025[76] = 0.0;
out_8304527178040913025[77] = 0.0;
out_8304527178040913025[78] = 0.0;
out_8304527178040913025[79] = 0.0;
out_8304527178040913025[80] = 1.0;
void H_mod_fun(double *state, double *out_1232909156344202498) {
out_1232909156344202498[0] = 1.0;
out_1232909156344202498[1] = 0.0;
out_1232909156344202498[2] = 0.0;
out_1232909156344202498[3] = 0.0;
out_1232909156344202498[4] = 0.0;
out_1232909156344202498[5] = 0.0;
out_1232909156344202498[6] = 0.0;
out_1232909156344202498[7] = 0.0;
out_1232909156344202498[8] = 0.0;
out_1232909156344202498[9] = 0.0;
out_1232909156344202498[10] = 1.0;
out_1232909156344202498[11] = 0.0;
out_1232909156344202498[12] = 0.0;
out_1232909156344202498[13] = 0.0;
out_1232909156344202498[14] = 0.0;
out_1232909156344202498[15] = 0.0;
out_1232909156344202498[16] = 0.0;
out_1232909156344202498[17] = 0.0;
out_1232909156344202498[18] = 0.0;
out_1232909156344202498[19] = 0.0;
out_1232909156344202498[20] = 1.0;
out_1232909156344202498[21] = 0.0;
out_1232909156344202498[22] = 0.0;
out_1232909156344202498[23] = 0.0;
out_1232909156344202498[24] = 0.0;
out_1232909156344202498[25] = 0.0;
out_1232909156344202498[26] = 0.0;
out_1232909156344202498[27] = 0.0;
out_1232909156344202498[28] = 0.0;
out_1232909156344202498[29] = 0.0;
out_1232909156344202498[30] = 1.0;
out_1232909156344202498[31] = 0.0;
out_1232909156344202498[32] = 0.0;
out_1232909156344202498[33] = 0.0;
out_1232909156344202498[34] = 0.0;
out_1232909156344202498[35] = 0.0;
out_1232909156344202498[36] = 0.0;
out_1232909156344202498[37] = 0.0;
out_1232909156344202498[38] = 0.0;
out_1232909156344202498[39] = 0.0;
out_1232909156344202498[40] = 1.0;
out_1232909156344202498[41] = 0.0;
out_1232909156344202498[42] = 0.0;
out_1232909156344202498[43] = 0.0;
out_1232909156344202498[44] = 0.0;
out_1232909156344202498[45] = 0.0;
out_1232909156344202498[46] = 0.0;
out_1232909156344202498[47] = 0.0;
out_1232909156344202498[48] = 0.0;
out_1232909156344202498[49] = 0.0;
out_1232909156344202498[50] = 1.0;
out_1232909156344202498[51] = 0.0;
out_1232909156344202498[52] = 0.0;
out_1232909156344202498[53] = 0.0;
out_1232909156344202498[54] = 0.0;
out_1232909156344202498[55] = 0.0;
out_1232909156344202498[56] = 0.0;
out_1232909156344202498[57] = 0.0;
out_1232909156344202498[58] = 0.0;
out_1232909156344202498[59] = 0.0;
out_1232909156344202498[60] = 1.0;
out_1232909156344202498[61] = 0.0;
out_1232909156344202498[62] = 0.0;
out_1232909156344202498[63] = 0.0;
out_1232909156344202498[64] = 0.0;
out_1232909156344202498[65] = 0.0;
out_1232909156344202498[66] = 0.0;
out_1232909156344202498[67] = 0.0;
out_1232909156344202498[68] = 0.0;
out_1232909156344202498[69] = 0.0;
out_1232909156344202498[70] = 1.0;
out_1232909156344202498[71] = 0.0;
out_1232909156344202498[72] = 0.0;
out_1232909156344202498[73] = 0.0;
out_1232909156344202498[74] = 0.0;
out_1232909156344202498[75] = 0.0;
out_1232909156344202498[76] = 0.0;
out_1232909156344202498[77] = 0.0;
out_1232909156344202498[78] = 0.0;
out_1232909156344202498[79] = 0.0;
out_1232909156344202498[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_1355173870151422192) {
out_1355173870151422192[0] = state[0];
out_1355173870151422192[1] = state[1];
out_1355173870151422192[2] = state[2];
out_1355173870151422192[3] = state[3];
out_1355173870151422192[4] = state[4];
out_1355173870151422192[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_1355173870151422192[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_1355173870151422192[7] = state[7];
out_1355173870151422192[8] = state[8];
void f_fun(double *state, double dt, double *out_241923337051056220) {
out_241923337051056220[0] = state[0];
out_241923337051056220[1] = state[1];
out_241923337051056220[2] = state[2];
out_241923337051056220[3] = state[3];
out_241923337051056220[4] = state[4];
out_241923337051056220[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_241923337051056220[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_241923337051056220[7] = state[7];
out_241923337051056220[8] = state[8];
}
void F_fun(double *state, double dt, double *out_3219998669912464347) {
out_3219998669912464347[0] = 1;
out_3219998669912464347[1] = 0;
out_3219998669912464347[2] = 0;
out_3219998669912464347[3] = 0;
out_3219998669912464347[4] = 0;
out_3219998669912464347[5] = 0;
out_3219998669912464347[6] = 0;
out_3219998669912464347[7] = 0;
out_3219998669912464347[8] = 0;
out_3219998669912464347[9] = 0;
out_3219998669912464347[10] = 1;
out_3219998669912464347[11] = 0;
out_3219998669912464347[12] = 0;
out_3219998669912464347[13] = 0;
out_3219998669912464347[14] = 0;
out_3219998669912464347[15] = 0;
out_3219998669912464347[16] = 0;
out_3219998669912464347[17] = 0;
out_3219998669912464347[18] = 0;
out_3219998669912464347[19] = 0;
out_3219998669912464347[20] = 1;
out_3219998669912464347[21] = 0;
out_3219998669912464347[22] = 0;
out_3219998669912464347[23] = 0;
out_3219998669912464347[24] = 0;
out_3219998669912464347[25] = 0;
out_3219998669912464347[26] = 0;
out_3219998669912464347[27] = 0;
out_3219998669912464347[28] = 0;
out_3219998669912464347[29] = 0;
out_3219998669912464347[30] = 1;
out_3219998669912464347[31] = 0;
out_3219998669912464347[32] = 0;
out_3219998669912464347[33] = 0;
out_3219998669912464347[34] = 0;
out_3219998669912464347[35] = 0;
out_3219998669912464347[36] = 0;
out_3219998669912464347[37] = 0;
out_3219998669912464347[38] = 0;
out_3219998669912464347[39] = 0;
out_3219998669912464347[40] = 1;
out_3219998669912464347[41] = 0;
out_3219998669912464347[42] = 0;
out_3219998669912464347[43] = 0;
out_3219998669912464347[44] = 0;
out_3219998669912464347[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_3219998669912464347[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_3219998669912464347[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_3219998669912464347[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_3219998669912464347[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_3219998669912464347[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_3219998669912464347[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_3219998669912464347[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_3219998669912464347[53] = -9.8100000000000005*dt;
out_3219998669912464347[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_3219998669912464347[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_3219998669912464347[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_3219998669912464347[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_3219998669912464347[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_3219998669912464347[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_3219998669912464347[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_3219998669912464347[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_3219998669912464347[62] = 0;
out_3219998669912464347[63] = 0;
out_3219998669912464347[64] = 0;
out_3219998669912464347[65] = 0;
out_3219998669912464347[66] = 0;
out_3219998669912464347[67] = 0;
out_3219998669912464347[68] = 0;
out_3219998669912464347[69] = 0;
out_3219998669912464347[70] = 1;
out_3219998669912464347[71] = 0;
out_3219998669912464347[72] = 0;
out_3219998669912464347[73] = 0;
out_3219998669912464347[74] = 0;
out_3219998669912464347[75] = 0;
out_3219998669912464347[76] = 0;
out_3219998669912464347[77] = 0;
out_3219998669912464347[78] = 0;
out_3219998669912464347[79] = 0;
out_3219998669912464347[80] = 1;
void F_fun(double *state, double dt, double *out_2984795699500747305) {
out_2984795699500747305[0] = 1;
out_2984795699500747305[1] = 0;
out_2984795699500747305[2] = 0;
out_2984795699500747305[3] = 0;
out_2984795699500747305[4] = 0;
out_2984795699500747305[5] = 0;
out_2984795699500747305[6] = 0;
out_2984795699500747305[7] = 0;
out_2984795699500747305[8] = 0;
out_2984795699500747305[9] = 0;
out_2984795699500747305[10] = 1;
out_2984795699500747305[11] = 0;
out_2984795699500747305[12] = 0;
out_2984795699500747305[13] = 0;
out_2984795699500747305[14] = 0;
out_2984795699500747305[15] = 0;
out_2984795699500747305[16] = 0;
out_2984795699500747305[17] = 0;
out_2984795699500747305[18] = 0;
out_2984795699500747305[19] = 0;
out_2984795699500747305[20] = 1;
out_2984795699500747305[21] = 0;
out_2984795699500747305[22] = 0;
out_2984795699500747305[23] = 0;
out_2984795699500747305[24] = 0;
out_2984795699500747305[25] = 0;
out_2984795699500747305[26] = 0;
out_2984795699500747305[27] = 0;
out_2984795699500747305[28] = 0;
out_2984795699500747305[29] = 0;
out_2984795699500747305[30] = 1;
out_2984795699500747305[31] = 0;
out_2984795699500747305[32] = 0;
out_2984795699500747305[33] = 0;
out_2984795699500747305[34] = 0;
out_2984795699500747305[35] = 0;
out_2984795699500747305[36] = 0;
out_2984795699500747305[37] = 0;
out_2984795699500747305[38] = 0;
out_2984795699500747305[39] = 0;
out_2984795699500747305[40] = 1;
out_2984795699500747305[41] = 0;
out_2984795699500747305[42] = 0;
out_2984795699500747305[43] = 0;
out_2984795699500747305[44] = 0;
out_2984795699500747305[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_2984795699500747305[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_2984795699500747305[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_2984795699500747305[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_2984795699500747305[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_2984795699500747305[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_2984795699500747305[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_2984795699500747305[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_2984795699500747305[53] = -9.8100000000000005*dt;
out_2984795699500747305[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_2984795699500747305[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_2984795699500747305[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2984795699500747305[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2984795699500747305[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_2984795699500747305[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_2984795699500747305[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_2984795699500747305[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2984795699500747305[62] = 0;
out_2984795699500747305[63] = 0;
out_2984795699500747305[64] = 0;
out_2984795699500747305[65] = 0;
out_2984795699500747305[66] = 0;
out_2984795699500747305[67] = 0;
out_2984795699500747305[68] = 0;
out_2984795699500747305[69] = 0;
out_2984795699500747305[70] = 1;
out_2984795699500747305[71] = 0;
out_2984795699500747305[72] = 0;
out_2984795699500747305[73] = 0;
out_2984795699500747305[74] = 0;
out_2984795699500747305[75] = 0;
out_2984795699500747305[76] = 0;
out_2984795699500747305[77] = 0;
out_2984795699500747305[78] = 0;
out_2984795699500747305[79] = 0;
out_2984795699500747305[80] = 1;
}
void h_25(double *state, double *unused, double *out_7686598041594365016) {
out_7686598041594365016[0] = state[6];
void h_25(double *state, double *unused, double *out_4848168669919202886) {
out_4848168669919202886[0] = state[6];
}
void H_25(double *state, double *unused, double *out_2220878862694778786) {
out_2220878862694778786[0] = 0;
out_2220878862694778786[1] = 0;
out_2220878862694778786[2] = 0;
out_2220878862694778786[3] = 0;
out_2220878862694778786[4] = 0;
out_2220878862694778786[5] = 0;
out_2220878862694778786[6] = 1;
out_2220878862694778786[7] = 0;
out_2220878862694778786[8] = 0;
void H_25(double *state, double *unused, double *out_7960279069568034739) {
out_7960279069568034739[0] = 0;
out_7960279069568034739[1] = 0;
out_7960279069568034739[2] = 0;
out_7960279069568034739[3] = 0;
out_7960279069568034739[4] = 0;
out_7960279069568034739[5] = 0;
out_7960279069568034739[6] = 1;
out_7960279069568034739[7] = 0;
out_7960279069568034739[8] = 0;
}
void h_24(double *state, double *unused, double *out_3813839643785118531) {
out_3813839643785118531[0] = state[4];
out_3813839643785118531[1] = state[5];
void h_24(double *state, double *unused, double *out_5973332433336352466) {
out_5973332433336352466[0] = state[4];
out_5973332433336352466[1] = state[5];
}
void H_24(double *state, double *unused, double *out_1337046736107544350) {
out_1337046736107544350[0] = 0;
out_1337046736107544350[1] = 0;
out_1337046736107544350[2] = 0;
out_1337046736107544350[3] = 0;
out_1337046736107544350[4] = 1;
out_1337046736107544350[5] = 0;
out_1337046736107544350[6] = 0;
out_1337046736107544350[7] = 0;
out_1337046736107544350[8] = 0;
out_1337046736107544350[9] = 0;
out_1337046736107544350[10] = 0;
out_1337046736107544350[11] = 0;
out_1337046736107544350[12] = 0;
out_1337046736107544350[13] = 0;
out_1337046736107544350[14] = 1;
out_1337046736107544350[15] = 0;
out_1337046736107544350[16] = 0;
out_1337046736107544350[17] = 0;
void H_24(double *state, double *unused, double *out_8313815405136017311) {
out_8313815405136017311[0] = 0;
out_8313815405136017311[1] = 0;
out_8313815405136017311[2] = 0;
out_8313815405136017311[3] = 0;
out_8313815405136017311[4] = 1;
out_8313815405136017311[5] = 0;
out_8313815405136017311[6] = 0;
out_8313815405136017311[7] = 0;
out_8313815405136017311[8] = 0;
out_8313815405136017311[9] = 0;
out_8313815405136017311[10] = 0;
out_8313815405136017311[11] = 0;
out_8313815405136017311[12] = 0;
out_8313815405136017311[13] = 0;
out_8313815405136017311[14] = 1;
out_8313815405136017311[15] = 0;
out_8313815405136017311[16] = 0;
out_8313815405136017311[17] = 0;
}
void h_30(double *state, double *unused, double *out_2120268555396644716) {
out_2120268555396644716[0] = state[4];
void h_30(double *state, double *unused, double *out_7443128207949688601) {
out_7443128207949688601[0] = state[4];
}
void H_30(double *state, double *unused, double *out_297454095812469841) {
out_297454095812469841[0] = 0;
out_297454095812469841[1] = 0;
out_297454095812469841[2] = 0;
out_297454095812469841[3] = 0;
out_297454095812469841[4] = 1;
out_297454095812469841[5] = 0;
out_297454095812469841[6] = 0;
out_297454095812469841[7] = 0;
out_297454095812469841[8] = 0;
void H_30(double *state, double *unused, double *out_5441946111060786112) {
out_5441946111060786112[0] = 0;
out_5441946111060786112[1] = 0;
out_5441946111060786112[2] = 0;
out_5441946111060786112[3] = 0;
out_5441946111060786112[4] = 1;
out_5441946111060786112[5] = 0;
out_5441946111060786112[6] = 0;
out_5441946111060786112[7] = 0;
out_5441946111060786112[8] = 0;
}
void h_26(double *state, double *unused, double *out_2881404712264200929) {
out_2881404712264200929[0] = state[7];
void h_26(double *state, double *unused, double *out_5333477160662303659) {
out_5333477160662303659[0] = state[7];
}
void H_26(double *state, double *unused, double *out_5962382181568835010) {
out_5962382181568835010[0] = 0;
out_5962382181568835010[1] = 0;
out_5962382181568835010[2] = 0;
out_5962382181568835010[3] = 0;
out_5962382181568835010[4] = 0;
out_5962382181568835010[5] = 0;
out_5962382181568835010[6] = 0;
out_5962382181568835010[7] = 1;
out_5962382181568835010[8] = 0;
void H_26(double *state, double *unused, double *out_6744961685267460653) {
out_6744961685267460653[0] = 0;
out_6744961685267460653[1] = 0;
out_6744961685267460653[2] = 0;
out_6744961685267460653[3] = 0;
out_6744961685267460653[4] = 0;
out_6744961685267460653[5] = 0;
out_6744961685267460653[6] = 0;
out_6744961685267460653[7] = 1;
out_6744961685267460653[8] = 0;
}
void h_27(double *state, double *unused, double *out_901564640210476392) {
out_901564640210476392[0] = state[3];
void h_27(double *state, double *unused, double *out_5010412721985507322) {
out_5010412721985507322[0] = state[3];
}
void H_27(double *state, double *unused, double *out_2521048166996413058) {
out_2521048166996413058[0] = 0;
out_2521048166996413058[1] = 0;
out_2521048166996413058[2] = 0;
out_2521048166996413058[3] = 1;
out_2521048166996413058[4] = 0;
out_2521048166996413058[5] = 0;
out_2521048166996413058[6] = 0;
out_2521048166996413058[7] = 0;
out_2521048166996413058[8] = 0;
void H_27(double *state, double *unused, double *out_8182362745197851896) {
out_8182362745197851896[0] = 0;
out_8182362745197851896[1] = 0;
out_8182362745197851896[2] = 0;
out_8182362745197851896[3] = 1;
out_8182362745197851896[4] = 0;
out_8182362745197851896[5] = 0;
out_8182362745197851896[6] = 0;
out_8182362745197851896[7] = 0;
out_8182362745197851896[8] = 0;
}
void h_29(double *state, double *unused, double *out_952524754320502979) {
out_952524754320502979[0] = state[1];
void h_29(double *state, double *unused, double *out_8399210544951165364) {
out_8399210544951165364[0] = state[1];
}
void H_29(double *state, double *unused, double *out_807685440126862025) {
out_807685440126862025[0] = 0;
out_807685440126862025[1] = 1;
out_807685440126862025[2] = 0;
out_807685440126862025[3] = 0;
out_807685440126862025[4] = 0;
out_807685440126862025[5] = 0;
out_807685440126862025[6] = 0;
out_807685440126862025[7] = 0;
out_807685440126862025[8] = 0;
void H_29(double *state, double *unused, double *out_6469000018328300863) {
out_6469000018328300863[0] = 0;
out_6469000018328300863[1] = 1;
out_6469000018328300863[2] = 0;
out_6469000018328300863[3] = 0;
out_6469000018328300863[4] = 0;
out_6469000018328300863[5] = 0;
out_6469000018328300863[6] = 0;
out_6469000018328300863[7] = 0;
out_6469000018328300863[8] = 0;
}
void h_28(double *state, double *unused, double *out_8550909813521810734) {
out_8550909813521810734[0] = state[0];
void h_28(double *state, double *unused, double *out_2638932451325827020) {
out_2638932451325827020[0] = state[0];
}
void H_28(double *state, double *unused, double *out_4274713576942668549) {
out_4274713576942668549[0] = 1;
out_4274713576942668549[1] = 0;
out_4274713576942668549[2] = 0;
out_4274713576942668549[3] = 0;
out_4274713576942668549[4] = 0;
out_4274713576942668549[5] = 0;
out_4274713576942668549[6] = 0;
out_4274713576942668549[7] = 0;
out_4274713576942668549[8] = 0;
void H_28(double *state, double *unused, double *out_8432630289893627114) {
out_8432630289893627114[0] = 1;
out_8432630289893627114[1] = 0;
out_8432630289893627114[2] = 0;
out_8432630289893627114[3] = 0;
out_8432630289893627114[4] = 0;
out_8432630289893627114[5] = 0;
out_8432630289893627114[6] = 0;
out_8432630289893627114[7] = 0;
out_8432630289893627114[8] = 0;
}
void h_31(double *state, double *unused, double *out_7961792103878870905) {
out_7961792103878870905[0] = state[8];
void h_31(double *state, double *unused, double *out_4572974607634696997) {
out_4572974607634696997[0] = state[8];
}
void H_31(double *state, double *unused, double *out_6588590283802186486) {
out_6588590283802186486[0] = 0;
out_6588590283802186486[1] = 0;
out_6588590283802186486[2] = 0;
out_6588590283802186486[3] = 0;
out_6588590283802186486[4] = 0;
out_6588590283802186486[5] = 0;
out_6588590283802186486[6] = 0;
out_6588590283802186486[7] = 0;
out_6588590283802186486[8] = 1;
void H_31(double *state, double *unused, double *out_6118753583034109177) {
out_6118753583034109177[0] = 0;
out_6118753583034109177[1] = 0;
out_6118753583034109177[2] = 0;
out_6118753583034109177[3] = 0;
out_6118753583034109177[4] = 0;
out_6118753583034109177[5] = 0;
out_6118753583034109177[6] = 0;
out_6118753583034109177[7] = 0;
out_6118753583034109177[8] = 1;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
}
void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) {
err_fun(nom_x, delta_x, out_1461273597009064329);
void car_err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701) {
err_fun(nom_x, delta_x, out_4611641411917091701);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) {
inv_err_fun(nom_x, true_x, out_7289765730580897738);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) {
inv_err_fun(nom_x, true_x, out_3968532722392907058);
}
void car_H_mod_fun(double *state, double *out_8304527178040913025) {
H_mod_fun(state, out_8304527178040913025);
void car_H_mod_fun(double *state, double *out_1232909156344202498) {
H_mod_fun(state, out_1232909156344202498);
}
void car_f_fun(double *state, double dt, double *out_1355173870151422192) {
f_fun(state, dt, out_1355173870151422192);
void car_f_fun(double *state, double dt, double *out_241923337051056220) {
f_fun(state, dt, out_241923337051056220);
}
void car_F_fun(double *state, double dt, double *out_3219998669912464347) {
F_fun(state, dt, out_3219998669912464347);
void car_F_fun(double *state, double dt, double *out_2984795699500747305) {
F_fun(state, dt, out_2984795699500747305);
}
void car_h_25(double *state, double *unused, double *out_7686598041594365016) {
h_25(state, unused, out_7686598041594365016);
void car_h_25(double *state, double *unused, double *out_4848168669919202886) {
h_25(state, unused, out_4848168669919202886);
}
void car_H_25(double *state, double *unused, double *out_2220878862694778786) {
H_25(state, unused, out_2220878862694778786);
void car_H_25(double *state, double *unused, double *out_7960279069568034739) {
H_25(state, unused, out_7960279069568034739);
}
void car_h_24(double *state, double *unused, double *out_3813839643785118531) {
h_24(state, unused, out_3813839643785118531);
void car_h_24(double *state, double *unused, double *out_5973332433336352466) {
h_24(state, unused, out_5973332433336352466);
}
void car_H_24(double *state, double *unused, double *out_1337046736107544350) {
H_24(state, unused, out_1337046736107544350);
void car_H_24(double *state, double *unused, double *out_8313815405136017311) {
H_24(state, unused, out_8313815405136017311);
}
void car_h_30(double *state, double *unused, double *out_2120268555396644716) {
h_30(state, unused, out_2120268555396644716);
void car_h_30(double *state, double *unused, double *out_7443128207949688601) {
h_30(state, unused, out_7443128207949688601);
}
void car_H_30(double *state, double *unused, double *out_297454095812469841) {
H_30(state, unused, out_297454095812469841);
void car_H_30(double *state, double *unused, double *out_5441946111060786112) {
H_30(state, unused, out_5441946111060786112);
}
void car_h_26(double *state, double *unused, double *out_2881404712264200929) {
h_26(state, unused, out_2881404712264200929);
void car_h_26(double *state, double *unused, double *out_5333477160662303659) {
h_26(state, unused, out_5333477160662303659);
}
void car_H_26(double *state, double *unused, double *out_5962382181568835010) {
H_26(state, unused, out_5962382181568835010);
void car_H_26(double *state, double *unused, double *out_6744961685267460653) {
H_26(state, unused, out_6744961685267460653);
}
void car_h_27(double *state, double *unused, double *out_901564640210476392) {
h_27(state, unused, out_901564640210476392);
void car_h_27(double *state, double *unused, double *out_5010412721985507322) {
h_27(state, unused, out_5010412721985507322);
}
void car_H_27(double *state, double *unused, double *out_2521048166996413058) {
H_27(state, unused, out_2521048166996413058);
void car_H_27(double *state, double *unused, double *out_8182362745197851896) {
H_27(state, unused, out_8182362745197851896);
}
void car_h_29(double *state, double *unused, double *out_952524754320502979) {
h_29(state, unused, out_952524754320502979);
void car_h_29(double *state, double *unused, double *out_8399210544951165364) {
h_29(state, unused, out_8399210544951165364);
}
void car_H_29(double *state, double *unused, double *out_807685440126862025) {
H_29(state, unused, out_807685440126862025);
void car_H_29(double *state, double *unused, double *out_6469000018328300863) {
H_29(state, unused, out_6469000018328300863);
}
void car_h_28(double *state, double *unused, double *out_8550909813521810734) {
h_28(state, unused, out_8550909813521810734);
void car_h_28(double *state, double *unused, double *out_2638932451325827020) {
h_28(state, unused, out_2638932451325827020);
}
void car_H_28(double *state, double *unused, double *out_4274713576942668549) {
H_28(state, unused, out_4274713576942668549);
void car_H_28(double *state, double *unused, double *out_8432630289893627114) {
H_28(state, unused, out_8432630289893627114);
}
void car_h_31(double *state, double *unused, double *out_7961792103878870905) {
h_31(state, unused, out_7961792103878870905);
void car_h_31(double *state, double *unused, double *out_4572974607634696997) {
h_31(state, unused, out_4572974607634696997);
}
void car_H_31(double *state, double *unused, double *out_6588590283802186486) {
H_31(state, unused, out_6588590283802186486);
void car_H_31(double *state, double *unused, double *out_6118753583034109177) {
H_31(state, unused, out_6118753583034109177);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738);
void car_H_mod_fun(double *state, double *out_8304527178040913025);
void car_f_fun(double *state, double dt, double *out_1355173870151422192);
void car_F_fun(double *state, double dt, double *out_3219998669912464347);
void car_h_25(double *state, double *unused, double *out_7686598041594365016);
void car_H_25(double *state, double *unused, double *out_2220878862694778786);
void car_h_24(double *state, double *unused, double *out_3813839643785118531);
void car_H_24(double *state, double *unused, double *out_1337046736107544350);
void car_h_30(double *state, double *unused, double *out_2120268555396644716);
void car_H_30(double *state, double *unused, double *out_297454095812469841);
void car_h_26(double *state, double *unused, double *out_2881404712264200929);
void car_H_26(double *state, double *unused, double *out_5962382181568835010);
void car_h_27(double *state, double *unused, double *out_901564640210476392);
void car_H_27(double *state, double *unused, double *out_2521048166996413058);
void car_h_29(double *state, double *unused, double *out_952524754320502979);
void car_H_29(double *state, double *unused, double *out_807685440126862025);
void car_h_28(double *state, double *unused, double *out_8550909813521810734);
void car_H_28(double *state, double *unused, double *out_4274713576942668549);
void car_h_31(double *state, double *unused, double *out_7961792103878870905);
void car_H_31(double *state, double *unused, double *out_6588590283802186486);
void car_err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058);
void car_H_mod_fun(double *state, double *out_1232909156344202498);
void car_f_fun(double *state, double dt, double *out_241923337051056220);
void car_F_fun(double *state, double dt, double *out_2984795699500747305);
void car_h_25(double *state, double *unused, double *out_4848168669919202886);
void car_H_25(double *state, double *unused, double *out_7960279069568034739);
void car_h_24(double *state, double *unused, double *out_5973332433336352466);
void car_H_24(double *state, double *unused, double *out_8313815405136017311);
void car_h_30(double *state, double *unused, double *out_7443128207949688601);
void car_H_30(double *state, double *unused, double *out_5441946111060786112);
void car_h_26(double *state, double *unused, double *out_5333477160662303659);
void car_H_26(double *state, double *unused, double *out_6744961685267460653);
void car_h_27(double *state, double *unused, double *out_5010412721985507322);
void car_H_27(double *state, double *unused, double *out_8182362745197851896);
void car_h_29(double *state, double *unused, double *out_8399210544951165364);
void car_H_29(double *state, double *unused, double *out_6469000018328300863);
void car_h_28(double *state, double *unused, double *out_2638932451325827020);
void car_H_28(double *state, double *unused, double *out_8432630289893627114);
void car_h_31(double *state, double *unused, double *out_4572974607634696997);
void car_H_31(double *state, double *unused, double *out_6118753583034109177);
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
void car_set_mass(double x);
void car_set_rotational_inertia(double x);
selfdrive/locationd/models/generated/pose.cpp
+969 -969
View File
File diff suppressed because it is too large Load Diff
selfdrive/locationd/models/generated/pose.h
+13 -13
View File
@@ -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_7352766374165151156);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654);
void pose_H_mod_fun(double *state, double *out_2863715751586478773);
void pose_f_fun(double *state, double dt, double *out_4163803070138990822);
void pose_F_fun(double *state, double dt, double *out_3325085581577154625);
void pose_h_4(double *state, double *unused, double *out_1824245269356126422);
void pose_H_4(double *state, double *unused, double *out_908091458257428799);
void pose_h_10(double *state, double *unused, double *out_5992945801677381357);
void pose_H_10(double *state, double *unused, double *out_1551783764236526040);
void pose_h_13(double *state, double *unused, double *out_4650572533653388734);
void pose_H_13(double *state, double *unused, double *out_1472693377939272903);
void pose_h_14(double *state, double *unused, double *out_9091688560146400700);
void pose_H_14(double *state, double *unused, double *out_2174696974037943497);
void pose_err_fun(double *nom_x, double *delta_x, double *out_8612662017984698055);
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086);
void pose_H_mod_fun(double *state, double *out_7537065087244600898);
void pose_f_fun(double *state, double dt, double *out_6289537546199484080);
void pose_F_fun(double *state, double dt, double *out_3263235949829739422);
void pose_h_4(double *state, double *unused, double *out_6871661940623032085);
void pose_H_4(double *state, double *unused, double *out_5061597179712991997);
void pose_h_10(double *state, double *unused, double *out_974669649895256171);
void pose_H_10(double *state, double *unused, double *out_3862977066975629010);
void pose_h_13(double *state, double *unused, double *out_2719357598944466940);
void pose_H_13(double *state, double *unused, double *out_8273871005045324798);
void pose_h_14(double *state, double *unused, double *out_5270922229605318979);
void pose_H_14(double *state, double *unused, double *out_9024838036052476526);
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/ui/ui
BIN
View File
Binary file not shown.