mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-29 19:03:59 +08:00
nav / planner / sonata buttons
This commit is contained in:
firestar5683
@@ -8,7 +8,7 @@ from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, HyundaiStarPilotFlags, HyundaiStarPilotSafetyFlags, CAR, DBC, Buttons, CarControllerParams, \
|
||||
hyundai_cancel_button_enables_cruise, ALT_BUS_LDA_BUTTON_CARS
|
||||
hyundai_cancel_button_enables_cruise, ALT_BUS_LDA_BUTTON_CARS, ALT_BUS_LDA_BUTTON_SWL_STAT_CARS
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
@@ -175,8 +175,12 @@ class CarState(CarStateBase):
|
||||
return False
|
||||
|
||||
def create_alt_bus_lda_button_events(self, cp_source: CANParser) -> list[structs.CarState.ButtonEvent]:
|
||||
raw_lda_button = int(cp_source.vl["CLU13"]["CF_Clu_LdwsLkasSW"])
|
||||
raw_lda_button_ts_nanos = cp_source.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"]
|
||||
if self.CP.carFingerprint in ALT_BUS_LDA_BUTTON_SWL_STAT_CARS:
|
||||
raw_lda_button = int(cp_source.vl["CLU13"]["CF_Clu_SWL_Stat"] == 4)
|
||||
raw_lda_button_ts_nanos = cp_source.ts_nanos["CLU13"]["CF_Clu_SWL_Stat"]
|
||||
else:
|
||||
raw_lda_button = int(cp_source.vl["CLU13"]["CF_Clu_LdwsLkasSW"])
|
||||
raw_lda_button_ts_nanos = cp_source.ts_nanos["CLU13"]["CF_Clu_LdwsLkasSW"]
|
||||
button_events: list[structs.CarState.ButtonEvent] = []
|
||||
|
||||
# Some alt-bus LKAS button layouts pulse several times per physical press burst.
|
||||
|
||||
@@ -629,6 +629,43 @@ class TestHyundaiFingerprint:
|
||||
ret = update(0, 3)
|
||||
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
|
||||
|
||||
def test_sonata_hybrid_uses_alt_bus_lkas_parser(self):
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x391] = 8
|
||||
fingerprint[1][0x50C] = 8
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_SONATA_HYBRID, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA_HYBRID, fingerprint, [], CP, toggles)
|
||||
|
||||
car_state = CarState(CP, FPCP)
|
||||
can_parsers = car_state.get_can_parsers(CP)
|
||||
|
||||
assert Bus.alt in can_parsers
|
||||
|
||||
def test_sonata_hybrid_alt_bus_clu13_lkas_button_event(self):
|
||||
toggles = get_test_toggles()
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x391] = 8
|
||||
fingerprint[1][0x50C] = 8
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_SONATA_HYBRID, fingerprint, [], False, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA_HYBRID, fingerprint, [], CP, toggles)
|
||||
|
||||
car_state = CarState(CP, FPCP)
|
||||
can_parsers = car_state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
def update(lkas_button: int, frame: int):
|
||||
msg = packer.make_can_msg("CLU13", 1, {
|
||||
"CF_Clu_SWL_Stat": lkas_button,
|
||||
})
|
||||
can_parsers[Bus.alt].update([(frame, [msg])])
|
||||
return car_state.update(can_parsers, toggles)[0]
|
||||
|
||||
update(0, 1)
|
||||
ret = update(4, 2)
|
||||
assert any(be.type == ButtonType.lkas and be.pressed for be in ret.buttonEvents)
|
||||
assert any(be.type == ButtonType.lkas and not be.pressed for be in ret.buttonEvents)
|
||||
|
||||
def test_genesis_g90_does_not_use_alt_bus_lkas_parser(self):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.GENESIS_G90, gen_empty_fingerprint(), [], False, False, False, toggles)
|
||||
|
||||
@@ -927,10 +927,20 @@ CANCEL_BUTTON_ENABLE_CARS = frozenset({
|
||||
})
|
||||
|
||||
|
||||
# No classic HKG platforms are currently opted into the synthetic alt-bus LKAS
|
||||
# button path. The G90 regression is mitigated by falling back to the standard
|
||||
# Hyundai LKAS button handling until a route-proven alt-bus implementation is ready.
|
||||
ALT_BUS_LDA_BUTTON_CARS = frozenset()
|
||||
# These classic HKG platforms publish the LKAS button on CLU13 over the alt bus.
|
||||
# Keep G90 excluded until its alt-bus path is route-proven without the recent
|
||||
# engage/disengage regression.
|
||||
ALT_BUS_LDA_BUTTON_CARS = frozenset({
|
||||
CAR.HYUNDAI_SONATA,
|
||||
CAR.HYUNDAI_SONATA_HYBRID,
|
||||
})
|
||||
|
||||
# On these Sonata layouts the alt-bus LKAS button pulses through the CLU13
|
||||
# steering-wheel-status field instead of the dedicated LKAS bit.
|
||||
ALT_BUS_LDA_BUTTON_SWL_STAT_CARS = frozenset({
|
||||
CAR.HYUNDAI_SONATA,
|
||||
CAR.HYUNDAI_SONATA_HYBRID,
|
||||
})
|
||||
|
||||
|
||||
def hyundai_cancel_button_enables_cruise(car_fingerprint) -> bool:
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-3de95a34-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-6ca397b8-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-3de95a34-DEBUG
|
||||
DEV-6ca397b8-DEBUG
|
||||
@@ -224,6 +224,18 @@ FAR_LEAD_COMFORT_BRAKE_CAP_FULL_HEADWAY_MARGIN = 1.00
|
||||
FAR_LEAD_COMFORT_BRAKE_CAP_MIN_DECEL = 0.05
|
||||
FAR_LEAD_COMFORT_BRAKE_CAP_MAX_DECEL = 0.18
|
||||
FAR_LEAD_COMFORT_BRAKE_CAP_FULL_RELAX_DECEL = 0.05
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN = 0.25
|
||||
MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN = 0.75
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB = 0.9
|
||||
MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE = 0.18
|
||||
MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED = 1.75
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_TTC = 12.0
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP = 0.08
|
||||
MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP = 0.18
|
||||
MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP = 0.08
|
||||
MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP = 0.16
|
||||
MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP = 0.10
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95
|
||||
TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_DECEL = 0.80
|
||||
@@ -1186,6 +1198,74 @@ class LongitudinalPlanner:
|
||||
))
|
||||
return -max(0.0, cap_decel - relax_decel)
|
||||
|
||||
def get_matched_follow_transition_target(self, lead, v_ego, base_t_follow, prev_output_a_target, output_a_target):
|
||||
if lead is None or not lead.status:
|
||||
return None
|
||||
if float(v_ego) < MATCHED_FOLLOW_TRANSITION_MIN_SPEED:
|
||||
return None
|
||||
|
||||
lead_prob = float(getattr(lead, "modelProb", 0.0))
|
||||
if lead_prob < MATCHED_FOLLOW_TRANSITION_MIN_MODEL_PROB:
|
||||
return None
|
||||
|
||||
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
|
||||
if lead_brake > MATCHED_FOLLOW_TRANSITION_MAX_LEAD_BRAKE:
|
||||
return None
|
||||
|
||||
relative_speed = float(v_ego) - float(lead.vLead)
|
||||
if not (STEADY_FOLLOW_BRAKE_CAP_MIN_REL_SPEED <= relative_speed <= STEADY_FOLLOW_SMOOTHING_MAX_CLOSING_SPEED):
|
||||
return None
|
||||
|
||||
closing_speed = max(0.0, relative_speed)
|
||||
if closing_speed > MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED:
|
||||
return None
|
||||
|
||||
ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
|
||||
if ttc < MATCHED_FOLLOW_TRANSITION_MIN_TTC:
|
||||
return None
|
||||
|
||||
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
|
||||
headway_margin = actual_headway - float(base_t_follow)
|
||||
if headway_margin < MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN:
|
||||
return None
|
||||
if actual_headway > float(base_t_follow) + STEADY_FOLLOW_BRAKE_CAP_MAX_HEADWAY_ABOVE_TARGET:
|
||||
return None
|
||||
|
||||
target_delta = float(output_a_target) - float(prev_output_a_target)
|
||||
if abs(target_delta) < 1e-3:
|
||||
return None
|
||||
|
||||
headway_factor = float(np.clip(
|
||||
(headway_margin - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN) /
|
||||
max(MATCHED_FOLLOW_TRANSITION_FULL_HEADWAY_MARGIN - MATCHED_FOLLOW_TRANSITION_MIN_HEADWAY_MARGIN, 1e-3),
|
||||
0.0,
|
||||
1.0,
|
||||
))
|
||||
|
||||
positive_step = float(np.interp(
|
||||
max(float(lead.vLead) - float(v_ego), 0.0),
|
||||
[0.0, 1.0],
|
||||
[MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_POSITIVE_STEP],
|
||||
))
|
||||
negative_step = float(np.interp(
|
||||
closing_speed,
|
||||
[0.0, MATCHED_FOLLOW_TRANSITION_MAX_CLOSING_SPEED],
|
||||
[MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP, MATCHED_FOLLOW_TRANSITION_MAX_NEGATIVE_STEP],
|
||||
))
|
||||
|
||||
# The more space we still have, the less abrupt the comfort path should be.
|
||||
positive_step = float(np.interp(headway_factor, [0.0, 1.0], [positive_step, MATCHED_FOLLOW_TRANSITION_MIN_POSITIVE_STEP]))
|
||||
negative_step = float(np.interp(headway_factor, [0.0, 1.0], [negative_step, MATCHED_FOLLOW_TRANSITION_MIN_NEGATIVE_STEP]))
|
||||
|
||||
if float(prev_output_a_target) * float(output_a_target) < 0.0:
|
||||
positive_step = min(positive_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP)
|
||||
negative_step = min(negative_step, MATCHED_FOLLOW_TRANSITION_SIGN_CROSS_STEP)
|
||||
|
||||
lower = float(prev_output_a_target) - negative_step
|
||||
upper = float(prev_output_a_target) + positive_step
|
||||
smoothed_target = float(np.clip(output_a_target, lower, upper))
|
||||
return smoothed_target if abs(smoothed_target - float(output_a_target)) > 1e-6 else None
|
||||
|
||||
def get_tracked_vision_model_brake_floor(self, lead, v_ego, accel_min, t_follow, model_desired):
|
||||
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
|
||||
return None
|
||||
@@ -1886,6 +1966,21 @@ class LongitudinalPlanner:
|
||||
self.a_desired = max(self.a_desired, tracked_vision_model_brake_cap)
|
||||
output_a_target = max(output_a_target, tracked_vision_model_brake_cap)
|
||||
|
||||
if optional_far_lead_comfort and follow_control_lead is not None and not panic_bypass and not output_should_stop and not vision_low_speed_stop_active:
|
||||
matched_follow_transition_target = self.get_matched_follow_transition_target(
|
||||
follow_control_lead,
|
||||
scene_v_ego,
|
||||
effective_t_follow,
|
||||
prev_output_a_target,
|
||||
output_a_target,
|
||||
)
|
||||
if matched_follow_transition_target is not None:
|
||||
if matched_follow_transition_target < output_a_target:
|
||||
self.a_desired = min(self.a_desired, matched_follow_transition_target)
|
||||
else:
|
||||
self.a_desired = max(self.a_desired, matched_follow_transition_target)
|
||||
output_a_target = matched_follow_transition_target
|
||||
|
||||
output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
|
||||
output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
|
||||
|
||||
|
||||
@@ -1748,3 +1748,39 @@ def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead():
|
||||
assert cap is not None
|
||||
assert cap > -0.2
|
||||
assert cap < -0.05
|
||||
|
||||
|
||||
def test_matched_follow_transition_target_damps_large_comfort_sign_flip():
|
||||
v_ego = 20.3
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead = make_lead(status=True, d_rel=45.6, v_lead=19.19, a_lead=0.0, radar=False, model_prob=0.99)
|
||||
|
||||
smoothed = planner.get_matched_follow_transition_target(
|
||||
lead,
|
||||
v_ego,
|
||||
1.45,
|
||||
prev_output_a_target=0.12,
|
||||
output_a_target=-0.40,
|
||||
)
|
||||
|
||||
assert smoothed is not None
|
||||
assert smoothed > -0.05
|
||||
assert smoothed < 0.12
|
||||
|
||||
|
||||
def test_matched_follow_transition_target_skips_urgent_closure():
|
||||
v_ego = 31.5
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
lead = make_lead(status=True, d_rel=35.0, v_lead=28.0, a_lead=0.0, radar=False, model_prob=0.99)
|
||||
|
||||
smoothed = planner.get_matched_follow_transition_target(
|
||||
lead,
|
||||
v_ego,
|
||||
1.45,
|
||||
prev_output_a_target=0.10,
|
||||
output_a_target=-0.60,
|
||||
)
|
||||
|
||||
assert smoothed is None
|
||||
|
||||
@@ -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_8206107047934752633) {
|
||||
out_8206107047934752633[0] = delta_x[0] + nom_x[0];
|
||||
out_8206107047934752633[1] = delta_x[1] + nom_x[1];
|
||||
out_8206107047934752633[2] = delta_x[2] + nom_x[2];
|
||||
out_8206107047934752633[3] = delta_x[3] + nom_x[3];
|
||||
out_8206107047934752633[4] = delta_x[4] + nom_x[4];
|
||||
out_8206107047934752633[5] = delta_x[5] + nom_x[5];
|
||||
out_8206107047934752633[6] = delta_x[6] + nom_x[6];
|
||||
out_8206107047934752633[7] = delta_x[7] + nom_x[7];
|
||||
out_8206107047934752633[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_4770141140221383025) {
|
||||
out_4770141140221383025[0] = delta_x[0] + nom_x[0];
|
||||
out_4770141140221383025[1] = delta_x[1] + nom_x[1];
|
||||
out_4770141140221383025[2] = delta_x[2] + nom_x[2];
|
||||
out_4770141140221383025[3] = delta_x[3] + nom_x[3];
|
||||
out_4770141140221383025[4] = delta_x[4] + nom_x[4];
|
||||
out_4770141140221383025[5] = delta_x[5] + nom_x[5];
|
||||
out_4770141140221383025[6] = delta_x[6] + nom_x[6];
|
||||
out_4770141140221383025[7] = delta_x[7] + nom_x[7];
|
||||
out_4770141140221383025[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_3582621965851683853) {
|
||||
out_3582621965851683853[0] = -nom_x[0] + true_x[0];
|
||||
out_3582621965851683853[1] = -nom_x[1] + true_x[1];
|
||||
out_3582621965851683853[2] = -nom_x[2] + true_x[2];
|
||||
out_3582621965851683853[3] = -nom_x[3] + true_x[3];
|
||||
out_3582621965851683853[4] = -nom_x[4] + true_x[4];
|
||||
out_3582621965851683853[5] = -nom_x[5] + true_x[5];
|
||||
out_3582621965851683853[6] = -nom_x[6] + true_x[6];
|
||||
out_3582621965851683853[7] = -nom_x[7] + true_x[7];
|
||||
out_3582621965851683853[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8267480455965364332) {
|
||||
out_8267480455965364332[0] = -nom_x[0] + true_x[0];
|
||||
out_8267480455965364332[1] = -nom_x[1] + true_x[1];
|
||||
out_8267480455965364332[2] = -nom_x[2] + true_x[2];
|
||||
out_8267480455965364332[3] = -nom_x[3] + true_x[3];
|
||||
out_8267480455965364332[4] = -nom_x[4] + true_x[4];
|
||||
out_8267480455965364332[5] = -nom_x[5] + true_x[5];
|
||||
out_8267480455965364332[6] = -nom_x[6] + true_x[6];
|
||||
out_8267480455965364332[7] = -nom_x[7] + true_x[7];
|
||||
out_8267480455965364332[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_3381247545329584694) {
|
||||
out_3381247545329584694[0] = 1.0;
|
||||
out_3381247545329584694[1] = 0.0;
|
||||
out_3381247545329584694[2] = 0.0;
|
||||
out_3381247545329584694[3] = 0.0;
|
||||
out_3381247545329584694[4] = 0.0;
|
||||
out_3381247545329584694[5] = 0.0;
|
||||
out_3381247545329584694[6] = 0.0;
|
||||
out_3381247545329584694[7] = 0.0;
|
||||
out_3381247545329584694[8] = 0.0;
|
||||
out_3381247545329584694[9] = 0.0;
|
||||
out_3381247545329584694[10] = 1.0;
|
||||
out_3381247545329584694[11] = 0.0;
|
||||
out_3381247545329584694[12] = 0.0;
|
||||
out_3381247545329584694[13] = 0.0;
|
||||
out_3381247545329584694[14] = 0.0;
|
||||
out_3381247545329584694[15] = 0.0;
|
||||
out_3381247545329584694[16] = 0.0;
|
||||
out_3381247545329584694[17] = 0.0;
|
||||
out_3381247545329584694[18] = 0.0;
|
||||
out_3381247545329584694[19] = 0.0;
|
||||
out_3381247545329584694[20] = 1.0;
|
||||
out_3381247545329584694[21] = 0.0;
|
||||
out_3381247545329584694[22] = 0.0;
|
||||
out_3381247545329584694[23] = 0.0;
|
||||
out_3381247545329584694[24] = 0.0;
|
||||
out_3381247545329584694[25] = 0.0;
|
||||
out_3381247545329584694[26] = 0.0;
|
||||
out_3381247545329584694[27] = 0.0;
|
||||
out_3381247545329584694[28] = 0.0;
|
||||
out_3381247545329584694[29] = 0.0;
|
||||
out_3381247545329584694[30] = 1.0;
|
||||
out_3381247545329584694[31] = 0.0;
|
||||
out_3381247545329584694[32] = 0.0;
|
||||
out_3381247545329584694[33] = 0.0;
|
||||
out_3381247545329584694[34] = 0.0;
|
||||
out_3381247545329584694[35] = 0.0;
|
||||
out_3381247545329584694[36] = 0.0;
|
||||
out_3381247545329584694[37] = 0.0;
|
||||
out_3381247545329584694[38] = 0.0;
|
||||
out_3381247545329584694[39] = 0.0;
|
||||
out_3381247545329584694[40] = 1.0;
|
||||
out_3381247545329584694[41] = 0.0;
|
||||
out_3381247545329584694[42] = 0.0;
|
||||
out_3381247545329584694[43] = 0.0;
|
||||
out_3381247545329584694[44] = 0.0;
|
||||
out_3381247545329584694[45] = 0.0;
|
||||
out_3381247545329584694[46] = 0.0;
|
||||
out_3381247545329584694[47] = 0.0;
|
||||
out_3381247545329584694[48] = 0.0;
|
||||
out_3381247545329584694[49] = 0.0;
|
||||
out_3381247545329584694[50] = 1.0;
|
||||
out_3381247545329584694[51] = 0.0;
|
||||
out_3381247545329584694[52] = 0.0;
|
||||
out_3381247545329584694[53] = 0.0;
|
||||
out_3381247545329584694[54] = 0.0;
|
||||
out_3381247545329584694[55] = 0.0;
|
||||
out_3381247545329584694[56] = 0.0;
|
||||
out_3381247545329584694[57] = 0.0;
|
||||
out_3381247545329584694[58] = 0.0;
|
||||
out_3381247545329584694[59] = 0.0;
|
||||
out_3381247545329584694[60] = 1.0;
|
||||
out_3381247545329584694[61] = 0.0;
|
||||
out_3381247545329584694[62] = 0.0;
|
||||
out_3381247545329584694[63] = 0.0;
|
||||
out_3381247545329584694[64] = 0.0;
|
||||
out_3381247545329584694[65] = 0.0;
|
||||
out_3381247545329584694[66] = 0.0;
|
||||
out_3381247545329584694[67] = 0.0;
|
||||
out_3381247545329584694[68] = 0.0;
|
||||
out_3381247545329584694[69] = 0.0;
|
||||
out_3381247545329584694[70] = 1.0;
|
||||
out_3381247545329584694[71] = 0.0;
|
||||
out_3381247545329584694[72] = 0.0;
|
||||
out_3381247545329584694[73] = 0.0;
|
||||
out_3381247545329584694[74] = 0.0;
|
||||
out_3381247545329584694[75] = 0.0;
|
||||
out_3381247545329584694[76] = 0.0;
|
||||
out_3381247545329584694[77] = 0.0;
|
||||
out_3381247545329584694[78] = 0.0;
|
||||
out_3381247545329584694[79] = 0.0;
|
||||
out_3381247545329584694[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_4700437164455479144) {
|
||||
out_4700437164455479144[0] = 1.0;
|
||||
out_4700437164455479144[1] = 0.0;
|
||||
out_4700437164455479144[2] = 0.0;
|
||||
out_4700437164455479144[3] = 0.0;
|
||||
out_4700437164455479144[4] = 0.0;
|
||||
out_4700437164455479144[5] = 0.0;
|
||||
out_4700437164455479144[6] = 0.0;
|
||||
out_4700437164455479144[7] = 0.0;
|
||||
out_4700437164455479144[8] = 0.0;
|
||||
out_4700437164455479144[9] = 0.0;
|
||||
out_4700437164455479144[10] = 1.0;
|
||||
out_4700437164455479144[11] = 0.0;
|
||||
out_4700437164455479144[12] = 0.0;
|
||||
out_4700437164455479144[13] = 0.0;
|
||||
out_4700437164455479144[14] = 0.0;
|
||||
out_4700437164455479144[15] = 0.0;
|
||||
out_4700437164455479144[16] = 0.0;
|
||||
out_4700437164455479144[17] = 0.0;
|
||||
out_4700437164455479144[18] = 0.0;
|
||||
out_4700437164455479144[19] = 0.0;
|
||||
out_4700437164455479144[20] = 1.0;
|
||||
out_4700437164455479144[21] = 0.0;
|
||||
out_4700437164455479144[22] = 0.0;
|
||||
out_4700437164455479144[23] = 0.0;
|
||||
out_4700437164455479144[24] = 0.0;
|
||||
out_4700437164455479144[25] = 0.0;
|
||||
out_4700437164455479144[26] = 0.0;
|
||||
out_4700437164455479144[27] = 0.0;
|
||||
out_4700437164455479144[28] = 0.0;
|
||||
out_4700437164455479144[29] = 0.0;
|
||||
out_4700437164455479144[30] = 1.0;
|
||||
out_4700437164455479144[31] = 0.0;
|
||||
out_4700437164455479144[32] = 0.0;
|
||||
out_4700437164455479144[33] = 0.0;
|
||||
out_4700437164455479144[34] = 0.0;
|
||||
out_4700437164455479144[35] = 0.0;
|
||||
out_4700437164455479144[36] = 0.0;
|
||||
out_4700437164455479144[37] = 0.0;
|
||||
out_4700437164455479144[38] = 0.0;
|
||||
out_4700437164455479144[39] = 0.0;
|
||||
out_4700437164455479144[40] = 1.0;
|
||||
out_4700437164455479144[41] = 0.0;
|
||||
out_4700437164455479144[42] = 0.0;
|
||||
out_4700437164455479144[43] = 0.0;
|
||||
out_4700437164455479144[44] = 0.0;
|
||||
out_4700437164455479144[45] = 0.0;
|
||||
out_4700437164455479144[46] = 0.0;
|
||||
out_4700437164455479144[47] = 0.0;
|
||||
out_4700437164455479144[48] = 0.0;
|
||||
out_4700437164455479144[49] = 0.0;
|
||||
out_4700437164455479144[50] = 1.0;
|
||||
out_4700437164455479144[51] = 0.0;
|
||||
out_4700437164455479144[52] = 0.0;
|
||||
out_4700437164455479144[53] = 0.0;
|
||||
out_4700437164455479144[54] = 0.0;
|
||||
out_4700437164455479144[55] = 0.0;
|
||||
out_4700437164455479144[56] = 0.0;
|
||||
out_4700437164455479144[57] = 0.0;
|
||||
out_4700437164455479144[58] = 0.0;
|
||||
out_4700437164455479144[59] = 0.0;
|
||||
out_4700437164455479144[60] = 1.0;
|
||||
out_4700437164455479144[61] = 0.0;
|
||||
out_4700437164455479144[62] = 0.0;
|
||||
out_4700437164455479144[63] = 0.0;
|
||||
out_4700437164455479144[64] = 0.0;
|
||||
out_4700437164455479144[65] = 0.0;
|
||||
out_4700437164455479144[66] = 0.0;
|
||||
out_4700437164455479144[67] = 0.0;
|
||||
out_4700437164455479144[68] = 0.0;
|
||||
out_4700437164455479144[69] = 0.0;
|
||||
out_4700437164455479144[70] = 1.0;
|
||||
out_4700437164455479144[71] = 0.0;
|
||||
out_4700437164455479144[72] = 0.0;
|
||||
out_4700437164455479144[73] = 0.0;
|
||||
out_4700437164455479144[74] = 0.0;
|
||||
out_4700437164455479144[75] = 0.0;
|
||||
out_4700437164455479144[76] = 0.0;
|
||||
out_4700437164455479144[77] = 0.0;
|
||||
out_4700437164455479144[78] = 0.0;
|
||||
out_4700437164455479144[79] = 0.0;
|
||||
out_4700437164455479144[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_4828443839748202677) {
|
||||
out_4828443839748202677[0] = state[0];
|
||||
out_4828443839748202677[1] = state[1];
|
||||
out_4828443839748202677[2] = state[2];
|
||||
out_4828443839748202677[3] = state[3];
|
||||
out_4828443839748202677[4] = state[4];
|
||||
out_4828443839748202677[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_4828443839748202677[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_4828443839748202677[7] = state[7];
|
||||
out_4828443839748202677[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_2370248777466162255) {
|
||||
out_2370248777466162255[0] = state[0];
|
||||
out_2370248777466162255[1] = state[1];
|
||||
out_2370248777466162255[2] = state[2];
|
||||
out_2370248777466162255[3] = state[3];
|
||||
out_2370248777466162255[4] = state[4];
|
||||
out_2370248777466162255[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_2370248777466162255[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_2370248777466162255[7] = state[7];
|
||||
out_2370248777466162255[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_3545622658498023447) {
|
||||
out_3545622658498023447[0] = 1;
|
||||
out_3545622658498023447[1] = 0;
|
||||
out_3545622658498023447[2] = 0;
|
||||
out_3545622658498023447[3] = 0;
|
||||
out_3545622658498023447[4] = 0;
|
||||
out_3545622658498023447[5] = 0;
|
||||
out_3545622658498023447[6] = 0;
|
||||
out_3545622658498023447[7] = 0;
|
||||
out_3545622658498023447[8] = 0;
|
||||
out_3545622658498023447[9] = 0;
|
||||
out_3545622658498023447[10] = 1;
|
||||
out_3545622658498023447[11] = 0;
|
||||
out_3545622658498023447[12] = 0;
|
||||
out_3545622658498023447[13] = 0;
|
||||
out_3545622658498023447[14] = 0;
|
||||
out_3545622658498023447[15] = 0;
|
||||
out_3545622658498023447[16] = 0;
|
||||
out_3545622658498023447[17] = 0;
|
||||
out_3545622658498023447[18] = 0;
|
||||
out_3545622658498023447[19] = 0;
|
||||
out_3545622658498023447[20] = 1;
|
||||
out_3545622658498023447[21] = 0;
|
||||
out_3545622658498023447[22] = 0;
|
||||
out_3545622658498023447[23] = 0;
|
||||
out_3545622658498023447[24] = 0;
|
||||
out_3545622658498023447[25] = 0;
|
||||
out_3545622658498023447[26] = 0;
|
||||
out_3545622658498023447[27] = 0;
|
||||
out_3545622658498023447[28] = 0;
|
||||
out_3545622658498023447[29] = 0;
|
||||
out_3545622658498023447[30] = 1;
|
||||
out_3545622658498023447[31] = 0;
|
||||
out_3545622658498023447[32] = 0;
|
||||
out_3545622658498023447[33] = 0;
|
||||
out_3545622658498023447[34] = 0;
|
||||
out_3545622658498023447[35] = 0;
|
||||
out_3545622658498023447[36] = 0;
|
||||
out_3545622658498023447[37] = 0;
|
||||
out_3545622658498023447[38] = 0;
|
||||
out_3545622658498023447[39] = 0;
|
||||
out_3545622658498023447[40] = 1;
|
||||
out_3545622658498023447[41] = 0;
|
||||
out_3545622658498023447[42] = 0;
|
||||
out_3545622658498023447[43] = 0;
|
||||
out_3545622658498023447[44] = 0;
|
||||
out_3545622658498023447[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_3545622658498023447[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3545622658498023447[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3545622658498023447[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3545622658498023447[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_3545622658498023447[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3545622658498023447[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3545622658498023447[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3545622658498023447[53] = -9.8100000000000005*dt;
|
||||
out_3545622658498023447[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_3545622658498023447[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3545622658498023447[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3545622658498023447[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3545622658498023447[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_3545622658498023447[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3545622658498023447[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_3545622658498023447[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3545622658498023447[62] = 0;
|
||||
out_3545622658498023447[63] = 0;
|
||||
out_3545622658498023447[64] = 0;
|
||||
out_3545622658498023447[65] = 0;
|
||||
out_3545622658498023447[66] = 0;
|
||||
out_3545622658498023447[67] = 0;
|
||||
out_3545622658498023447[68] = 0;
|
||||
out_3545622658498023447[69] = 0;
|
||||
out_3545622658498023447[70] = 1;
|
||||
out_3545622658498023447[71] = 0;
|
||||
out_3545622658498023447[72] = 0;
|
||||
out_3545622658498023447[73] = 0;
|
||||
out_3545622658498023447[74] = 0;
|
||||
out_3545622658498023447[75] = 0;
|
||||
out_3545622658498023447[76] = 0;
|
||||
out_3545622658498023447[77] = 0;
|
||||
out_3545622658498023447[78] = 0;
|
||||
out_3545622658498023447[79] = 0;
|
||||
out_3545622658498023447[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_7809506053810665802) {
|
||||
out_7809506053810665802[0] = 1;
|
||||
out_7809506053810665802[1] = 0;
|
||||
out_7809506053810665802[2] = 0;
|
||||
out_7809506053810665802[3] = 0;
|
||||
out_7809506053810665802[4] = 0;
|
||||
out_7809506053810665802[5] = 0;
|
||||
out_7809506053810665802[6] = 0;
|
||||
out_7809506053810665802[7] = 0;
|
||||
out_7809506053810665802[8] = 0;
|
||||
out_7809506053810665802[9] = 0;
|
||||
out_7809506053810665802[10] = 1;
|
||||
out_7809506053810665802[11] = 0;
|
||||
out_7809506053810665802[12] = 0;
|
||||
out_7809506053810665802[13] = 0;
|
||||
out_7809506053810665802[14] = 0;
|
||||
out_7809506053810665802[15] = 0;
|
||||
out_7809506053810665802[16] = 0;
|
||||
out_7809506053810665802[17] = 0;
|
||||
out_7809506053810665802[18] = 0;
|
||||
out_7809506053810665802[19] = 0;
|
||||
out_7809506053810665802[20] = 1;
|
||||
out_7809506053810665802[21] = 0;
|
||||
out_7809506053810665802[22] = 0;
|
||||
out_7809506053810665802[23] = 0;
|
||||
out_7809506053810665802[24] = 0;
|
||||
out_7809506053810665802[25] = 0;
|
||||
out_7809506053810665802[26] = 0;
|
||||
out_7809506053810665802[27] = 0;
|
||||
out_7809506053810665802[28] = 0;
|
||||
out_7809506053810665802[29] = 0;
|
||||
out_7809506053810665802[30] = 1;
|
||||
out_7809506053810665802[31] = 0;
|
||||
out_7809506053810665802[32] = 0;
|
||||
out_7809506053810665802[33] = 0;
|
||||
out_7809506053810665802[34] = 0;
|
||||
out_7809506053810665802[35] = 0;
|
||||
out_7809506053810665802[36] = 0;
|
||||
out_7809506053810665802[37] = 0;
|
||||
out_7809506053810665802[38] = 0;
|
||||
out_7809506053810665802[39] = 0;
|
||||
out_7809506053810665802[40] = 1;
|
||||
out_7809506053810665802[41] = 0;
|
||||
out_7809506053810665802[42] = 0;
|
||||
out_7809506053810665802[43] = 0;
|
||||
out_7809506053810665802[44] = 0;
|
||||
out_7809506053810665802[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_7809506053810665802[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_7809506053810665802[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7809506053810665802[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7809506053810665802[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_7809506053810665802[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_7809506053810665802[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_7809506053810665802[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_7809506053810665802[53] = -9.8100000000000005*dt;
|
||||
out_7809506053810665802[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_7809506053810665802[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_7809506053810665802[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7809506053810665802[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7809506053810665802[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_7809506053810665802[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_7809506053810665802[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_7809506053810665802[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_7809506053810665802[62] = 0;
|
||||
out_7809506053810665802[63] = 0;
|
||||
out_7809506053810665802[64] = 0;
|
||||
out_7809506053810665802[65] = 0;
|
||||
out_7809506053810665802[66] = 0;
|
||||
out_7809506053810665802[67] = 0;
|
||||
out_7809506053810665802[68] = 0;
|
||||
out_7809506053810665802[69] = 0;
|
||||
out_7809506053810665802[70] = 1;
|
||||
out_7809506053810665802[71] = 0;
|
||||
out_7809506053810665802[72] = 0;
|
||||
out_7809506053810665802[73] = 0;
|
||||
out_7809506053810665802[74] = 0;
|
||||
out_7809506053810665802[75] = 0;
|
||||
out_7809506053810665802[76] = 0;
|
||||
out_7809506053810665802[77] = 0;
|
||||
out_7809506053810665802[78] = 0;
|
||||
out_7809506053810665802[79] = 0;
|
||||
out_7809506053810665802[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_7855544973847125055) {
|
||||
out_7855544973847125055[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_2714863388055632273) {
|
||||
out_2714863388055632273[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_5811940680582652543) {
|
||||
out_5811940680582652543[0] = 0;
|
||||
out_5811940680582652543[1] = 0;
|
||||
out_5811940680582652543[2] = 0;
|
||||
out_5811940680582652543[3] = 0;
|
||||
out_5811940680582652543[4] = 0;
|
||||
out_5811940680582652543[5] = 0;
|
||||
out_5811940680582652543[6] = 1;
|
||||
out_5811940680582652543[7] = 0;
|
||||
out_5811940680582652543[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_5824968876280212667) {
|
||||
out_5824968876280212667[0] = 0;
|
||||
out_5824968876280212667[1] = 0;
|
||||
out_5824968876280212667[2] = 0;
|
||||
out_5824968876280212667[3] = 0;
|
||||
out_5824968876280212667[4] = 0;
|
||||
out_5824968876280212667[5] = 0;
|
||||
out_5824968876280212667[6] = 1;
|
||||
out_5824968876280212667[7] = 0;
|
||||
out_5824968876280212667[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_7215169173686495729) {
|
||||
out_7215169173686495729[0] = state[4];
|
||||
out_7215169173686495729[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8643275185285187003) {
|
||||
out_8643275185285187003[0] = state[4];
|
||||
out_8643275185285187003[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7984590279588152109) {
|
||||
out_7984590279588152109[0] = 0;
|
||||
out_7984590279588152109[1] = 0;
|
||||
out_7984590279588152109[2] = 0;
|
||||
out_7984590279588152109[3] = 0;
|
||||
out_7984590279588152109[4] = 1;
|
||||
out_7984590279588152109[5] = 0;
|
||||
out_7984590279588152109[6] = 0;
|
||||
out_7984590279588152109[7] = 0;
|
||||
out_7984590279588152109[8] = 0;
|
||||
out_7984590279588152109[9] = 0;
|
||||
out_7984590279588152109[10] = 0;
|
||||
out_7984590279588152109[11] = 0;
|
||||
out_7984590279588152109[12] = 0;
|
||||
out_7984590279588152109[13] = 0;
|
||||
out_7984590279588152109[14] = 1;
|
||||
out_7984590279588152109[15] = 0;
|
||||
out_7984590279588152109[16] = 0;
|
||||
out_7984590279588152109[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_3403096309788982558) {
|
||||
out_3403096309788982558[0] = 0;
|
||||
out_3403096309788982558[1] = 0;
|
||||
out_3403096309788982558[2] = 0;
|
||||
out_3403096309788982558[3] = 0;
|
||||
out_3403096309788982558[4] = 1;
|
||||
out_3403096309788982558[5] = 0;
|
||||
out_3403096309788982558[6] = 0;
|
||||
out_3403096309788982558[7] = 0;
|
||||
out_3403096309788982558[8] = 0;
|
||||
out_3403096309788982558[9] = 0;
|
||||
out_3403096309788982558[10] = 0;
|
||||
out_3403096309788982558[11] = 0;
|
||||
out_3403096309788982558[12] = 0;
|
||||
out_3403096309788982558[13] = 0;
|
||||
out_3403096309788982558[14] = 1;
|
||||
out_3403096309788982558[15] = 0;
|
||||
out_3403096309788982558[16] = 0;
|
||||
out_3403096309788982558[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_1700097778006464926) {
|
||||
out_1700097778006464926[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_4165494780684515440) {
|
||||
out_4165494780684515440[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_1104749660908964212) {
|
||||
out_1104749660908964212[0] = 0;
|
||||
out_1104749660908964212[1] = 0;
|
||||
out_1104749660908964212[2] = 0;
|
||||
out_1104749660908964212[3] = 0;
|
||||
out_1104749660908964212[4] = 1;
|
||||
out_1104749660908964212[5] = 0;
|
||||
out_1104749660908964212[6] = 0;
|
||||
out_1104749660908964212[7] = 0;
|
||||
out_1104749660908964212[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_8094078867301730751) {
|
||||
out_8094078867301730751[0] = 0;
|
||||
out_8094078867301730751[1] = 0;
|
||||
out_8094078867301730751[2] = 0;
|
||||
out_8094078867301730751[3] = 0;
|
||||
out_8094078867301730751[4] = 1;
|
||||
out_8094078867301730751[5] = 0;
|
||||
out_8094078867301730751[6] = 0;
|
||||
out_8094078867301730751[7] = 0;
|
||||
out_8094078867301730751[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_6125458693304696459) {
|
||||
out_6125458693304696459[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_749221491035445424) {
|
||||
out_749221491035445424[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_8893300074252842849) {
|
||||
out_8893300074252842849[0] = 0;
|
||||
out_8893300074252842849[1] = 0;
|
||||
out_8893300074252842849[2] = 0;
|
||||
out_8893300074252842849[3] = 0;
|
||||
out_8893300074252842849[4] = 0;
|
||||
out_8893300074252842849[5] = 0;
|
||||
out_8893300074252842849[6] = 0;
|
||||
out_8893300074252842849[7] = 1;
|
||||
out_8893300074252842849[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_8880271878555282725) {
|
||||
out_8880271878555282725[0] = 0;
|
||||
out_8880271878555282725[1] = 0;
|
||||
out_8880271878555282725[2] = 0;
|
||||
out_8880271878555282725[3] = 0;
|
||||
out_8880271878555282725[4] = 0;
|
||||
out_8880271878555282725[5] = 0;
|
||||
out_8880271878555282725[6] = 0;
|
||||
out_8880271878555282725[7] = 1;
|
||||
out_8880271878555282725[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2212815102766168220) {
|
||||
out_2212815102766168220[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_8472848302049370229) {
|
||||
out_8472848302049370229[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_1070013650891460699) {
|
||||
out_1070013650891460699[0] = 0;
|
||||
out_1070013650891460699[1] = 0;
|
||||
out_1070013650891460699[2] = 0;
|
||||
out_1070013650891460699[3] = 1;
|
||||
out_1070013650891460699[4] = 0;
|
||||
out_1070013650891460699[5] = 0;
|
||||
out_1070013650891460699[6] = 0;
|
||||
out_1070013650891460699[7] = 0;
|
||||
out_1070013650891460699[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5919315555501305840) {
|
||||
out_5919315555501305840[0] = 0;
|
||||
out_5919315555501305840[1] = 0;
|
||||
out_5919315555501305840[2] = 0;
|
||||
out_5919315555501305840[3] = 1;
|
||||
out_5919315555501305840[4] = 0;
|
||||
out_5919315555501305840[5] = 0;
|
||||
out_5919315555501305840[6] = 0;
|
||||
out_5919315555501305840[7] = 0;
|
||||
out_5919315555501305840[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_4304503098815162577) {
|
||||
out_4304503098815162577[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_836178486976330205) {
|
||||
out_836178486976330205[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_2783376377761011732) {
|
||||
out_2783376377761011732[0] = 0;
|
||||
out_2783376377761011732[1] = 1;
|
||||
out_2783376377761011732[2] = 0;
|
||||
out_2783376377761011732[3] = 0;
|
||||
out_2783376377761011732[4] = 0;
|
||||
out_2783376377761011732[5] = 0;
|
||||
out_2783376377761011732[6] = 0;
|
||||
out_2783376377761011732[7] = 0;
|
||||
out_2783376377761011732[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_8604310211616122935) {
|
||||
out_8604310211616122935[0] = 0;
|
||||
out_8604310211616122935[1] = 1;
|
||||
out_8604310211616122935[2] = 0;
|
||||
out_8604310211616122935[3] = 0;
|
||||
out_8604310211616122935[4] = 0;
|
||||
out_8604310211616122935[5] = 0;
|
||||
out_8604310211616122935[6] = 0;
|
||||
out_8604310211616122935[7] = 0;
|
||||
out_8604310211616122935[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_8381962881269050695) {
|
||||
out_8381962881269050695[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_5838057092722024830) {
|
||||
out_5838057092722024830[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7865775394830542306) {
|
||||
out_7865775394830542306[0] = 1;
|
||||
out_7865775394830542306[1] = 0;
|
||||
out_7865775394830542306[2] = 0;
|
||||
out_7865775394830542306[3] = 0;
|
||||
out_7865775394830542306[4] = 0;
|
||||
out_7865775394830542306[5] = 0;
|
||||
out_7865775394830542306[6] = 0;
|
||||
out_7865775394830542306[7] = 0;
|
||||
out_7865775394830542306[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_7878803590528102430) {
|
||||
out_7878803590528102430[0] = 1;
|
||||
out_7878803590528102430[1] = 0;
|
||||
out_7878803590528102430[2] = 0;
|
||||
out_7878803590528102430[3] = 0;
|
||||
out_7878803590528102430[4] = 0;
|
||||
out_7878803590528102430[5] = 0;
|
||||
out_7878803590528102430[6] = 0;
|
||||
out_7878803590528102430[7] = 0;
|
||||
out_7878803590528102430[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_8130739036131630944) {
|
||||
out_8130739036131630944[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_5711256312889543433) {
|
||||
out_5711256312889543433[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5781294718705692115) {
|
||||
out_5781294718705692115[0] = 0;
|
||||
out_5781294718705692115[1] = 0;
|
||||
out_5781294718705692115[2] = 0;
|
||||
out_5781294718705692115[3] = 0;
|
||||
out_5781294718705692115[4] = 0;
|
||||
out_5781294718705692115[5] = 0;
|
||||
out_5781294718705692115[6] = 0;
|
||||
out_5781294718705692115[7] = 0;
|
||||
out_5781294718705692115[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_8254063776321931249) {
|
||||
out_8254063776321931249[0] = 0;
|
||||
out_8254063776321931249[1] = 0;
|
||||
out_8254063776321931249[2] = 0;
|
||||
out_8254063776321931249[3] = 0;
|
||||
out_8254063776321931249[4] = 0;
|
||||
out_8254063776321931249[5] = 0;
|
||||
out_8254063776321931249[6] = 0;
|
||||
out_8254063776321931249[7] = 0;
|
||||
out_8254063776321931249[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_8206107047934752633) {
|
||||
err_fun(nom_x, delta_x, out_8206107047934752633);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4770141140221383025) {
|
||||
err_fun(nom_x, delta_x, out_4770141140221383025);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3582621965851683853) {
|
||||
inv_err_fun(nom_x, true_x, out_3582621965851683853);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8267480455965364332) {
|
||||
inv_err_fun(nom_x, true_x, out_8267480455965364332);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_3381247545329584694) {
|
||||
H_mod_fun(state, out_3381247545329584694);
|
||||
void car_H_mod_fun(double *state, double *out_4700437164455479144) {
|
||||
H_mod_fun(state, out_4700437164455479144);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_4828443839748202677) {
|
||||
f_fun(state, dt, out_4828443839748202677);
|
||||
void car_f_fun(double *state, double dt, double *out_2370248777466162255) {
|
||||
f_fun(state, dt, out_2370248777466162255);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_3545622658498023447) {
|
||||
F_fun(state, dt, out_3545622658498023447);
|
||||
void car_F_fun(double *state, double dt, double *out_7809506053810665802) {
|
||||
F_fun(state, dt, out_7809506053810665802);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_7855544973847125055) {
|
||||
h_25(state, unused, out_7855544973847125055);
|
||||
void car_h_25(double *state, double *unused, double *out_2714863388055632273) {
|
||||
h_25(state, unused, out_2714863388055632273);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_5811940680582652543) {
|
||||
H_25(state, unused, out_5811940680582652543);
|
||||
void car_H_25(double *state, double *unused, double *out_5824968876280212667) {
|
||||
H_25(state, unused, out_5824968876280212667);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_7215169173686495729) {
|
||||
h_24(state, unused, out_7215169173686495729);
|
||||
void car_h_24(double *state, double *unused, double *out_8643275185285187003) {
|
||||
h_24(state, unused, out_8643275185285187003);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7984590279588152109) {
|
||||
H_24(state, unused, out_7984590279588152109);
|
||||
void car_H_24(double *state, double *unused, double *out_3403096309788982558) {
|
||||
H_24(state, unused, out_3403096309788982558);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_1700097778006464926) {
|
||||
h_30(state, unused, out_1700097778006464926);
|
||||
void car_h_30(double *state, double *unused, double *out_4165494780684515440) {
|
||||
h_30(state, unused, out_4165494780684515440);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_1104749660908964212) {
|
||||
H_30(state, unused, out_1104749660908964212);
|
||||
void car_H_30(double *state, double *unused, double *out_8094078867301730751) {
|
||||
H_30(state, unused, out_8094078867301730751);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_6125458693304696459) {
|
||||
h_26(state, unused, out_6125458693304696459);
|
||||
void car_h_26(double *state, double *unused, double *out_749221491035445424) {
|
||||
h_26(state, unused, out_749221491035445424);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_8893300074252842849) {
|
||||
H_26(state, unused, out_8893300074252842849);
|
||||
void car_H_26(double *state, double *unused, double *out_8880271878555282725) {
|
||||
H_26(state, unused, out_8880271878555282725);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2212815102766168220) {
|
||||
h_27(state, unused, out_2212815102766168220);
|
||||
void car_h_27(double *state, double *unused, double *out_8472848302049370229) {
|
||||
h_27(state, unused, out_8472848302049370229);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_1070013650891460699) {
|
||||
H_27(state, unused, out_1070013650891460699);
|
||||
void car_H_27(double *state, double *unused, double *out_5919315555501305840) {
|
||||
H_27(state, unused, out_5919315555501305840);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_4304503098815162577) {
|
||||
h_29(state, unused, out_4304503098815162577);
|
||||
void car_h_29(double *state, double *unused, double *out_836178486976330205) {
|
||||
h_29(state, unused, out_836178486976330205);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_2783376377761011732) {
|
||||
H_29(state, unused, out_2783376377761011732);
|
||||
void car_H_29(double *state, double *unused, double *out_8604310211616122935) {
|
||||
H_29(state, unused, out_8604310211616122935);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_8381962881269050695) {
|
||||
h_28(state, unused, out_8381962881269050695);
|
||||
void car_h_28(double *state, double *unused, double *out_5838057092722024830) {
|
||||
h_28(state, unused, out_5838057092722024830);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7865775394830542306) {
|
||||
H_28(state, unused, out_7865775394830542306);
|
||||
void car_H_28(double *state, double *unused, double *out_7878803590528102430) {
|
||||
H_28(state, unused, out_7878803590528102430);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_8130739036131630944) {
|
||||
h_31(state, unused, out_8130739036131630944);
|
||||
void car_h_31(double *state, double *unused, double *out_5711256312889543433) {
|
||||
h_31(state, unused, out_5711256312889543433);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5781294718705692115) {
|
||||
H_31(state, unused, out_5781294718705692115);
|
||||
void car_H_31(double *state, double *unused, double *out_8254063776321931249) {
|
||||
H_31(state, unused, out_8254063776321931249);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8206107047934752633);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3582621965851683853);
|
||||
void car_H_mod_fun(double *state, double *out_3381247545329584694);
|
||||
void car_f_fun(double *state, double dt, double *out_4828443839748202677);
|
||||
void car_F_fun(double *state, double dt, double *out_3545622658498023447);
|
||||
void car_h_25(double *state, double *unused, double *out_7855544973847125055);
|
||||
void car_H_25(double *state, double *unused, double *out_5811940680582652543);
|
||||
void car_h_24(double *state, double *unused, double *out_7215169173686495729);
|
||||
void car_H_24(double *state, double *unused, double *out_7984590279588152109);
|
||||
void car_h_30(double *state, double *unused, double *out_1700097778006464926);
|
||||
void car_H_30(double *state, double *unused, double *out_1104749660908964212);
|
||||
void car_h_26(double *state, double *unused, double *out_6125458693304696459);
|
||||
void car_H_26(double *state, double *unused, double *out_8893300074252842849);
|
||||
void car_h_27(double *state, double *unused, double *out_2212815102766168220);
|
||||
void car_H_27(double *state, double *unused, double *out_1070013650891460699);
|
||||
void car_h_29(double *state, double *unused, double *out_4304503098815162577);
|
||||
void car_H_29(double *state, double *unused, double *out_2783376377761011732);
|
||||
void car_h_28(double *state, double *unused, double *out_8381962881269050695);
|
||||
void car_H_28(double *state, double *unused, double *out_7865775394830542306);
|
||||
void car_h_31(double *state, double *unused, double *out_8130739036131630944);
|
||||
void car_H_31(double *state, double *unused, double *out_5781294718705692115);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4770141140221383025);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8267480455965364332);
|
||||
void car_H_mod_fun(double *state, double *out_4700437164455479144);
|
||||
void car_f_fun(double *state, double dt, double *out_2370248777466162255);
|
||||
void car_F_fun(double *state, double dt, double *out_7809506053810665802);
|
||||
void car_h_25(double *state, double *unused, double *out_2714863388055632273);
|
||||
void car_H_25(double *state, double *unused, double *out_5824968876280212667);
|
||||
void car_h_24(double *state, double *unused, double *out_8643275185285187003);
|
||||
void car_H_24(double *state, double *unused, double *out_3403096309788982558);
|
||||
void car_h_30(double *state, double *unused, double *out_4165494780684515440);
|
||||
void car_H_30(double *state, double *unused, double *out_8094078867301730751);
|
||||
void car_h_26(double *state, double *unused, double *out_749221491035445424);
|
||||
void car_H_26(double *state, double *unused, double *out_8880271878555282725);
|
||||
void car_h_27(double *state, double *unused, double *out_8472848302049370229);
|
||||
void car_H_27(double *state, double *unused, double *out_5919315555501305840);
|
||||
void car_h_29(double *state, double *unused, double *out_836178486976330205);
|
||||
void car_H_29(double *state, double *unused, double *out_8604310211616122935);
|
||||
void car_h_28(double *state, double *unused, double *out_5838057092722024830);
|
||||
void car_H_28(double *state, double *unused, double *out_7878803590528102430);
|
||||
void car_h_31(double *state, double *unused, double *out_5711256312889543433);
|
||||
void car_H_31(double *state, double *unused, double *out_8254063776321931249);
|
||||
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);
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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_3083751291667230086);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8958923596036680453);
|
||||
void pose_H_mod_fun(double *state, double *out_3451746955233961922);
|
||||
void pose_f_fun(double *state, double dt, double *out_9153581491484683214);
|
||||
void pose_F_fun(double *state, double dt, double *out_1650230700148071035);
|
||||
void pose_h_4(double *state, double *unused, double *out_7718420596511956484);
|
||||
void pose_H_4(double *state, double *unused, double *out_9146915311723630973);
|
||||
void pose_h_10(double *state, double *unused, double *out_5513215277046893653);
|
||||
void pose_H_10(double *state, double *unused, double *out_1077084666490538527);
|
||||
void pose_h_13(double *state, double *unused, double *out_399423813345080057);
|
||||
void pose_H_13(double *state, double *unused, double *out_6087554936653587842);
|
||||
void pose_h_14(double *state, double *unused, double *out_1933161813507433673);
|
||||
void pose_H_14(double *state, double *unused, double *out_5336587905646436114);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_2755267697534836281);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4688414877314135206);
|
||||
void pose_H_mod_fun(double *state, double *out_7837152669402150713);
|
||||
void pose_f_fun(double *state, double dt, double *out_8293850048606202954);
|
||||
void pose_F_fun(double *state, double dt, double *out_1277983044864762150);
|
||||
void pose_h_4(double *state, double *unused, double *out_6167073772337384283);
|
||||
void pose_H_4(double *state, double *unused, double *out_4761509597555442182);
|
||||
void pose_h_10(double *state, double *unused, double *out_3137948981903642358);
|
||||
void pose_H_10(double *state, double *unused, double *out_6455930617601822676);
|
||||
void pose_h_13(double *state, double *unused, double *out_1578870771935541941);
|
||||
void pose_H_13(double *state, double *unused, double *out_7973783422887774983);
|
||||
void pose_h_14(double *state, double *unused, double *out_7672211974709550584);
|
||||
void pose_H_14(double *state, double *unused, double *out_4326393070910558583);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -14,6 +14,7 @@ from openpilot.system.ui.widgets import Widget
|
||||
|
||||
ASSETS_PATH = Path(__file__).resolve().parents[4] / "starpilot" / "assets" / "navigation"
|
||||
FALLBACK_ICON = "direction_turn_straight.png"
|
||||
NAV_CANCEL_HOLD_SECONDS = 0.65
|
||||
|
||||
|
||||
def _format_distance(distance_m: float, is_metric: bool) -> str:
|
||||
@@ -77,8 +78,7 @@ class NavigationCardRenderer(Widget):
|
||||
self._collapsed_param_supported: bool | None = None
|
||||
self._interactive_rect = rl.Rectangle(0, 0, 0, 0)
|
||||
self._click_delay = 0.15
|
||||
|
||||
self.set_click_callback(self._toggle_collapsed)
|
||||
self._press_started_at: float | None = None
|
||||
|
||||
@property
|
||||
def _hit_rect(self) -> rl.Rectangle:
|
||||
@@ -98,6 +98,45 @@ class NavigationCardRenderer(Widget):
|
||||
self._collapsed_fallback = new_state
|
||||
self._collapsed = new_state
|
||||
|
||||
def _cancel_navigation(self) -> None:
|
||||
try:
|
||||
ui_state.params.remove("NavDestination")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
ui_state.params_memory.remove("NavInstructionState")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
try:
|
||||
ui_state.params_memory.remove("NavInstructionCollapsed")
|
||||
self._collapsed_param_supported = True
|
||||
except UnknownKeyName:
|
||||
self._collapsed_param_supported = False
|
||||
|
||||
self._collapsed_fallback = False
|
||||
self._collapsed = False
|
||||
self._valid = False
|
||||
|
||||
def _handle_mouse_press(self, mouse_pos) -> None:
|
||||
self._press_started_at = rl.get_time()
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos) -> None:
|
||||
if self._click_delay is not None:
|
||||
self._click_release_time = rl.get_time() + self._click_delay
|
||||
|
||||
if not self._valid or self._press_started_at is None:
|
||||
self._press_started_at = None
|
||||
return
|
||||
|
||||
held_seconds = rl.get_time() - self._press_started_at
|
||||
self._press_started_at = None
|
||||
if held_seconds >= NAV_CANCEL_HOLD_SECONDS:
|
||||
self._cancel_navigation()
|
||||
else:
|
||||
self._toggle_collapsed()
|
||||
|
||||
def _icon_filename(self, maneuver_type: str, modifier: str) -> str:
|
||||
normalized_type = _normalize_maneuver_type(maneuver_type)
|
||||
if modifier == "uturn":
|
||||
|
||||
@@ -27,8 +27,12 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
|
||||
screen_recorder->setVisible(false);
|
||||
}
|
||||
|
||||
bool AnnotatedCameraWidget::handleHudTap(const QPoint &pos) {
|
||||
return hud.handleNavigationTap(pos);
|
||||
bool AnnotatedCameraWidget::handleHudPress(const QPoint &pos) {
|
||||
return hud.handleNavigationPress(pos);
|
||||
}
|
||||
|
||||
bool AnnotatedCameraWidget::handleHudRelease(const QPoint &pos) {
|
||||
return hud.handleNavigationRelease(pos);
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState &fs) {
|
||||
|
||||
@@ -17,7 +17,8 @@ class AnnotatedCameraWidget : public CameraWidget {
|
||||
public:
|
||||
explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0);
|
||||
void updateState(const UIState &s, const StarPilotUIState &fs);
|
||||
bool handleHudTap(const QPoint &pos);
|
||||
bool handleHudPress(const QPoint &pos);
|
||||
bool handleHudRelease(const QPoint &pos);
|
||||
|
||||
double fps;
|
||||
|
||||
|
||||
@@ -9,18 +9,54 @@
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
|
||||
constexpr int SET_SPEED_NA = 255;
|
||||
constexpr int NAV_CANCEL_HOLD_MS = 650;
|
||||
|
||||
HudRenderer::HudRenderer() {}
|
||||
|
||||
bool HudRenderer::handleNavigationTap(const QPoint &pos) {
|
||||
bool HudRenderer::handleNavigationPress(const QPoint &pos) {
|
||||
if (!navigation_valid || !nav_hit_rect.contains(pos)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
params_memory.putBool("NavInstructionCollapsed", !navigation_collapsed);
|
||||
nav_press_active = true;
|
||||
nav_press_timer.restart();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HudRenderer::handleNavigationRelease(const QPoint &pos) {
|
||||
if (!nav_press_active) {
|
||||
return false;
|
||||
}
|
||||
|
||||
nav_press_active = false;
|
||||
if (!nav_hit_rect.contains(pos)) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (nav_press_timer.isValid() && nav_press_timer.elapsed() >= NAV_CANCEL_HOLD_MS) {
|
||||
cancelNavigation();
|
||||
} else {
|
||||
params_memory.putBool("NavInstructionCollapsed", !navigation_collapsed);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void HudRenderer::cancelNavigation() {
|
||||
params.remove("NavDestination");
|
||||
params_memory.remove("NavInstructionState");
|
||||
params_memory.remove("NavInstructionCollapsed");
|
||||
navigation_valid = false;
|
||||
navigation_collapsed = false;
|
||||
nav_hit_rect = QRect();
|
||||
nav_primary_text.clear();
|
||||
nav_secondary_text.clear();
|
||||
nav_distance.clear();
|
||||
nav_maneuver_type.clear();
|
||||
nav_modifier.clear();
|
||||
nav_next_maneuver_type.clear();
|
||||
nav_next_modifier.clear();
|
||||
}
|
||||
|
||||
void HudRenderer::updateState(const UIState &s) {
|
||||
is_metric = s.scene.is_metric;
|
||||
status = s.status;
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <QHash>
|
||||
#include <QElapsedTimer>
|
||||
#include <QJsonObject>
|
||||
#include <QPainter>
|
||||
#include <QPixmap>
|
||||
@@ -15,7 +16,8 @@ public:
|
||||
HudRenderer();
|
||||
void updateState(const UIState &s);
|
||||
void draw(QPainter &p, const QRect &surface_rect);
|
||||
bool handleNavigationTap(const QPoint &pos);
|
||||
bool handleNavigationPress(const QPoint &pos);
|
||||
bool handleNavigationRelease(const QPoint &pos);
|
||||
|
||||
StarPilotAnnotatedCameraWidget *starpilot_nvg;
|
||||
|
||||
@@ -30,6 +32,7 @@ private:
|
||||
QString formatNavDistance(float distance_m) const;
|
||||
QString navigationIconFilename(const QString &maneuver_type, const QString &modifier) const;
|
||||
QPixmap getNavigationIcon(const QString &maneuver_type, const QString &modifier, int size);
|
||||
void cancelNavigation();
|
||||
|
||||
float speed = 0;
|
||||
float set_speed = 0;
|
||||
@@ -51,6 +54,8 @@ private:
|
||||
QString nav_next_modifier;
|
||||
QRect nav_hit_rect;
|
||||
QHash<QString, QPixmap> nav_icon_cache;
|
||||
QElapsedTimer nav_press_timer;
|
||||
bool nav_press_active = false;
|
||||
Params params;
|
||||
Params params_memory{"", true};
|
||||
};
|
||||
|
||||
@@ -113,7 +113,7 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
|
||||
}
|
||||
|
||||
void OnroadWindow::mousePressEvent(QMouseEvent* mouseEvent) {
|
||||
if (nvg->handleHudTap(mouseEvent->pos())) {
|
||||
if (nvg->handleHudPress(mouseEvent->pos())) {
|
||||
mouseEvent->accept();
|
||||
return;
|
||||
}
|
||||
@@ -127,3 +127,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* mouseEvent) {
|
||||
// propagation event to parent(HomeWindow)
|
||||
QWidget::mousePressEvent(mouseEvent);
|
||||
}
|
||||
|
||||
void OnroadWindow::mouseReleaseEvent(QMouseEvent* mouseEvent) {
|
||||
if (nvg->handleHudRelease(mouseEvent->pos())) {
|
||||
mouseEvent->accept();
|
||||
return;
|
||||
}
|
||||
|
||||
QWidget::mouseReleaseEvent(mouseEvent);
|
||||
}
|
||||
|
||||
@@ -19,6 +19,7 @@ private:
|
||||
QHBoxLayout* split;
|
||||
|
||||
void mousePressEvent(QMouseEvent* mouseEvent);
|
||||
void mouseReleaseEvent(QMouseEvent* mouseEvent);
|
||||
|
||||
StarPilotAnnotatedCameraWidget *starpilot_nvg;
|
||||
StarPilotOnroadWindow *starpilot_onroad;
|
||||
|
||||
Binary file not shown.
@@ -115,6 +115,7 @@ class StarPilotPlanner:
|
||||
"speed": v_ego,
|
||||
"hasFix": bool(getattr(gps_location, "hasFix", False)),
|
||||
"updatedAtMonotonic": time.monotonic(),
|
||||
"updatedAtSec": time.time(),
|
||||
}
|
||||
self.gps_valid = self.gps_position["hasFix"] and (self.gps_position["latitude"] != 0 or self.gps_position["longitude"] != 0)
|
||||
bearing = self.gps_position["bearing"]
|
||||
|
||||
@@ -7,6 +7,7 @@ import time
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.api import Api, api_get
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL, Priority, Ratekeeper, config_realtime_process
|
||||
from openpilot.common.time_helpers import system_time_valid
|
||||
@@ -30,6 +31,8 @@ from openpilot.starpilot.system.starpilot_tracking import StarPilotTracking
|
||||
|
||||
ASSET_CHECK_RATE = (1 / DT_MDL)
|
||||
DRIVE_STATS_SYNC_RATE = 30
|
||||
OFFROAD_GPS_MEMORY_REFRESH_SECONDS = 1.0
|
||||
OFFROAD_GPS_PERSIST_REFRESH_SECONDS = 30.0
|
||||
TOGGLE_BROADCAST_INTERVAL_FRAMES = int(1 / DT_MDL)
|
||||
UPDATE_CHECK_INTERVAL_SECONDS = 60 * 60
|
||||
|
||||
@@ -53,6 +56,31 @@ def get_next_periodic_update_check(monotonic_now, phase_seconds):
|
||||
next_check += UPDATE_CHECK_INTERVAL_SECONDS
|
||||
return next_check
|
||||
|
||||
|
||||
def build_gps_position(gps_location, speed):
|
||||
return {
|
||||
"latitude": gps_location.latitude,
|
||||
"longitude": gps_location.longitude,
|
||||
"bearing": gps_location.bearingDeg,
|
||||
"speed": max(float(speed), 0.0),
|
||||
"hasFix": bool(getattr(gps_location, "hasFix", False)),
|
||||
"updatedAtMonotonic": time.monotonic(),
|
||||
"updatedAtSec": time.time(),
|
||||
}
|
||||
|
||||
|
||||
def gps_position_valid(gps_position):
|
||||
return bool(gps_position["hasFix"]) and (gps_position["latitude"] != 0 or gps_position["longitude"] != 0)
|
||||
|
||||
|
||||
def gps_position_signature(gps_position):
|
||||
return (
|
||||
round(float(gps_position["latitude"]), 6),
|
||||
round(float(gps_position["longitude"]), 6),
|
||||
round(float(gps_position["bearing"]), 1),
|
||||
bool(gps_position["hasFix"]),
|
||||
)
|
||||
|
||||
def check_assets(now, model_manager, theme_manager, thread_manager, params, params_memory, starpilot_toggles):
|
||||
if params_memory.get_bool(MODEL_DOWNLOAD_ALL_PARAM):
|
||||
thread_manager.run_with_lock(model_manager.download_all_models)
|
||||
@@ -182,6 +210,7 @@ def starpilot_thread():
|
||||
model_manager = ModelManager(params, params_memory)
|
||||
theme_manager = ThemeManager(params, params_memory)
|
||||
thread_manager = ThreadManager()
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
|
||||
starpilot_toggles = starpilot_variables.starpilot_toggles
|
||||
serialized_starpilot_toggles = serialize_starpilot_toggles(starpilot_toggles)
|
||||
@@ -191,6 +220,9 @@ def starpilot_thread():
|
||||
next_drive_stats_sync = 0.0
|
||||
periodic_update_phase = get_update_check_phase_seconds(params_raw)
|
||||
next_periodic_update_check = get_next_periodic_update_check(time.monotonic(), periodic_update_phase)
|
||||
last_offroad_gps_memory_write = 0.0
|
||||
last_offroad_gps_persist_write = 0.0
|
||||
last_offroad_gps_persist_signature = None
|
||||
|
||||
run_update_checks = False
|
||||
safe_mode_active = safe_mode_enabled(params_raw)
|
||||
@@ -243,6 +275,21 @@ def starpilot_thread():
|
||||
starpilot_plan_send.starpilotPlan.themeUpdated = theme_manager.theme_updated
|
||||
pm.send("starpilotPlan", starpilot_plan_send)
|
||||
|
||||
gps_position = build_gps_position(sm[gps_location_service], getattr(sm["carState"], "vEgo", 0.0))
|
||||
if gps_position_valid(gps_position):
|
||||
gps_memory_state = json.dumps(gps_position, allow_nan=False)
|
||||
if (monotonic_now - last_offroad_gps_memory_write) >= OFFROAD_GPS_MEMORY_REFRESH_SECONDS:
|
||||
params_memory.put_nonblocking("LastGPSPosition", gps_memory_state)
|
||||
last_offroad_gps_memory_write = monotonic_now
|
||||
|
||||
gps_signature = gps_position_signature(gps_position)
|
||||
persist_due = (monotonic_now - last_offroad_gps_persist_write) >= OFFROAD_GPS_PERSIST_REFRESH_SECONDS
|
||||
first_persist = last_offroad_gps_persist_signature is None
|
||||
if gps_signature != last_offroad_gps_persist_signature and (first_persist or persist_due):
|
||||
params.put("LastGPSPosition", gps_memory_state)
|
||||
last_offroad_gps_persist_signature = gps_signature
|
||||
last_offroad_gps_persist_write = monotonic_now
|
||||
|
||||
started_previously = started
|
||||
|
||||
if not started and time_validated and sm["deviceState"].screenBrightnessPercent > 0:
|
||||
|
||||
@@ -303,8 +303,7 @@ export function NavDestination() {
|
||||
state.destination = JSON.parse(data.destination);
|
||||
} catch { }
|
||||
|
||||
state.lastPosition = parseCoordinatePair(data.lastPosition) ??
|
||||
parseCoordinatePair(state.destination);
|
||||
state.lastPosition = parseCoordinatePair(data.lastPosition);
|
||||
|
||||
try {
|
||||
const prev = JSON.parse(data.previousDestinations);
|
||||
|
||||
@@ -462,6 +462,9 @@ KEYS = {
|
||||
"secret": ("secret", "sk.", "MapboxSecretKey", "Secret key", 80),
|
||||
}
|
||||
|
||||
NAVIGATION_MEMORY_LOCATION_STALE_SECONDS = 10.0
|
||||
NAVIGATION_PERSISTED_LOCATION_BOOT_SKEW_SECONDS = 5.0
|
||||
|
||||
TMUX_LOGS_PATH = Path("/data/tmux_logs")
|
||||
|
||||
MODEL_DOWNLOAD_PARAM = "ModelToDownload"
|
||||
@@ -473,6 +476,82 @@ MODEL_USER_FAVORITES_PARAM = "UserFavorites"
|
||||
MAPS_DOWNLOAD_PARAM = "DownloadMaps"
|
||||
MAPS_CANCEL_DOWNLOAD_PARAM = "CancelDownloadMaps"
|
||||
|
||||
|
||||
def _parse_last_gps_position(raw_value):
|
||||
if not raw_value:
|
||||
return None
|
||||
|
||||
try:
|
||||
payload = json.loads(raw_value)
|
||||
except (TypeError, ValueError, json.JSONDecodeError):
|
||||
return None
|
||||
|
||||
if not isinstance(payload, dict):
|
||||
return None
|
||||
|
||||
latitude = payload.get("latitude")
|
||||
longitude = payload.get("longitude")
|
||||
has_fix = payload.get("hasFix", False)
|
||||
try:
|
||||
latitude = float(latitude or 0.0)
|
||||
longitude = float(longitude or 0.0)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
if not has_fix or (abs(latitude) < 1e-6 and abs(longitude) < 1e-6):
|
||||
return None
|
||||
|
||||
payload["latitude"] = latitude
|
||||
payload["longitude"] = longitude
|
||||
return payload
|
||||
|
||||
|
||||
def _last_gps_position_is_live(payload):
|
||||
if not isinstance(payload, dict):
|
||||
return False
|
||||
|
||||
try:
|
||||
updated_at_monotonic = float(payload.get("updatedAtMonotonic", 0.0) or 0.0)
|
||||
except (TypeError, ValueError):
|
||||
updated_at_monotonic = 0.0
|
||||
|
||||
if updated_at_monotonic <= 0.0:
|
||||
return False
|
||||
|
||||
return (time.monotonic() - updated_at_monotonic) <= NAVIGATION_MEMORY_LOCATION_STALE_SECONDS
|
||||
|
||||
|
||||
def _last_gps_position_is_current_boot(payload):
|
||||
if not isinstance(payload, dict):
|
||||
return False
|
||||
|
||||
try:
|
||||
updated_at_sec = float(payload.get("updatedAtSec", 0.0) or 0.0)
|
||||
except (TypeError, ValueError):
|
||||
updated_at_sec = 0.0
|
||||
|
||||
if updated_at_sec <= 0.0:
|
||||
return False
|
||||
|
||||
now_sec = time.time()
|
||||
boot_started_at_sec = now_sec - time.monotonic()
|
||||
if updated_at_sec > (now_sec + 60.0):
|
||||
return False
|
||||
|
||||
return updated_at_sec >= (boot_started_at_sec - NAVIGATION_PERSISTED_LOCATION_BOOT_SKEW_SECONDS)
|
||||
|
||||
|
||||
def _get_navigation_last_position():
|
||||
memory_position = _parse_last_gps_position(params_memory.get("LastGPSPosition", encoding="utf8") or "")
|
||||
if _last_gps_position_is_live(memory_position):
|
||||
return memory_position
|
||||
|
||||
persisted_position = _parse_last_gps_position(params.get("LastGPSPosition", encoding="utf8") or "")
|
||||
if _last_gps_position_is_current_boot(persisted_position):
|
||||
return persisted_position
|
||||
|
||||
return None
|
||||
|
||||
FINGERPRINT_MAKE_LABELS = [
|
||||
"Acura",
|
||||
"Audi",
|
||||
@@ -3444,14 +3523,7 @@ def setup(app):
|
||||
|
||||
@app.route("/api/navigation", methods=["GET"])
|
||||
def navigation():
|
||||
last_position_raw = params.get("LastGPSPosition", encoding="utf8") or ""
|
||||
try:
|
||||
last_position = json.loads(last_position_raw) if last_position_raw else {}
|
||||
except (TypeError, ValueError, json.JSONDecodeError):
|
||||
last_position = {}
|
||||
|
||||
if not isinstance(last_position, dict):
|
||||
last_position = {}
|
||||
last_position = _get_navigation_last_position() or {}
|
||||
|
||||
return {
|
||||
"amap1Key": params.get("AMapKey1", encoding="utf8") or "",
|
||||
|
||||
Reference in New Issue
Block a user