mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-09-30 19:33:49 +08:00
Offer applies with enrollment and triple advantage
This commit is contained in:
firestar5683
Binary file not shown.
@@ -382,6 +382,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"NavLongitudinalAllowed", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"NavDestination", {PERSISTENT, STRING, "", ""}},
|
||||
{"NavInstructionCollapsed", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}},
|
||||
{"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}},
|
||||
{"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
|
||||
{"VisionSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
|
||||
|
||||
Binary file not shown.
@@ -297,6 +297,7 @@ class CarController(CarControllerBase):
|
||||
copies_xp = REDNECK_BUTTON_COPIES_TIME_METRIC if CS.is_metric else REDNECK_BUTTON_COPIES_TIME_IMPERIAL
|
||||
copies = int(np.interp(REDNECK_BUTTON_COPIES_TIME, copies_xp, [1, REDNECK_BUTTON_COPIES]))
|
||||
can_sends = [hyundaican.create_clu11(self.packer, self.frame, CS.clu11, send_button, self.CP)] * copies
|
||||
CS.redneck_last_sent_button = getattr(CS, "redneck_send_button", 0)
|
||||
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
|
||||
self.last_button_frame = self.frame
|
||||
@@ -318,6 +319,7 @@ class CarController(CarControllerBase):
|
||||
hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, (CS.buttons_counter + button_counter_offset) % 0xF, send_button)
|
||||
for _ in range(20)
|
||||
]
|
||||
CS.redneck_last_sent_button = getattr(CS, "redneck_send_button", 0)
|
||||
self.last_button_frame = self.frame
|
||||
return can_sends
|
||||
|
||||
@@ -376,6 +378,7 @@ class CarController(CarControllerBase):
|
||||
accel = accel_cmd
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
|
||||
CS.redneck_last_sent_button = 0
|
||||
|
||||
can_sends = []
|
||||
|
||||
|
||||
@@ -2,6 +2,8 @@ import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.can.dbc import DBC as DBCReader
|
||||
from opendbc.can.parser import get_raw_value
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.interfaces import RadarInterfaceBase
|
||||
from opendbc.car.hyundai.values import CAR, DBC, HYUNDAI_MANDO_FRONT_RADAR_DBC, HYUNDAI_MRR30_RADAR_DBC, \
|
||||
@@ -10,6 +12,7 @@ from openpilot.common.swaglog import cloudlog
|
||||
|
||||
RADAR_START_ADDR = 0x500
|
||||
RADAR_MSG_COUNT = 32
|
||||
G90_RADAR_MSG_COUNT = 64
|
||||
MRR30_RADAR_START_ADDR = 0x210
|
||||
MRR30_RADAR_MSG_COUNT = 16
|
||||
MRR35_RADAR_START_ADDR = 0x3A5
|
||||
@@ -23,6 +26,11 @@ class RadarTrackConfig:
|
||||
radar_type: str
|
||||
bus: int = 1
|
||||
frequency: int = 50
|
||||
parser_msg_count: int | None = None
|
||||
|
||||
@property
|
||||
def can_parser_msg_count(self) -> int:
|
||||
return self.parser_msg_count if self.parser_msg_count is not None else self.msg_count
|
||||
|
||||
|
||||
RADAR_TRACK_CONFIGS = {
|
||||
@@ -36,6 +44,8 @@ RADAR_TRACK_CONFIGS = {
|
||||
|
||||
def get_radar_track_config(car_fingerprint) -> RadarTrackConfig | None:
|
||||
radar_dbc = DBC[car_fingerprint].get(Bus.radar)
|
||||
if car_fingerprint == CAR.GENESIS_G90 and radar_dbc == HYUNDAI_MANDO_FRONT_RADAR_DBC:
|
||||
return RadarTrackConfig(RADAR_START_ADDR, G90_RADAR_MSG_COUNT, "mando", parser_msg_count=RADAR_MSG_COUNT)
|
||||
return RADAR_TRACK_CONFIGS.get(radar_dbc)
|
||||
|
||||
|
||||
@@ -43,7 +53,8 @@ def get_radar_can_parser(CP, radar_config):
|
||||
if radar_config is None:
|
||||
return None
|
||||
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency) for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.msg_count)]
|
||||
messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency)
|
||||
for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.can_parser_msg_count)]
|
||||
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, radar_config.bus)
|
||||
|
||||
|
||||
@@ -52,8 +63,15 @@ class RadarInterface(RadarInterfaceBase):
|
||||
super().__init__(CP)
|
||||
self.radar_config = get_radar_track_config(CP.carFingerprint)
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = (self.radar_config.start_addr + self.radar_config.msg_count - 1) if self.radar_config is not None else RADAR_START_ADDR
|
||||
self.trigger_msg = (self.radar_config.start_addr + self.radar_config.can_parser_msg_count - 1
|
||||
if self.radar_config is not None else RADAR_START_ADDR)
|
||||
self.track_id = 0
|
||||
self.g90_extended_mando = (CP.carFingerprint == CAR.GENESIS_G90 and self.radar_config is not None and
|
||||
self.radar_config.msg_count > self.radar_config.can_parser_msg_count)
|
||||
self.g90_mando_signals = []
|
||||
if self.g90_extended_mando:
|
||||
radar_dbc = DBCReader(DBC[CP.carFingerprint][Bus.radar])
|
||||
self.g90_mando_signals = list(radar_dbc.addr_to_msg[RADAR_START_ADDR].sigs.values())
|
||||
|
||||
self.radar_off_can = CP.radarUnavailable
|
||||
# Probe whether radar tracks still exist on the Ioniq 6 while OP long is active,
|
||||
@@ -70,7 +88,7 @@ class RadarInterface(RadarInterfaceBase):
|
||||
if self.radar_config is not None:
|
||||
self.track_addrs = [(addr, f"RADAR_TRACK_{addr:x}")
|
||||
for addr in range(self.radar_config.start_addr,
|
||||
self.radar_config.start_addr + self.radar_config.msg_count)]
|
||||
self.radar_config.start_addr + self.radar_config.can_parser_msg_count)]
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.ioniq_6_radar_probe and self.rcp is not None and not self.ioniq_6_radar_probe_logged:
|
||||
@@ -93,6 +111,8 @@ class RadarInterface(RadarInterfaceBase):
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
if self.g90_extended_mando:
|
||||
self._update_g90_extended_mando_tracks(can_strings)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
@@ -102,6 +122,46 @@ class RadarInterface(RadarInterfaceBase):
|
||||
|
||||
return rr
|
||||
|
||||
def _decode_g90_mando_values(self, dat: bytes):
|
||||
vals = {}
|
||||
for sig in self.g90_mando_signals:
|
||||
raw = get_raw_value(dat, sig)
|
||||
if sig.is_signed:
|
||||
raw -= ((raw >> (sig.size - 1)) & 1) * (1 << sig.size)
|
||||
vals[sig.name] = raw * sig.factor + sig.offset
|
||||
return vals
|
||||
|
||||
def _update_g90_extended_mando_tracks(self, can_strings):
|
||||
if self.radar_config is None:
|
||||
return
|
||||
|
||||
start_addr = self.radar_config.start_addr + self.radar_config.can_parser_msg_count
|
||||
end_addr = self.radar_config.start_addr + self.radar_config.msg_count
|
||||
|
||||
for _, frames in can_strings:
|
||||
for address, dat, src in frames:
|
||||
if src != self.radar_config.bus or not (start_addr <= address < end_addr) or len(dat) < 8:
|
||||
continue
|
||||
|
||||
self.updated_messages.add(address)
|
||||
msg = self._decode_g90_mando_values(dat)
|
||||
valid = msg["STATE"] in (3, 4)
|
||||
if valid:
|
||||
if address not in self.pts:
|
||||
self.pts[address] = structs.RadarData.RadarPoint()
|
||||
self.pts[address].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
|
||||
azimuth = math.radians(msg["AZIMUTH"])
|
||||
self.pts[address].measured = True
|
||||
self.pts[address].dRel = math.cos(azimuth) * msg["LONG_DIST"]
|
||||
self.pts[address].yRel = 0.5 * -math.sin(azimuth) * msg["LONG_DIST"]
|
||||
self.pts[address].vRel = msg["REL_SPEED"]
|
||||
self.pts[address].aRel = msg["REL_ACCEL"]
|
||||
self.pts[address].yvRel = float("nan")
|
||||
elif address in self.pts:
|
||||
del self.pts[address]
|
||||
|
||||
def _update(self, updated_messages):
|
||||
ret = structs.RadarData()
|
||||
if self.rcp is None:
|
||||
|
||||
@@ -120,7 +120,7 @@ class TestHyundaiFingerprint:
|
||||
assert bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) == lka_steering
|
||||
|
||||
# radar available
|
||||
for candidate in (CAR.HYUNDAI_SONATA, CAR.GENESIS_G90):
|
||||
for candidate in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SONATA_HYBRID, CAR.GENESIS_G90):
|
||||
assert get_radar_track_config(candidate).start_addr == RADAR_START_ADDR
|
||||
for radar in (True, False):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
@@ -129,11 +129,16 @@ class TestHyundaiFingerprint:
|
||||
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None)
|
||||
assert CP.radarUnavailable != radar
|
||||
|
||||
assert get_radar_track_config(CAR.HYUNDAI_SONATA_HYBRID).msg_count == 32
|
||||
assert get_radar_track_config(CAR.GENESIS_G90).msg_count == 64
|
||||
assert get_radar_track_config(CAR.GENESIS_G90).can_parser_msg_count == 32
|
||||
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[1][RADAR_START_ADDR] = 8
|
||||
CP = CarInterface.get_params(CAR.GENESIS_G90, fingerprint, [], True, False, False, None)
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.radarUnavailable
|
||||
for candidate in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SONATA_HYBRID, CAR.GENESIS_G90):
|
||||
CP = CarInterface.get_params(candidate, fingerprint, [], True, False, False, None)
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.radarUnavailable
|
||||
|
||||
for candidate, radar_addr in (
|
||||
(CAR.HYUNDAI_IONIQ_5, MRR30_RADAR_START_ADDR),
|
||||
|
||||
@@ -1111,7 +1111,11 @@ CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) # TODO: merge with
|
||||
CANFD_SECURITYACCESS_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_KONA_EV_2ND_GEN}
|
||||
CANFD_UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) - CANFD_SECURITYACCESS_CAR # TODO: merge with UNSUPPORTED_LONGITUDINAL_CAR
|
||||
CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6}
|
||||
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {CAR.GENESIS_G90}
|
||||
RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {
|
||||
CAR.HYUNDAI_SONATA,
|
||||
CAR.HYUNDAI_SONATA_HYBRID,
|
||||
CAR.GENESIS_G90,
|
||||
}
|
||||
|
||||
CAMERA_SCC_CAR = CAR.with_flags(HyundaiFlags.CAMERA_SCC)
|
||||
|
||||
|
||||
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-f06c82b2-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-75b1deaf-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-f06c82b2-DEBUG
|
||||
DEV-75b1deaf-DEBUG
|
||||
+60
-11
@@ -22,7 +22,7 @@ from opendbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN
|
||||
from openpilot.selfdrive.car.redneck_cruise import RedneckCruise
|
||||
from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE
|
||||
from openpilot.selfdrive.car.car_specific import MockCarState
|
||||
|
||||
from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles
|
||||
@@ -30,6 +30,8 @@ from openpilot.starpilot.controls.starpilot_card import StarPilotCard
|
||||
|
||||
REPLAY = "REPLAY" in os.environ
|
||||
OPENPILOT_LEAD_MIN_DISTANCE = 0.1
|
||||
REDNECK_DECREASE_LOOKAHEAD_POINTS = 10
|
||||
REDNECK_AUTO_BUTTON_FILTER_FRAMES = int(0.3 / DT_CTRL)
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
|
||||
@@ -165,8 +167,12 @@ class Car:
|
||||
self.params.put_nonblocking("CarParamsPersistent", cp_bytes)
|
||||
|
||||
self.mock_carstate = MockCarState()
|
||||
self.v_cruise_helper = VCruiseHelper(self.CP)
|
||||
self.v_cruise_helper = VCruiseHelper(self.CP, self.FPCP)
|
||||
self.redneck_cruise = RedneckCruise(self.CP, self.FPCP) if self.CP.brand == "hyundai" else None
|
||||
self.redneck_button_event_filter_frames = {
|
||||
int(ButtonType.accelCruise): 0,
|
||||
int(ButtonType.decelCruise): 0,
|
||||
}
|
||||
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.safe_mode = self.params.get_bool("SafeMode")
|
||||
@@ -210,6 +216,8 @@ class Car:
|
||||
if self.CP.brand == 'mock':
|
||||
CS, FPCS = self.mock_carstate.update(CS, FPCS)
|
||||
|
||||
self._filter_redneck_button_events(CS)
|
||||
|
||||
# Update radar tracks from CAN
|
||||
RD: structs.RadarDataT | None = self.RI.update(can_list)
|
||||
|
||||
@@ -345,6 +353,7 @@ class Car:
|
||||
self._update_redneck_cruise(CS, CC)
|
||||
self._update_openpilot_lead_state(CC)
|
||||
self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles)
|
||||
self._record_redneck_button_feedback_filter()
|
||||
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
|
||||
|
||||
self.CC_prev = CC
|
||||
@@ -373,19 +382,59 @@ class Car:
|
||||
if self.redneck_cruise is None:
|
||||
return
|
||||
|
||||
send_button, v_target = self.redneck_cruise.run(CS, CC, self._get_redneck_target_speed(), self.is_metric)
|
||||
send_button, v_target = self.redneck_cruise.run(CS, CC, self._get_redneck_target_speed(CS), self.is_metric)
|
||||
self.CI.CS.redneck_send_button = send_button
|
||||
self.CI.CS.redneck_v_target = v_target
|
||||
|
||||
def _get_redneck_target_speed(self) -> float:
|
||||
if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']:
|
||||
speeds = self.sm['longitudinalPlan'].speeds
|
||||
if len(speeds) > 0:
|
||||
target_speed = float(speeds[0])
|
||||
if math.isfinite(target_speed):
|
||||
return target_speed
|
||||
def _get_redneck_target_speed(self, CS: car.CarState) -> float:
|
||||
fallback_target_speed = float(CS.cruiseState.speedCluster)
|
||||
|
||||
return float(self.sm['starpilotPlan'].vCruise)
|
||||
if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']:
|
||||
starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise)
|
||||
if starpilot_target_speed > 0.0:
|
||||
fallback_target_speed = starpilot_target_speed
|
||||
|
||||
if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']:
|
||||
plan_speeds = [float(speed) for speed in self.sm['longitudinalPlan'].speeds if math.isfinite(float(speed))]
|
||||
if len(plan_speeds) > 0:
|
||||
target_speed = plan_speeds[0]
|
||||
decrease_target_speed = min(plan_speeds[:REDNECK_DECREASE_LOOKAHEAD_POINTS])
|
||||
if decrease_target_speed < min(target_speed, float(CS.cruiseState.speedCluster)):
|
||||
return decrease_target_speed
|
||||
return target_speed
|
||||
|
||||
return fallback_target_speed
|
||||
|
||||
def _filter_redneck_button_events(self, CS: car.CarState) -> None:
|
||||
if self.redneck_cruise is None:
|
||||
return
|
||||
|
||||
for button_type in self.redneck_button_event_filter_frames:
|
||||
self.redneck_button_event_filter_frames[button_type] = max(0, self.redneck_button_event_filter_frames[button_type] - 1)
|
||||
|
||||
if len(CS.buttonEvents) == 0:
|
||||
return
|
||||
|
||||
filtered_button_events = []
|
||||
for event in CS.buttonEvents:
|
||||
button_type = event.type.raw if hasattr(event.type, "raw") else int(event.type)
|
||||
if self.redneck_button_event_filter_frames.get(button_type, 0) > 0:
|
||||
continue
|
||||
filtered_button_events.append(event)
|
||||
|
||||
CS.buttonEvents = filtered_button_events
|
||||
|
||||
def _record_redneck_button_feedback_filter(self) -> None:
|
||||
if self.redneck_cruise is None:
|
||||
return
|
||||
|
||||
sent_button = int(getattr(self.CI.CS, "redneck_last_sent_button", 0) or 0)
|
||||
if sent_button == SEND_BUTTON_INCREASE:
|
||||
self.redneck_button_event_filter_frames[int(ButtonType.accelCruise)] = REDNECK_AUTO_BUTTON_FILTER_FRAMES
|
||||
elif sent_button == SEND_BUTTON_DECREASE:
|
||||
self.redneck_button_event_filter_frames[int(ButtonType.decelCruise)] = REDNECK_AUTO_BUTTON_FILTER_FRAMES
|
||||
|
||||
self.CI.CS.redneck_last_sent_button = 0
|
||||
|
||||
def step(self):
|
||||
CS, RD, FPCS = self.state_update()
|
||||
|
||||
@@ -32,7 +32,7 @@ CRUISE_INTERVAL_SIGN = {
|
||||
|
||||
|
||||
class VCruiseHelper:
|
||||
def __init__(self, CP):
|
||||
def __init__(self, CP, FPCP=None):
|
||||
self.CP = CP
|
||||
self.v_cruise_kph = V_CRUISE_UNSET
|
||||
self.v_cruise_cluster_kph = V_CRUISE_UNSET
|
||||
@@ -41,6 +41,7 @@ class VCruiseHelper:
|
||||
self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
|
||||
|
||||
self.gm_cc_only = self.CP.carFingerprint in CC_ONLY_CAR and self.CP.flags & GMFlags.CC_LONG.value
|
||||
self.redneck_non_pcm = bool(FPCP is not None and not getattr(FPCP, "pcmCruiseSpeed", True))
|
||||
|
||||
def _get_short_press_delta(self, is_metric, starpilot_toggles: SimpleNamespace) -> float:
|
||||
base_delta = 1. if is_metric else IMPERIAL_INCREMENT
|
||||
@@ -58,7 +59,7 @@ class VCruiseHelper:
|
||||
self.v_cruise_kph_last = self.v_cruise_kph
|
||||
|
||||
if CS.cruiseState.available:
|
||||
if self.gm_cc_only or not self.CP.pcmCruise:
|
||||
if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise:
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles)
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
@@ -145,7 +146,7 @@ class VCruiseHelper:
|
||||
def initialize_v_cruise(self, CS, experimental_mode: bool, resume_prev_button: bool,
|
||||
starpilot_toggles: SimpleNamespace, desired_speed_limit: float = 0.0) -> None:
|
||||
# initializing is handled by the PCM
|
||||
if self.CP.pcmCruise and not self.gm_cc_only:
|
||||
if self.CP.pcmCruise and not (self.gm_cc_only or self.redneck_non_pcm):
|
||||
return
|
||||
|
||||
engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH)
|
||||
@@ -158,6 +159,8 @@ class VCruiseHelper:
|
||||
# the custom cruise-button interval.
|
||||
initialized_speed_limit_kph = round(desired_speed_limit * CV.MS_TO_KPH, 1)
|
||||
self.v_cruise_kph = float(np.clip(initialized_speed_limit_kph, V_CRUISE_MIN, V_CRUISE_MAX))
|
||||
elif self.redneck_non_pcm and CS.cruiseState.speedCluster > 0:
|
||||
self.v_cruise_kph = float(np.clip(CS.cruiseState.speedCluster * CV.MS_TO_KPH, V_CRUISE_MIN, V_CRUISE_MAX))
|
||||
else:
|
||||
self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, engage_floor_kph, V_CRUISE_MAX)))
|
||||
|
||||
|
||||
@@ -10,7 +10,8 @@ SEND_BUTTON_INCREASE = 1
|
||||
SEND_BUTTON_DECREASE = 2
|
||||
|
||||
HYST_GAP = 0.0
|
||||
INACTIVE_TIMER = 0.4
|
||||
INCREASE_INACTIVE_TIMER = 0.4
|
||||
DECREASE_INACTIVE_TIMER = 0.1
|
||||
|
||||
CRUISE_BUTTON_TIMERS = {
|
||||
int(ButtonType.decelCruise): 0,
|
||||
@@ -80,32 +81,53 @@ class RedneckCruise:
|
||||
button_pressed = any(timer > 0 for timer in self.cruise_button_timers.values())
|
||||
self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed
|
||||
|
||||
def _update_state_machine(self) -> int:
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
def _desired_state(self) -> str:
|
||||
if self.v_target > self.v_cruise_cluster:
|
||||
return "increasing"
|
||||
if self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
|
||||
return "decreasing"
|
||||
return "holding"
|
||||
|
||||
if self.state != "inactive":
|
||||
if not self.is_ready:
|
||||
self.state = "inactive"
|
||||
elif self.state == "preActive":
|
||||
@staticmethod
|
||||
def _get_pre_active_frames(state: str) -> int:
|
||||
timer = DECREASE_INACTIVE_TIMER if state == "decreasing" else INCREASE_INACTIVE_TIMER
|
||||
return int(timer / DT_CTRL)
|
||||
|
||||
def _arm_pre_active(self, desired_state: str) -> None:
|
||||
if desired_state == "holding":
|
||||
self.state = "holding"
|
||||
self.pre_active_timer = 0
|
||||
return
|
||||
|
||||
self.state = "preActive"
|
||||
self.pre_active_timer = self._get_pre_active_frames(desired_state)
|
||||
|
||||
def _update_state_machine(self) -> int:
|
||||
desired_state = self._desired_state()
|
||||
|
||||
if not self.is_ready:
|
||||
self.state = "inactive"
|
||||
self.pre_active_timer = 0
|
||||
elif self.state == "inactive":
|
||||
if not self.is_ready_prev:
|
||||
self._arm_pre_active(desired_state)
|
||||
elif self.state == "preActive":
|
||||
if desired_state == "holding":
|
||||
self.state = "holding"
|
||||
self.pre_active_timer = 0
|
||||
else:
|
||||
desired_frames = self._get_pre_active_frames(desired_state)
|
||||
self.pre_active_timer = max(0, min(self.pre_active_timer, desired_frames) - 1)
|
||||
if self.pre_active_timer <= 0:
|
||||
if self.v_target == self.v_cruise_cluster:
|
||||
self.state = "holding"
|
||||
elif self.v_target > self.v_cruise_cluster:
|
||||
self.state = "increasing"
|
||||
elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
|
||||
self.state = "decreasing"
|
||||
elif self.state == "holding":
|
||||
if self.v_target != self.v_cruise_cluster:
|
||||
self.state = "preActive"
|
||||
elif self.state == "increasing":
|
||||
if self.v_target <= self.v_cruise_cluster:
|
||||
self.state = "holding"
|
||||
elif self.state == "decreasing":
|
||||
if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min:
|
||||
self.state = "holding"
|
||||
elif self.is_ready and not self.is_ready_prev:
|
||||
self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL)
|
||||
self.state = "preActive"
|
||||
self.state = desired_state
|
||||
elif self.state == "holding":
|
||||
if desired_state != "holding":
|
||||
self._arm_pre_active(desired_state)
|
||||
elif self.state != desired_state:
|
||||
if desired_state == "holding":
|
||||
self.state = "holding"
|
||||
else:
|
||||
self._arm_pre_active(desired_state)
|
||||
|
||||
return self._send_button_for_state(self.state)
|
||||
|
||||
|
||||
@@ -304,3 +304,43 @@ class TestVCruiseHelper:
|
||||
)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == pytest.approx(initial_v_cruise_kph + IMPERIAL_INCREMENT)
|
||||
|
||||
|
||||
class TestVCruiseHelperRedneck:
|
||||
def setup_method(self):
|
||||
self.CP = car.CarParams(pcmCruise=True)
|
||||
self.FPCP = SimpleNamespace(pcmCruiseSpeed=False)
|
||||
self.v_cruise_helper = VCruiseHelper(self.CP, self.FPCP)
|
||||
self.starpilot_toggles = SimpleNamespace(
|
||||
cruise_increase=1,
|
||||
cruise_increase_long=5,
|
||||
is_metric=False,
|
||||
set_speed_limit=False,
|
||||
)
|
||||
|
||||
def test_initialize_v_cruise_uses_cluster_speed(self):
|
||||
cs = car.CarState(
|
||||
vEgo=55 * CV.MPH_TO_MS,
|
||||
cruiseState={"speedCluster": 62 * CV.MPH_TO_MS},
|
||||
)
|
||||
self.v_cruise_helper.initialize_v_cruise(cs, experimental_mode=False, resume_prev_button=False,
|
||||
starpilot_toggles=self.starpilot_toggles)
|
||||
assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH)
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH)
|
||||
|
||||
def test_update_v_cruise_does_not_follow_stock_pcm_speed(self):
|
||||
cs = car.CarState(
|
||||
vEgo=55 * CV.MPH_TO_MS,
|
||||
cruiseState={"speedCluster": 62 * CV.MPH_TO_MS},
|
||||
)
|
||||
self.v_cruise_helper.initialize_v_cruise(cs, experimental_mode=False, resume_prev_button=False,
|
||||
starpilot_toggles=self.starpilot_toggles)
|
||||
|
||||
update_cs = car.CarState(
|
||||
cruiseState={"available": True, "speed": 50 * CV.MPH_TO_MS, "speedCluster": 50 * CV.MPH_TO_MS},
|
||||
)
|
||||
self.v_cruise_helper.update_v_cruise(update_cs, enabled=True, is_metric=False,
|
||||
speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles)
|
||||
|
||||
assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH)
|
||||
assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH)
|
||||
|
||||
@@ -5,7 +5,8 @@ from cereal import car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.car.redneck_cruise import (
|
||||
INACTIVE_TIMER,
|
||||
DECREASE_INACTIVE_TIMER,
|
||||
INCREASE_INACTIVE_TIMER,
|
||||
RedneckCruise,
|
||||
SEND_BUTTON_DECREASE,
|
||||
SEND_BUTTON_INCREASE,
|
||||
@@ -40,7 +41,7 @@ class TestRedneckCruise(unittest.TestCase):
|
||||
return SimpleNamespace(type=button_type, pressed=pressed)
|
||||
|
||||
def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None, override=False, cancel=False, resume=False):
|
||||
frames = int(INACTIVE_TIMER / DT_CTRL) + 2
|
||||
frames = int(max(INCREASE_INACTIVE_TIMER, DECREASE_INACTIVE_TIMER) / DT_CTRL) + 2
|
||||
send_button = SEND_BUTTON_NONE
|
||||
v_target = 0
|
||||
for _ in range(frames):
|
||||
@@ -53,6 +54,19 @@ class TestRedneckCruise(unittest.TestCase):
|
||||
button_events = None
|
||||
return send_button, v_target
|
||||
|
||||
def _frames_until_button(self, target_mph, speed_cluster_mph):
|
||||
frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4
|
||||
for frame in range(frames):
|
||||
send_button, _ = self.redneck.run(
|
||||
self._new_state(speed_cluster_mph=speed_cluster_mph),
|
||||
self._new_control(),
|
||||
target_mph * CV.MPH_TO_MS,
|
||||
is_metric=False,
|
||||
)
|
||||
if send_button != SEND_BUTTON_NONE:
|
||||
return frame
|
||||
return None
|
||||
|
||||
def test_increases_cluster_speed_toward_target(self):
|
||||
send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0)
|
||||
self.assertEqual(SEND_BUTTON_INCREASE, send_button)
|
||||
@@ -63,6 +77,15 @@ class TestRedneckCruise(unittest.TestCase):
|
||||
self.assertEqual(SEND_BUTTON_DECREASE, send_button)
|
||||
self.assertEqual(20, v_target)
|
||||
|
||||
def test_decrease_activates_faster_than_increase(self):
|
||||
decrease_frame = self._frames_until_button(target_mph=20.0, speed_cluster_mph=25.0)
|
||||
self.redneck = RedneckCruise(self.CP, self.FPCP)
|
||||
increase_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0)
|
||||
|
||||
self.assertIsNotNone(decrease_frame)
|
||||
self.assertIsNotNone(increase_frame)
|
||||
self.assertLess(decrease_frame, increase_frame)
|
||||
|
||||
def test_suppresses_output_during_manual_cruise_button_use(self):
|
||||
button_event = self._button_event(ButtonType.accelCruise, True)
|
||||
send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, button_events=[button_event])
|
||||
|
||||
@@ -113,6 +113,10 @@ VOLT_STANDARD_CARS = (
|
||||
GENESIS_G90_CARS = (
|
||||
HYUNDAI_CAR.GENESIS_G90,
|
||||
)
|
||||
PALISADE_CARS = (
|
||||
HYUNDAI_CAR.HYUNDAI_PALISADE,
|
||||
HYUNDAI_CAR.HYUNDAI_PALISADE_2023,
|
||||
)
|
||||
IONIQ_5_CARS = (
|
||||
HYUNDAI_CAR.HYUNDAI_IONIQ_5,
|
||||
)
|
||||
@@ -300,6 +304,31 @@ KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.03
|
||||
KIA_FORTE_CENTER_TAPER_SPEED = 24.0
|
||||
KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 2.5
|
||||
|
||||
PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98
|
||||
PALISADE_FF_GAIN_LEFT = 0.14
|
||||
PALISADE_FF_GAIN_RIGHT = 0.12
|
||||
PALISADE_FF_ONSET = 0.08
|
||||
PALISADE_FF_ONSET_WIDTH = 0.04
|
||||
PALISADE_FF_CUTOFF = 1.25
|
||||
PALISADE_FF_CUTOFF_WIDTH = 0.36
|
||||
PALISADE_TRANSITION_SPEED = 9.0
|
||||
PALISADE_PHASE_SCALE = 0.11
|
||||
PALISADE_TURN_IN_BOOST_LEFT = 0.34
|
||||
PALISADE_TURN_IN_BOOST_RIGHT = 0.24
|
||||
PALISADE_UNWIND_TAPER_LEFT = 0.18
|
||||
PALISADE_UNWIND_TAPER_RIGHT = 0.30
|
||||
PALISADE_FRICTION_MULT = 1.02
|
||||
PALISADE_FRICTION_LAT_RISE = 0.20
|
||||
PALISADE_FRICTION_JERK_RISE = 0.24
|
||||
PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18
|
||||
PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14
|
||||
PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14
|
||||
PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22
|
||||
PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08
|
||||
PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06
|
||||
PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12
|
||||
PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20
|
||||
|
||||
GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
|
||||
GENESIS_G90_FF_GAIN_LEFT = 0.32
|
||||
GENESIS_G90_FF_GAIN_RIGHT = 0.16
|
||||
@@ -1077,6 +1106,74 @@ def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float)
|
||||
return 1.0 - reduction
|
||||
|
||||
|
||||
def _palisade_sigmoid(x: float) -> float:
|
||||
return _sigmoid(x)
|
||||
|
||||
|
||||
def _palisade_low_speed_factor(v_ego: float) -> float:
|
||||
return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2)
|
||||
|
||||
|
||||
def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE)
|
||||
|
||||
|
||||
def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
|
||||
return left_value if desired_lateral_accel >= 0.0 else right_value
|
||||
|
||||
|
||||
def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE)
|
||||
jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE)
|
||||
return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor
|
||||
|
||||
|
||||
def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
|
||||
if desired_lateral_accel == 0.0:
|
||||
return 1.0
|
||||
|
||||
gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT)
|
||||
abs_lateral_accel = abs(desired_lateral_accel)
|
||||
onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH)
|
||||
cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH)
|
||||
extra_scale = gain * onset * cutoff
|
||||
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
|
||||
turn_in_weight = max(phase, 0.0)
|
||||
unwind_weight = max(-phase, 0.0)
|
||||
low_speed_factor = _palisade_low_speed_factor(v_ego)
|
||||
turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) *
|
||||
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
|
||||
unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_UNWIND_TAPER_RIGHT) *
|
||||
unwind_weight * (0.35 + 0.65 * low_speed_factor))
|
||||
return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
|
||||
|
||||
|
||||
def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
|
||||
base_threshold = get_friction_threshold(v_ego)
|
||||
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
|
||||
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
|
||||
turn_in_weight = max(phase, 0.0)
|
||||
unwind_weight = max(-phase, 0.0)
|
||||
threshold_scale = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) *
|
||||
transition_envelope * turn_in_weight)
|
||||
threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) *
|
||||
transition_envelope * unwind_weight)
|
||||
return base_threshold * min(max(threshold_scale, 0.84), 1.14)
|
||||
|
||||
|
||||
def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
|
||||
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
|
||||
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
|
||||
turn_in_weight = max(phase, 0.0)
|
||||
unwind_weight = max(-phase, 0.0)
|
||||
friction_scale = PALISADE_FRICTION_MULT
|
||||
friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) *
|
||||
transition_envelope * turn_in_weight)
|
||||
friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) *
|
||||
transition_envelope * unwind_weight)
|
||||
return min(max(friction_scale, 0.92), 1.12)
|
||||
|
||||
|
||||
def genesis_g90_lateral_testing_ground_active() -> bool:
|
||||
return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID)
|
||||
|
||||
@@ -1577,6 +1674,7 @@ class LatControlTorque(LatControl):
|
||||
self.is_bolt_2017 = CP.carFingerprint in BOLT_2017_CARS
|
||||
self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS
|
||||
self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS
|
||||
self.is_palisade = CP.carFingerprint in PALISADE_CARS
|
||||
self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
|
||||
self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS
|
||||
self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS
|
||||
@@ -1593,6 +1691,8 @@ class LatControlTorque(LatControl):
|
||||
self.torque_ff_scale_neg = 1.0
|
||||
self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0))
|
||||
self.torque_ki_mult = 1.0
|
||||
if self.is_palisade:
|
||||
self.torque_params.latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT
|
||||
if self.is_ioniq_5:
|
||||
self.torque_params.latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT
|
||||
if self.is_ioniq_ev_old:
|
||||
@@ -1620,6 +1720,8 @@ class LatControlTorque(LatControl):
|
||||
self.pid._k_i = [self.pid._k_i[0], [k * self.torque_ki_mult for k in self.pid._k_i[1]]]
|
||||
|
||||
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
|
||||
if self.is_palisade:
|
||||
latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT
|
||||
if self.is_ioniq_5:
|
||||
latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT
|
||||
if self.is_ioniq_ev_old:
|
||||
@@ -1703,6 +1805,7 @@ class LatControlTorque(LatControl):
|
||||
bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021
|
||||
volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active()
|
||||
genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active()
|
||||
palisade_active = self.is_palisade
|
||||
ioniq_5_active = self.is_ioniq_5
|
||||
ioniq_ev_old_active = self.is_ioniq_ev_old
|
||||
ioniq_6_active = self.is_ioniq_6
|
||||
@@ -1739,6 +1842,10 @@ class LatControlTorque(LatControl):
|
||||
ff *= get_genesis_g90_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
elif palisade_active:
|
||||
ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
|
||||
friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
elif ioniq_5_active:
|
||||
ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper
|
||||
friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
|
||||
|
||||
@@ -590,7 +590,8 @@ class LongitudinalMpc:
|
||||
self.max_a = max_a
|
||||
|
||||
def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow,
|
||||
personality=log.LongitudinalPersonality.standard, tracking_lead=True):
|
||||
personality=log.LongitudinalPersonality.standard, tracking_lead=True,
|
||||
optional_far_lead_comfort=True):
|
||||
v_ego = self.x0[1]
|
||||
lead_one = radarstate.leadOne
|
||||
lead_two = radarstate.leadTwo
|
||||
@@ -622,11 +623,12 @@ class LongitudinalMpc:
|
||||
v_upper)
|
||||
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
|
||||
prev_source = self.source
|
||||
if prev_source == 'lead0':
|
||||
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow)
|
||||
elif prev_source == 'lead1':
|
||||
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow)
|
||||
if tracking_lead and lead_one.status:
|
||||
if optional_far_lead_comfort:
|
||||
if prev_source == 'lead0':
|
||||
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow)
|
||||
elif prev_source == 'lead1':
|
||||
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow)
|
||||
if optional_far_lead_comfort and tracking_lead and lead_one.status:
|
||||
desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
|
||||
closing_speed = max(0.0, v_ego - lead_one.vLead)
|
||||
cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed, v_cruise=v_cruise)
|
||||
|
||||
@@ -1086,17 +1086,15 @@ class LongitudinalPlanner:
|
||||
return self.lead_two
|
||||
return None
|
||||
|
||||
def get_follow_control_lead(self, lead_control_active, v_ego, t_follow):
|
||||
matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow)
|
||||
if matched_follow_lead is not None:
|
||||
return matched_follow_lead
|
||||
def get_follow_control_lead(self, lead_control_active, v_ego, t_follow, *, allow_optional_far_lead_logic=True):
|
||||
if allow_optional_far_lead_logic:
|
||||
matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow)
|
||||
if matched_follow_lead is not None:
|
||||
return matched_follow_lead
|
||||
|
||||
if not lead_control_active:
|
||||
return None
|
||||
|
||||
if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow):
|
||||
return self.lead_two
|
||||
|
||||
if self.lead_one.status:
|
||||
return self.lead_one
|
||||
if self.lead_two.status:
|
||||
@@ -1560,9 +1558,11 @@ class LongitudinalPlanner:
|
||||
dec_mpc_mode = self.get_mpc_mode()
|
||||
if not self.mlsim:
|
||||
self.mpc.mode = dec_mpc_mode
|
||||
optional_far_lead_comfort = getattr(starpilot_toggles, "coast_up_to_leads", True)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
|
||||
sm['starpilotPlan'].dangerFactor, effective_t_follow,
|
||||
personality=personality, tracking_lead=lead_control_active)
|
||||
personality=personality, tracking_lead=lead_control_active,
|
||||
optional_far_lead_comfort=optional_far_lead_comfort)
|
||||
|
||||
self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
@@ -1831,7 +1831,12 @@ class LongitudinalPlanner:
|
||||
if vision_brake_cap_active:
|
||||
output_accel_min = min(output_accel_min, vision_cap_accel_min)
|
||||
|
||||
follow_control_lead = self.get_follow_control_lead(lead_control_active, scene_v_ego, effective_t_follow)
|
||||
follow_control_lead = self.get_follow_control_lead(
|
||||
lead_control_active,
|
||||
scene_v_ego,
|
||||
effective_t_follow,
|
||||
allow_optional_far_lead_logic=optional_far_lead_comfort,
|
||||
)
|
||||
if follow_control_lead is not None and not panic_bypass:
|
||||
if not output_should_stop and not vision_low_speed_stop_active:
|
||||
tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor(
|
||||
@@ -1845,10 +1850,11 @@ class LongitudinalPlanner:
|
||||
self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor)
|
||||
output_a_target = min(output_a_target, tracked_vision_model_brake_floor)
|
||||
|
||||
matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow)
|
||||
if matched_follow_brake_cap is not None:
|
||||
self.a_desired = max(self.a_desired, matched_follow_brake_cap)
|
||||
output_a_target = max(output_a_target, matched_follow_brake_cap)
|
||||
if optional_far_lead_comfort:
|
||||
matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow)
|
||||
if matched_follow_brake_cap is not None:
|
||||
self.a_desired = max(self.a_desired, matched_follow_brake_cap)
|
||||
output_a_target = max(output_a_target, matched_follow_brake_cap)
|
||||
|
||||
if not close_lead_caps and not output_should_stop and not vision_low_speed_stop_active:
|
||||
low_speed_transition_brake_cap = self.get_low_speed_follow_transition_brake_cap(
|
||||
@@ -1863,7 +1869,7 @@ class LongitudinalPlanner:
|
||||
output_a_target = max(output_a_target, low_speed_transition_brake_cap)
|
||||
|
||||
comfort_lead = self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one
|
||||
if comfort_lead is not None and not panic_bypass:
|
||||
if optional_far_lead_comfort and comfort_lead is not None and not panic_bypass:
|
||||
far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow)
|
||||
if far_lead_brake_cap is not None:
|
||||
self.a_desired = max(self.a_desired, far_lead_brake_cap)
|
||||
|
||||
@@ -27,6 +27,8 @@ V_EGO_STATIONARY = 4. # no stationary object flag below this speed
|
||||
|
||||
RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car
|
||||
RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame
|
||||
G90_RADAR_LOW_SPEED_MAX_DIST = 12.0
|
||||
G90_RADAR_LOW_SPEED_MAX_Y = 0.6
|
||||
|
||||
|
||||
class KalmanParams:
|
||||
@@ -127,7 +129,19 @@ def laplacian_pdf(x: float, mu: float, b: float):
|
||||
return math.exp(-abs(x-mu)/b)
|
||||
|
||||
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], starpilot_toggles: SimpleNamespace):
|
||||
def g90_radar_lead_lateral_sane(track: Track) -> bool:
|
||||
max_y = min(6.0, max(2.5, 1.5 + 0.08 * max(track.dRel, 0.0)))
|
||||
return abs(track.yRel) <= max_y
|
||||
|
||||
|
||||
def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool:
|
||||
return (track.cnt >= 3 and v_ego < 3.0 and
|
||||
0.75 < track.dRel < G90_RADAR_LOW_SPEED_MAX_DIST and
|
||||
abs(track.yRel) < G90_RADAR_LOW_SPEED_MAX_Y)
|
||||
|
||||
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track],
|
||||
starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False):
|
||||
if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(starpilot_toggles, "human_lane_changes", False):
|
||||
direction = model_data.meta.laneChangeDirection
|
||||
if direction == LaneChangeDirection.left:
|
||||
@@ -135,6 +149,9 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_
|
||||
elif direction == LaneChangeDirection.right:
|
||||
tracks = {k: v for k, v in tracks.items() if v.yRel < 0}
|
||||
|
||||
if g90_radar_filter:
|
||||
tracks = {k: v for k, v in tracks.items() if g90_radar_lead_lateral_sane(v)}
|
||||
|
||||
if not tracks:
|
||||
return None
|
||||
|
||||
@@ -180,12 +197,12 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
|
||||
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
|
||||
model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool,
|
||||
starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace,
|
||||
low_speed_override: bool = True) -> dict[str, Any]:
|
||||
low_speed_override: bool = True, g90_radar_filter: bool = False) -> dict[str, Any]:
|
||||
lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35))
|
||||
|
||||
# Determine leads, this is where the essential logic happens
|
||||
if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability:
|
||||
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles)
|
||||
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter)
|
||||
else:
|
||||
track = None
|
||||
|
||||
@@ -196,7 +213,10 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
|
||||
lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
|
||||
|
||||
if low_speed_override:
|
||||
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
|
||||
if g90_radar_filter:
|
||||
low_speed_tracks = [c for c in tracks.values() if g90_low_speed_radar_lead_sane(c, v_ego)]
|
||||
else:
|
||||
low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
|
||||
if len(low_speed_tracks) > 0:
|
||||
closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
|
||||
|
||||
@@ -225,11 +245,12 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
|
||||
|
||||
|
||||
class RadarD:
|
||||
def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0):
|
||||
def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False):
|
||||
self.current_time = 0.0
|
||||
|
||||
self.tracks: dict[int, Track] = {}
|
||||
self.kalman_params = KalmanParams(radar_ts)
|
||||
self.g90_radar_filter = g90_radar_filter
|
||||
|
||||
self.v_ego = 0.0
|
||||
self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1)
|
||||
@@ -286,9 +307,11 @@ class RadarD:
|
||||
leads_v3 = sm['modelV2'].leadsV3
|
||||
if len(leads_v3) > 1:
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'],
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True)
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True,
|
||||
g90_radar_filter=self.g90_radar_filter)
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'],
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False)
|
||||
sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False,
|
||||
g90_radar_filter=self.g90_radar_filter)
|
||||
|
||||
if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes):
|
||||
self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
|
||||
@@ -328,7 +351,8 @@ def main() -> None:
|
||||
if not 0.01 < radar_ts < 0.2:
|
||||
radar_ts = DT_MDL
|
||||
|
||||
RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay)
|
||||
g90_radar_filter = CP.brand == "hyundai" and CP.carFingerprint == "GENESIS_G90"
|
||||
RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay, g90_radar_filter=g90_radar_filter)
|
||||
|
||||
sm = sm.extend(['starpilotPlan'])
|
||||
pm = pm.extend(['starpilotRadarState'])
|
||||
|
||||
@@ -40,6 +40,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
|
||||
get_genesis_g90_friction_scale,
|
||||
get_genesis_g90_friction_threshold,
|
||||
get_elantra_non_scc_ff_scale,
|
||||
get_palisade_ff_scale,
|
||||
get_palisade_friction_scale,
|
||||
get_palisade_friction_threshold,
|
||||
get_ioniq_5_ff_scale,
|
||||
get_ioniq_5_friction_scale,
|
||||
get_ioniq_5_friction_threshold,
|
||||
@@ -305,6 +308,39 @@ class TestLatControl:
|
||||
assert left_turn_in > right_turn_in > base
|
||||
assert base > left_unwind > right_unwind
|
||||
|
||||
def test_palisade_ff_scale_curve(self):
|
||||
assert get_palisade_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
steady_left = get_palisade_ff_scale(0.6, 0.0, 8.0)
|
||||
steady_right = get_palisade_ff_scale(-0.6, 0.0, 8.0)
|
||||
turn_in_left = get_palisade_ff_scale(0.6, 0.8, 8.0)
|
||||
turn_in_right = get_palisade_ff_scale(-0.6, -0.8, 8.0)
|
||||
unwind_left = get_palisade_ff_scale(0.6, -0.8, 8.0)
|
||||
unwind_right = get_palisade_ff_scale(-0.6, 0.8, 8.0)
|
||||
assert steady_left > 1.0
|
||||
assert steady_right > 1.0
|
||||
assert turn_in_left > steady_left
|
||||
assert turn_in_right > steady_right
|
||||
assert unwind_left < steady_left
|
||||
assert unwind_right < steady_right
|
||||
assert unwind_right < unwind_left
|
||||
|
||||
def test_palisade_friction_threshold_curve(self):
|
||||
base = get_friction_threshold(6.0)
|
||||
left_turn_in = get_palisade_friction_threshold(6.0, 0.7, 0.8)
|
||||
right_turn_in = get_palisade_friction_threshold(6.0, -0.7, -0.8)
|
||||
left_unwind = get_palisade_friction_threshold(6.0, 0.7, -0.8)
|
||||
right_unwind = get_palisade_friction_threshold(6.0, -0.7, 0.8)
|
||||
assert left_turn_in < right_turn_in < base < left_unwind < right_unwind
|
||||
|
||||
def test_palisade_friction_scale_curve(self):
|
||||
base = get_palisade_friction_scale(25.0, 0.7, 0.8)
|
||||
left_turn_in = get_palisade_friction_scale(6.0, 0.7, 0.8)
|
||||
right_turn_in = get_palisade_friction_scale(6.0, -0.7, -0.8)
|
||||
left_unwind = get_palisade_friction_scale(6.0, 0.7, -0.8)
|
||||
right_unwind = get_palisade_friction_scale(6.0, -0.7, 0.8)
|
||||
assert left_turn_in > right_turn_in > base
|
||||
assert base > left_unwind > right_unwind
|
||||
|
||||
def test_ioniq_5_ff_scale_curve(self):
|
||||
assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0
|
||||
steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0)
|
||||
@@ -491,6 +527,16 @@ class TestLatControl:
|
||||
|
||||
assert lac_log.active
|
||||
|
||||
def test_palisade_default_update_path(self):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_PALISADE_2023)
|
||||
CarInterface = interfaces[HYUNDAI.HYUNDAI_PALISADE_2023]
|
||||
CP = CarInterface.get_non_essential_params(HYUNDAI.HYUNDAI_PALISADE_2023)
|
||||
|
||||
_, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles)
|
||||
|
||||
assert lac_log.active
|
||||
assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 0.98)
|
||||
|
||||
def test_ioniq_5_default_update_path(self):
|
||||
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5)
|
||||
CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5]
|
||||
|
||||
@@ -1,10 +1,23 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
import cereal.messaging as messaging
|
||||
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.selfdrive.test.process_replay import replay_process_with_name
|
||||
from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane
|
||||
|
||||
|
||||
class TestLeads:
|
||||
def test_g90_radar_filters_side_tracks(self):
|
||||
side_track = SimpleNamespace(dRel=13.0, yRel=-10.38, cnt=10)
|
||||
centered_track = SimpleNamespace(dRel=10.8, yRel=-0.21, cnt=5)
|
||||
far_low_speed_track = SimpleNamespace(dRel=15.5, yRel=0.58, cnt=5)
|
||||
|
||||
assert not g90_radar_lead_lateral_sane(side_track)
|
||||
assert g90_radar_lead_lateral_sane(centered_track)
|
||||
assert g90_low_speed_radar_lead_sane(centered_track, 2.0)
|
||||
assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5)
|
||||
|
||||
def test_radar_fault(self):
|
||||
# if there's no radar-related can traffic, radard should either not respond or respond with an error
|
||||
# this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
|
||||
|
||||
@@ -1702,6 +1702,19 @@ def test_follow_control_lead_prefers_active_lead1_for_matched_follow():
|
||||
assert follow_lead is planner.lead_two
|
||||
|
||||
|
||||
def test_follow_control_lead_disables_optional_matched_follow_override():
|
||||
v_ego = 23.3
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6)
|
||||
planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98)
|
||||
planner.mpc.source = "lead1"
|
||||
|
||||
follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45, allow_optional_far_lead_logic=False)
|
||||
|
||||
assert follow_lead is planner.lead_one
|
||||
|
||||
|
||||
def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch():
|
||||
v_ego = 27.5
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
@@ -1713,6 +1726,17 @@ def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch():
|
||||
assert follow_lead is planner.lead_one
|
||||
|
||||
|
||||
def test_follow_control_lead_requires_real_lead_control_when_optional_logic_disabled():
|
||||
v_ego = 27.5
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
planner = LongitudinalPlanner(CP, init_v=v_ego)
|
||||
planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99)
|
||||
|
||||
follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45, allow_optional_far_lead_logic=False)
|
||||
|
||||
assert follow_lead is None
|
||||
|
||||
|
||||
def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead():
|
||||
v_ego = 32.37
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
|
||||
@@ -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_4387195160971747702) {
|
||||
out_4387195160971747702[0] = delta_x[0] + nom_x[0];
|
||||
out_4387195160971747702[1] = delta_x[1] + nom_x[1];
|
||||
out_4387195160971747702[2] = delta_x[2] + nom_x[2];
|
||||
out_4387195160971747702[3] = delta_x[3] + nom_x[3];
|
||||
out_4387195160971747702[4] = delta_x[4] + nom_x[4];
|
||||
out_4387195160971747702[5] = delta_x[5] + nom_x[5];
|
||||
out_4387195160971747702[6] = delta_x[6] + nom_x[6];
|
||||
out_4387195160971747702[7] = delta_x[7] + nom_x[7];
|
||||
out_4387195160971747702[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402) {
|
||||
out_7214711627163091402[0] = delta_x[0] + nom_x[0];
|
||||
out_7214711627163091402[1] = delta_x[1] + nom_x[1];
|
||||
out_7214711627163091402[2] = delta_x[2] + nom_x[2];
|
||||
out_7214711627163091402[3] = delta_x[3] + nom_x[3];
|
||||
out_7214711627163091402[4] = delta_x[4] + nom_x[4];
|
||||
out_7214711627163091402[5] = delta_x[5] + nom_x[5];
|
||||
out_7214711627163091402[6] = delta_x[6] + nom_x[6];
|
||||
out_7214711627163091402[7] = delta_x[7] + nom_x[7];
|
||||
out_7214711627163091402[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) {
|
||||
out_7707264064834051115[0] = -nom_x[0] + true_x[0];
|
||||
out_7707264064834051115[1] = -nom_x[1] + true_x[1];
|
||||
out_7707264064834051115[2] = -nom_x[2] + true_x[2];
|
||||
out_7707264064834051115[3] = -nom_x[3] + true_x[3];
|
||||
out_7707264064834051115[4] = -nom_x[4] + true_x[4];
|
||||
out_7707264064834051115[5] = -nom_x[5] + true_x[5];
|
||||
out_7707264064834051115[6] = -nom_x[6] + true_x[6];
|
||||
out_7707264064834051115[7] = -nom_x[7] + true_x[7];
|
||||
out_7707264064834051115[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438) {
|
||||
out_911828672798028438[0] = -nom_x[0] + true_x[0];
|
||||
out_911828672798028438[1] = -nom_x[1] + true_x[1];
|
||||
out_911828672798028438[2] = -nom_x[2] + true_x[2];
|
||||
out_911828672798028438[3] = -nom_x[3] + true_x[3];
|
||||
out_911828672798028438[4] = -nom_x[4] + true_x[4];
|
||||
out_911828672798028438[5] = -nom_x[5] + true_x[5];
|
||||
out_911828672798028438[6] = -nom_x[6] + true_x[6];
|
||||
out_911828672798028438[7] = -nom_x[7] + true_x[7];
|
||||
out_911828672798028438[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_7178605602671138983) {
|
||||
out_7178605602671138983[0] = 1.0;
|
||||
out_7178605602671138983[1] = 0.0;
|
||||
out_7178605602671138983[2] = 0.0;
|
||||
out_7178605602671138983[3] = 0.0;
|
||||
out_7178605602671138983[4] = 0.0;
|
||||
out_7178605602671138983[5] = 0.0;
|
||||
out_7178605602671138983[6] = 0.0;
|
||||
out_7178605602671138983[7] = 0.0;
|
||||
out_7178605602671138983[8] = 0.0;
|
||||
out_7178605602671138983[9] = 0.0;
|
||||
out_7178605602671138983[10] = 1.0;
|
||||
out_7178605602671138983[11] = 0.0;
|
||||
out_7178605602671138983[12] = 0.0;
|
||||
out_7178605602671138983[13] = 0.0;
|
||||
out_7178605602671138983[14] = 0.0;
|
||||
out_7178605602671138983[15] = 0.0;
|
||||
out_7178605602671138983[16] = 0.0;
|
||||
out_7178605602671138983[17] = 0.0;
|
||||
out_7178605602671138983[18] = 0.0;
|
||||
out_7178605602671138983[19] = 0.0;
|
||||
out_7178605602671138983[20] = 1.0;
|
||||
out_7178605602671138983[21] = 0.0;
|
||||
out_7178605602671138983[22] = 0.0;
|
||||
out_7178605602671138983[23] = 0.0;
|
||||
out_7178605602671138983[24] = 0.0;
|
||||
out_7178605602671138983[25] = 0.0;
|
||||
out_7178605602671138983[26] = 0.0;
|
||||
out_7178605602671138983[27] = 0.0;
|
||||
out_7178605602671138983[28] = 0.0;
|
||||
out_7178605602671138983[29] = 0.0;
|
||||
out_7178605602671138983[30] = 1.0;
|
||||
out_7178605602671138983[31] = 0.0;
|
||||
out_7178605602671138983[32] = 0.0;
|
||||
out_7178605602671138983[33] = 0.0;
|
||||
out_7178605602671138983[34] = 0.0;
|
||||
out_7178605602671138983[35] = 0.0;
|
||||
out_7178605602671138983[36] = 0.0;
|
||||
out_7178605602671138983[37] = 0.0;
|
||||
out_7178605602671138983[38] = 0.0;
|
||||
out_7178605602671138983[39] = 0.0;
|
||||
out_7178605602671138983[40] = 1.0;
|
||||
out_7178605602671138983[41] = 0.0;
|
||||
out_7178605602671138983[42] = 0.0;
|
||||
out_7178605602671138983[43] = 0.0;
|
||||
out_7178605602671138983[44] = 0.0;
|
||||
out_7178605602671138983[45] = 0.0;
|
||||
out_7178605602671138983[46] = 0.0;
|
||||
out_7178605602671138983[47] = 0.0;
|
||||
out_7178605602671138983[48] = 0.0;
|
||||
out_7178605602671138983[49] = 0.0;
|
||||
out_7178605602671138983[50] = 1.0;
|
||||
out_7178605602671138983[51] = 0.0;
|
||||
out_7178605602671138983[52] = 0.0;
|
||||
out_7178605602671138983[53] = 0.0;
|
||||
out_7178605602671138983[54] = 0.0;
|
||||
out_7178605602671138983[55] = 0.0;
|
||||
out_7178605602671138983[56] = 0.0;
|
||||
out_7178605602671138983[57] = 0.0;
|
||||
out_7178605602671138983[58] = 0.0;
|
||||
out_7178605602671138983[59] = 0.0;
|
||||
out_7178605602671138983[60] = 1.0;
|
||||
out_7178605602671138983[61] = 0.0;
|
||||
out_7178605602671138983[62] = 0.0;
|
||||
out_7178605602671138983[63] = 0.0;
|
||||
out_7178605602671138983[64] = 0.0;
|
||||
out_7178605602671138983[65] = 0.0;
|
||||
out_7178605602671138983[66] = 0.0;
|
||||
out_7178605602671138983[67] = 0.0;
|
||||
out_7178605602671138983[68] = 0.0;
|
||||
out_7178605602671138983[69] = 0.0;
|
||||
out_7178605602671138983[70] = 1.0;
|
||||
out_7178605602671138983[71] = 0.0;
|
||||
out_7178605602671138983[72] = 0.0;
|
||||
out_7178605602671138983[73] = 0.0;
|
||||
out_7178605602671138983[74] = 0.0;
|
||||
out_7178605602671138983[75] = 0.0;
|
||||
out_7178605602671138983[76] = 0.0;
|
||||
out_7178605602671138983[77] = 0.0;
|
||||
out_7178605602671138983[78] = 0.0;
|
||||
out_7178605602671138983[79] = 0.0;
|
||||
out_7178605602671138983[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_149552472482575511) {
|
||||
out_149552472482575511[0] = 1.0;
|
||||
out_149552472482575511[1] = 0.0;
|
||||
out_149552472482575511[2] = 0.0;
|
||||
out_149552472482575511[3] = 0.0;
|
||||
out_149552472482575511[4] = 0.0;
|
||||
out_149552472482575511[5] = 0.0;
|
||||
out_149552472482575511[6] = 0.0;
|
||||
out_149552472482575511[7] = 0.0;
|
||||
out_149552472482575511[8] = 0.0;
|
||||
out_149552472482575511[9] = 0.0;
|
||||
out_149552472482575511[10] = 1.0;
|
||||
out_149552472482575511[11] = 0.0;
|
||||
out_149552472482575511[12] = 0.0;
|
||||
out_149552472482575511[13] = 0.0;
|
||||
out_149552472482575511[14] = 0.0;
|
||||
out_149552472482575511[15] = 0.0;
|
||||
out_149552472482575511[16] = 0.0;
|
||||
out_149552472482575511[17] = 0.0;
|
||||
out_149552472482575511[18] = 0.0;
|
||||
out_149552472482575511[19] = 0.0;
|
||||
out_149552472482575511[20] = 1.0;
|
||||
out_149552472482575511[21] = 0.0;
|
||||
out_149552472482575511[22] = 0.0;
|
||||
out_149552472482575511[23] = 0.0;
|
||||
out_149552472482575511[24] = 0.0;
|
||||
out_149552472482575511[25] = 0.0;
|
||||
out_149552472482575511[26] = 0.0;
|
||||
out_149552472482575511[27] = 0.0;
|
||||
out_149552472482575511[28] = 0.0;
|
||||
out_149552472482575511[29] = 0.0;
|
||||
out_149552472482575511[30] = 1.0;
|
||||
out_149552472482575511[31] = 0.0;
|
||||
out_149552472482575511[32] = 0.0;
|
||||
out_149552472482575511[33] = 0.0;
|
||||
out_149552472482575511[34] = 0.0;
|
||||
out_149552472482575511[35] = 0.0;
|
||||
out_149552472482575511[36] = 0.0;
|
||||
out_149552472482575511[37] = 0.0;
|
||||
out_149552472482575511[38] = 0.0;
|
||||
out_149552472482575511[39] = 0.0;
|
||||
out_149552472482575511[40] = 1.0;
|
||||
out_149552472482575511[41] = 0.0;
|
||||
out_149552472482575511[42] = 0.0;
|
||||
out_149552472482575511[43] = 0.0;
|
||||
out_149552472482575511[44] = 0.0;
|
||||
out_149552472482575511[45] = 0.0;
|
||||
out_149552472482575511[46] = 0.0;
|
||||
out_149552472482575511[47] = 0.0;
|
||||
out_149552472482575511[48] = 0.0;
|
||||
out_149552472482575511[49] = 0.0;
|
||||
out_149552472482575511[50] = 1.0;
|
||||
out_149552472482575511[51] = 0.0;
|
||||
out_149552472482575511[52] = 0.0;
|
||||
out_149552472482575511[53] = 0.0;
|
||||
out_149552472482575511[54] = 0.0;
|
||||
out_149552472482575511[55] = 0.0;
|
||||
out_149552472482575511[56] = 0.0;
|
||||
out_149552472482575511[57] = 0.0;
|
||||
out_149552472482575511[58] = 0.0;
|
||||
out_149552472482575511[59] = 0.0;
|
||||
out_149552472482575511[60] = 1.0;
|
||||
out_149552472482575511[61] = 0.0;
|
||||
out_149552472482575511[62] = 0.0;
|
||||
out_149552472482575511[63] = 0.0;
|
||||
out_149552472482575511[64] = 0.0;
|
||||
out_149552472482575511[65] = 0.0;
|
||||
out_149552472482575511[66] = 0.0;
|
||||
out_149552472482575511[67] = 0.0;
|
||||
out_149552472482575511[68] = 0.0;
|
||||
out_149552472482575511[69] = 0.0;
|
||||
out_149552472482575511[70] = 1.0;
|
||||
out_149552472482575511[71] = 0.0;
|
||||
out_149552472482575511[72] = 0.0;
|
||||
out_149552472482575511[73] = 0.0;
|
||||
out_149552472482575511[74] = 0.0;
|
||||
out_149552472482575511[75] = 0.0;
|
||||
out_149552472482575511[76] = 0.0;
|
||||
out_149552472482575511[77] = 0.0;
|
||||
out_149552472482575511[78] = 0.0;
|
||||
out_149552472482575511[79] = 0.0;
|
||||
out_149552472482575511[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_2416599425795193412) {
|
||||
out_2416599425795193412[0] = state[0];
|
||||
out_2416599425795193412[1] = state[1];
|
||||
out_2416599425795193412[2] = state[2];
|
||||
out_2416599425795193412[3] = state[3];
|
||||
out_2416599425795193412[4] = state[4];
|
||||
out_2416599425795193412[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_2416599425795193412[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_2416599425795193412[7] = state[7];
|
||||
out_2416599425795193412[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_4547528931834134293) {
|
||||
out_4547528931834134293[0] = state[0];
|
||||
out_4547528931834134293[1] = state[1];
|
||||
out_4547528931834134293[2] = state[2];
|
||||
out_4547528931834134293[3] = state[3];
|
||||
out_4547528931834134293[4] = state[4];
|
||||
out_4547528931834134293[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_4547528931834134293[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_4547528931834134293[7] = state[7];
|
||||
out_4547528931834134293[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_6761986232654706930) {
|
||||
out_6761986232654706930[0] = 1;
|
||||
out_6761986232654706930[1] = 0;
|
||||
out_6761986232654706930[2] = 0;
|
||||
out_6761986232654706930[3] = 0;
|
||||
out_6761986232654706930[4] = 0;
|
||||
out_6761986232654706930[5] = 0;
|
||||
out_6761986232654706930[6] = 0;
|
||||
out_6761986232654706930[7] = 0;
|
||||
out_6761986232654706930[8] = 0;
|
||||
out_6761986232654706930[9] = 0;
|
||||
out_6761986232654706930[10] = 1;
|
||||
out_6761986232654706930[11] = 0;
|
||||
out_6761986232654706930[12] = 0;
|
||||
out_6761986232654706930[13] = 0;
|
||||
out_6761986232654706930[14] = 0;
|
||||
out_6761986232654706930[15] = 0;
|
||||
out_6761986232654706930[16] = 0;
|
||||
out_6761986232654706930[17] = 0;
|
||||
out_6761986232654706930[18] = 0;
|
||||
out_6761986232654706930[19] = 0;
|
||||
out_6761986232654706930[20] = 1;
|
||||
out_6761986232654706930[21] = 0;
|
||||
out_6761986232654706930[22] = 0;
|
||||
out_6761986232654706930[23] = 0;
|
||||
out_6761986232654706930[24] = 0;
|
||||
out_6761986232654706930[25] = 0;
|
||||
out_6761986232654706930[26] = 0;
|
||||
out_6761986232654706930[27] = 0;
|
||||
out_6761986232654706930[28] = 0;
|
||||
out_6761986232654706930[29] = 0;
|
||||
out_6761986232654706930[30] = 1;
|
||||
out_6761986232654706930[31] = 0;
|
||||
out_6761986232654706930[32] = 0;
|
||||
out_6761986232654706930[33] = 0;
|
||||
out_6761986232654706930[34] = 0;
|
||||
out_6761986232654706930[35] = 0;
|
||||
out_6761986232654706930[36] = 0;
|
||||
out_6761986232654706930[37] = 0;
|
||||
out_6761986232654706930[38] = 0;
|
||||
out_6761986232654706930[39] = 0;
|
||||
out_6761986232654706930[40] = 1;
|
||||
out_6761986232654706930[41] = 0;
|
||||
out_6761986232654706930[42] = 0;
|
||||
out_6761986232654706930[43] = 0;
|
||||
out_6761986232654706930[44] = 0;
|
||||
out_6761986232654706930[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_6761986232654706930[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_6761986232654706930[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6761986232654706930[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6761986232654706930[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_6761986232654706930[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_6761986232654706930[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_6761986232654706930[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6761986232654706930[53] = -9.8100000000000005*dt;
|
||||
out_6761986232654706930[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_6761986232654706930[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_6761986232654706930[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6761986232654706930[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6761986232654706930[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_6761986232654706930[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_6761986232654706930[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_6761986232654706930[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6761986232654706930[62] = 0;
|
||||
out_6761986232654706930[63] = 0;
|
||||
out_6761986232654706930[64] = 0;
|
||||
out_6761986232654706930[65] = 0;
|
||||
out_6761986232654706930[66] = 0;
|
||||
out_6761986232654706930[67] = 0;
|
||||
out_6761986232654706930[68] = 0;
|
||||
out_6761986232654706930[69] = 0;
|
||||
out_6761986232654706930[70] = 1;
|
||||
out_6761986232654706930[71] = 0;
|
||||
out_6761986232654706930[72] = 0;
|
||||
out_6761986232654706930[73] = 0;
|
||||
out_6761986232654706930[74] = 0;
|
||||
out_6761986232654706930[75] = 0;
|
||||
out_6761986232654706930[76] = 0;
|
||||
out_6761986232654706930[77] = 0;
|
||||
out_6761986232654706930[78] = 0;
|
||||
out_6761986232654706930[79] = 0;
|
||||
out_6761986232654706930[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_3560058468708809121) {
|
||||
out_3560058468708809121[0] = 1;
|
||||
out_3560058468708809121[1] = 0;
|
||||
out_3560058468708809121[2] = 0;
|
||||
out_3560058468708809121[3] = 0;
|
||||
out_3560058468708809121[4] = 0;
|
||||
out_3560058468708809121[5] = 0;
|
||||
out_3560058468708809121[6] = 0;
|
||||
out_3560058468708809121[7] = 0;
|
||||
out_3560058468708809121[8] = 0;
|
||||
out_3560058468708809121[9] = 0;
|
||||
out_3560058468708809121[10] = 1;
|
||||
out_3560058468708809121[11] = 0;
|
||||
out_3560058468708809121[12] = 0;
|
||||
out_3560058468708809121[13] = 0;
|
||||
out_3560058468708809121[14] = 0;
|
||||
out_3560058468708809121[15] = 0;
|
||||
out_3560058468708809121[16] = 0;
|
||||
out_3560058468708809121[17] = 0;
|
||||
out_3560058468708809121[18] = 0;
|
||||
out_3560058468708809121[19] = 0;
|
||||
out_3560058468708809121[20] = 1;
|
||||
out_3560058468708809121[21] = 0;
|
||||
out_3560058468708809121[22] = 0;
|
||||
out_3560058468708809121[23] = 0;
|
||||
out_3560058468708809121[24] = 0;
|
||||
out_3560058468708809121[25] = 0;
|
||||
out_3560058468708809121[26] = 0;
|
||||
out_3560058468708809121[27] = 0;
|
||||
out_3560058468708809121[28] = 0;
|
||||
out_3560058468708809121[29] = 0;
|
||||
out_3560058468708809121[30] = 1;
|
||||
out_3560058468708809121[31] = 0;
|
||||
out_3560058468708809121[32] = 0;
|
||||
out_3560058468708809121[33] = 0;
|
||||
out_3560058468708809121[34] = 0;
|
||||
out_3560058468708809121[35] = 0;
|
||||
out_3560058468708809121[36] = 0;
|
||||
out_3560058468708809121[37] = 0;
|
||||
out_3560058468708809121[38] = 0;
|
||||
out_3560058468708809121[39] = 0;
|
||||
out_3560058468708809121[40] = 1;
|
||||
out_3560058468708809121[41] = 0;
|
||||
out_3560058468708809121[42] = 0;
|
||||
out_3560058468708809121[43] = 0;
|
||||
out_3560058468708809121[44] = 0;
|
||||
out_3560058468708809121[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_3560058468708809121[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3560058468708809121[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3560058468708809121[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3560058468708809121[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_3560058468708809121[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3560058468708809121[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3560058468708809121[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3560058468708809121[53] = -9.8100000000000005*dt;
|
||||
out_3560058468708809121[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_3560058468708809121[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3560058468708809121[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3560058468708809121[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3560058468708809121[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_3560058468708809121[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3560058468708809121[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_3560058468708809121[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3560058468708809121[62] = 0;
|
||||
out_3560058468708809121[63] = 0;
|
||||
out_3560058468708809121[64] = 0;
|
||||
out_3560058468708809121[65] = 0;
|
||||
out_3560058468708809121[66] = 0;
|
||||
out_3560058468708809121[67] = 0;
|
||||
out_3560058468708809121[68] = 0;
|
||||
out_3560058468708809121[69] = 0;
|
||||
out_3560058468708809121[70] = 1;
|
||||
out_3560058468708809121[71] = 0;
|
||||
out_3560058468708809121[72] = 0;
|
||||
out_3560058468708809121[73] = 0;
|
||||
out_3560058468708809121[74] = 0;
|
||||
out_3560058468708809121[75] = 0;
|
||||
out_3560058468708809121[76] = 0;
|
||||
out_3560058468708809121[77] = 0;
|
||||
out_3560058468708809121[78] = 0;
|
||||
out_3560058468708809121[79] = 0;
|
||||
out_3560058468708809121[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_319074315832850169) {
|
||||
out_319074315832850169[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_1132582605868503102) {
|
||||
out_1132582605868503102[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_3655624952681647306) {
|
||||
out_3655624952681647306[0] = 0;
|
||||
out_3655624952681647306[1] = 0;
|
||||
out_3655624952681647306[2] = 0;
|
||||
out_3655624952681647306[3] = 0;
|
||||
out_3655624952681647306[4] = 0;
|
||||
out_3655624952681647306[5] = 0;
|
||||
out_3655624952681647306[6] = 1;
|
||||
out_3655624952681647306[7] = 0;
|
||||
out_3655624952681647306[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_9104003375314738868) {
|
||||
out_9104003375314738868[0] = 0;
|
||||
out_9104003375314738868[1] = 0;
|
||||
out_9104003375314738868[2] = 0;
|
||||
out_9104003375314738868[3] = 0;
|
||||
out_9104003375314738868[4] = 0;
|
||||
out_9104003375314738868[5] = 0;
|
||||
out_9104003375314738868[6] = 1;
|
||||
out_9104003375314738868[7] = 0;
|
||||
out_9104003375314738868[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5734853090591072423) {
|
||||
out_5734853090591072423[0] = state[4];
|
||||
out_5734853090591072423[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8381470974523360630) {
|
||||
out_8381470974523360630[0] = state[4];
|
||||
out_8381470974523360630[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_1429917168702778744) {
|
||||
out_1429917168702778744[0] = 0;
|
||||
out_1429917168702778744[1] = 0;
|
||||
out_1429917168702778744[2] = 0;
|
||||
out_1429917168702778744[3] = 0;
|
||||
out_1429917168702778744[4] = 1;
|
||||
out_1429917168702778744[5] = 0;
|
||||
out_1429917168702778744[6] = 0;
|
||||
out_1429917168702778744[7] = 0;
|
||||
out_1429917168702778744[8] = 0;
|
||||
out_1429917168702778744[9] = 0;
|
||||
out_1429917168702778744[10] = 0;
|
||||
out_1429917168702778744[11] = 0;
|
||||
out_1429917168702778744[12] = 0;
|
||||
out_1429917168702778744[13] = 0;
|
||||
out_1429917168702778744[14] = 1;
|
||||
out_1429917168702778744[15] = 0;
|
||||
out_1429917168702778744[16] = 0;
|
||||
out_1429917168702778744[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_4357494048348229834) {
|
||||
out_4357494048348229834[0] = 0;
|
||||
out_4357494048348229834[1] = 0;
|
||||
out_4357494048348229834[2] = 0;
|
||||
out_4357494048348229834[3] = 0;
|
||||
out_4357494048348229834[4] = 1;
|
||||
out_4357494048348229834[5] = 0;
|
||||
out_4357494048348229834[6] = 0;
|
||||
out_4357494048348229834[7] = 0;
|
||||
out_4357494048348229834[8] = 0;
|
||||
out_4357494048348229834[9] = 0;
|
||||
out_4357494048348229834[10] = 0;
|
||||
out_4357494048348229834[11] = 0;
|
||||
out_4357494048348229834[12] = 0;
|
||||
out_4357494048348229834[13] = 0;
|
||||
out_4357494048348229834[14] = 1;
|
||||
out_4357494048348229834[15] = 0;
|
||||
out_4357494048348229834[16] = 0;
|
||||
out_4357494048348229834[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_8212013810717373528) {
|
||||
out_8212013810717373528[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_975395937517373957) {
|
||||
out_975395937517373957[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_3784963899824887376) {
|
||||
out_3784963899824887376[0] = 0;
|
||||
out_3784963899824887376[1] = 0;
|
||||
out_3784963899824887376[2] = 0;
|
||||
out_3784963899824887376[3] = 0;
|
||||
out_3784963899824887376[4] = 1;
|
||||
out_3784963899824887376[5] = 0;
|
||||
out_3784963899824887376[6] = 0;
|
||||
out_3784963899824887376[7] = 0;
|
||||
out_3784963899824887376[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_2426050356903195993) {
|
||||
out_2426050356903195993[0] = 0;
|
||||
out_2426050356903195993[1] = 0;
|
||||
out_2426050356903195993[2] = 0;
|
||||
out_2426050356903195993[3] = 0;
|
||||
out_2426050356903195993[4] = 1;
|
||||
out_2426050356903195993[5] = 0;
|
||||
out_2426050356903195993[6] = 0;
|
||||
out_2426050356903195993[7] = 0;
|
||||
out_2426050356903195993[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_5233056995331332443) {
|
||||
out_5233056995331332443[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4026277330143471688) {
|
||||
out_4026277330143471688[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_2998770888571335402) {
|
||||
out_2998770888571335402[0] = 0;
|
||||
out_2998770888571335402[1] = 0;
|
||||
out_2998770888571335402[2] = 0;
|
||||
out_2998770888571335402[3] = 0;
|
||||
out_2998770888571335402[4] = 0;
|
||||
out_2998770888571335402[5] = 0;
|
||||
out_2998770888571335402[6] = 0;
|
||||
out_2998770888571335402[7] = 1;
|
||||
out_2998770888571335402[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_5362500056440682644) {
|
||||
out_5362500056440682644[0] = 0;
|
||||
out_5362500056440682644[1] = 0;
|
||||
out_5362500056440682644[2] = 0;
|
||||
out_5362500056440682644[3] = 0;
|
||||
out_5362500056440682644[4] = 0;
|
||||
out_5362500056440682644[5] = 0;
|
||||
out_5362500056440682644[6] = 0;
|
||||
out_5362500056440682644[7] = 1;
|
||||
out_5362500056440682644[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_3355860367277182854) {
|
||||
out_3355860367277182854[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_990245959502812892) {
|
||||
out_990245959502812892[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_5959727211625312287) {
|
||||
out_5959727211625312287[0] = 0;
|
||||
out_5959727211625312287[1] = 0;
|
||||
out_5959727211625312287[2] = 0;
|
||||
out_5959727211625312287[3] = 1;
|
||||
out_5959727211625312287[4] = 0;
|
||||
out_5959727211625312287[5] = 0;
|
||||
out_5959727211625312287[6] = 0;
|
||||
out_5959727211625312287[7] = 0;
|
||||
out_5959727211625312287[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_4600813668703620904) {
|
||||
out_4600813668703620904[0] = 0;
|
||||
out_4600813668703620904[1] = 0;
|
||||
out_4600813668703620904[2] = 0;
|
||||
out_4600813668703620904[3] = 1;
|
||||
out_4600813668703620904[4] = 0;
|
||||
out_4600813668703620904[5] = 0;
|
||||
out_4600813668703620904[6] = 0;
|
||||
out_4600813668703620904[7] = 0;
|
||||
out_4600813668703620904[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_7630324942183480437) {
|
||||
out_7630324942183480437[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8450804723751701217) {
|
||||
out_8450804723751701217[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_3274732555510495192) {
|
||||
out_3274732555510495192[0] = 0;
|
||||
out_3274732555510495192[1] = 1;
|
||||
out_3274732555510495192[2] = 0;
|
||||
out_3274732555510495192[3] = 0;
|
||||
out_3274732555510495192[4] = 0;
|
||||
out_3274732555510495192[5] = 0;
|
||||
out_3274732555510495192[6] = 0;
|
||||
out_3274732555510495192[7] = 0;
|
||||
out_3274732555510495192[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_1915819012588803809) {
|
||||
out_1915819012588803809[0] = 0;
|
||||
out_1915819012588803809[1] = 1;
|
||||
out_1915819012588803809[2] = 0;
|
||||
out_1915819012588803809[3] = 0;
|
||||
out_1915819012588803809[4] = 0;
|
||||
out_1915819012588803809[5] = 0;
|
||||
out_1915819012588803809[6] = 0;
|
||||
out_1915819012588803809[7] = 0;
|
||||
out_1915819012588803809[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4299370220178845289) {
|
||||
out_4299370220178845289[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_2847713298477492018) {
|
||||
out_2847713298477492018[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_1311102283945168941) {
|
||||
out_1311102283945168941[0] = 1;
|
||||
out_1311102283945168941[1] = 0;
|
||||
out_1311102283945168941[2] = 0;
|
||||
out_1311102283945168941[3] = 0;
|
||||
out_1311102283945168941[4] = 0;
|
||||
out_1311102283945168941[5] = 0;
|
||||
out_1311102283945168941[6] = 0;
|
||||
out_1311102283945168941[7] = 0;
|
||||
out_1311102283945168941[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_7050168661066849105) {
|
||||
out_7050168661066849105[0] = 1;
|
||||
out_7050168661066849105[1] = 0;
|
||||
out_7050168661066849105[2] = 0;
|
||||
out_7050168661066849105[3] = 0;
|
||||
out_7050168661066849105[4] = 0;
|
||||
out_7050168661066849105[5] = 0;
|
||||
out_7050168661066849105[6] = 0;
|
||||
out_7050168661066849105[7] = 0;
|
||||
out_7050168661066849105[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_594268378117356058) {
|
||||
out_594268378117356058[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_857388543583997213) {
|
||||
out_857388543583997213[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_3624978990804686878) {
|
||||
out_3624978990804686878[0] = 0;
|
||||
out_3624978990804686878[1] = 0;
|
||||
out_3624978990804686878[2] = 0;
|
||||
out_3624978990804686878[3] = 0;
|
||||
out_3624978990804686878[4] = 0;
|
||||
out_3624978990804686878[5] = 0;
|
||||
out_3624978990804686878[6] = 0;
|
||||
out_3624978990804686878[7] = 0;
|
||||
out_3624978990804686878[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_9134649337191699296) {
|
||||
out_9134649337191699296[0] = 0;
|
||||
out_9134649337191699296[1] = 0;
|
||||
out_9134649337191699296[2] = 0;
|
||||
out_9134649337191699296[3] = 0;
|
||||
out_9134649337191699296[4] = 0;
|
||||
out_9134649337191699296[5] = 0;
|
||||
out_9134649337191699296[6] = 0;
|
||||
out_9134649337191699296[7] = 0;
|
||||
out_9134649337191699296[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_4387195160971747702) {
|
||||
err_fun(nom_x, delta_x, out_4387195160971747702);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402) {
|
||||
err_fun(nom_x, delta_x, out_7214711627163091402);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) {
|
||||
inv_err_fun(nom_x, true_x, out_7707264064834051115);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438) {
|
||||
inv_err_fun(nom_x, true_x, out_911828672798028438);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_7178605602671138983) {
|
||||
H_mod_fun(state, out_7178605602671138983);
|
||||
void car_H_mod_fun(double *state, double *out_149552472482575511) {
|
||||
H_mod_fun(state, out_149552472482575511);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_2416599425795193412) {
|
||||
f_fun(state, dt, out_2416599425795193412);
|
||||
void car_f_fun(double *state, double dt, double *out_4547528931834134293) {
|
||||
f_fun(state, dt, out_4547528931834134293);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_6761986232654706930) {
|
||||
F_fun(state, dt, out_6761986232654706930);
|
||||
void car_F_fun(double *state, double dt, double *out_3560058468708809121) {
|
||||
F_fun(state, dt, out_3560058468708809121);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_319074315832850169) {
|
||||
h_25(state, unused, out_319074315832850169);
|
||||
void car_h_25(double *state, double *unused, double *out_1132582605868503102) {
|
||||
h_25(state, unused, out_1132582605868503102);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_3655624952681647306) {
|
||||
H_25(state, unused, out_3655624952681647306);
|
||||
void car_H_25(double *state, double *unused, double *out_9104003375314738868) {
|
||||
H_25(state, unused, out_9104003375314738868);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5734853090591072423) {
|
||||
h_24(state, unused, out_5734853090591072423);
|
||||
void car_h_24(double *state, double *unused, double *out_8381470974523360630) {
|
||||
h_24(state, unused, out_8381470974523360630);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_1429917168702778744) {
|
||||
H_24(state, unused, out_1429917168702778744);
|
||||
void car_H_24(double *state, double *unused, double *out_4357494048348229834) {
|
||||
H_24(state, unused, out_4357494048348229834);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_8212013810717373528) {
|
||||
h_30(state, unused, out_8212013810717373528);
|
||||
void car_h_30(double *state, double *unused, double *out_975395937517373957) {
|
||||
h_30(state, unused, out_975395937517373957);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_3784963899824887376) {
|
||||
H_30(state, unused, out_3784963899824887376);
|
||||
void car_H_30(double *state, double *unused, double *out_2426050356903195993) {
|
||||
H_30(state, unused, out_2426050356903195993);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_5233056995331332443) {
|
||||
h_26(state, unused, out_5233056995331332443);
|
||||
void car_h_26(double *state, double *unused, double *out_4026277330143471688) {
|
||||
h_26(state, unused, out_4026277330143471688);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_2998770888571335402) {
|
||||
H_26(state, unused, out_2998770888571335402);
|
||||
void car_H_26(double *state, double *unused, double *out_5362500056440682644) {
|
||||
H_26(state, unused, out_5362500056440682644);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_3355860367277182854) {
|
||||
h_27(state, unused, out_3355860367277182854);
|
||||
void car_h_27(double *state, double *unused, double *out_990245959502812892) {
|
||||
h_27(state, unused, out_990245959502812892);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_5959727211625312287) {
|
||||
H_27(state, unused, out_5959727211625312287);
|
||||
void car_H_27(double *state, double *unused, double *out_4600813668703620904) {
|
||||
H_27(state, unused, out_4600813668703620904);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_7630324942183480437) {
|
||||
h_29(state, unused, out_7630324942183480437);
|
||||
void car_h_29(double *state, double *unused, double *out_8450804723751701217) {
|
||||
h_29(state, unused, out_8450804723751701217);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_3274732555510495192) {
|
||||
H_29(state, unused, out_3274732555510495192);
|
||||
void car_H_29(double *state, double *unused, double *out_1915819012588803809) {
|
||||
H_29(state, unused, out_1915819012588803809);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4299370220178845289) {
|
||||
h_28(state, unused, out_4299370220178845289);
|
||||
void car_h_28(double *state, double *unused, double *out_2847713298477492018) {
|
||||
h_28(state, unused, out_2847713298477492018);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_1311102283945168941) {
|
||||
H_28(state, unused, out_1311102283945168941);
|
||||
void car_H_28(double *state, double *unused, double *out_7050168661066849105) {
|
||||
H_28(state, unused, out_7050168661066849105);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_594268378117356058) {
|
||||
h_31(state, unused, out_594268378117356058);
|
||||
void car_h_31(double *state, double *unused, double *out_857388543583997213) {
|
||||
h_31(state, unused, out_857388543583997213);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_3624978990804686878) {
|
||||
H_31(state, unused, out_3624978990804686878);
|
||||
void car_H_31(double *state, double *unused, double *out_9134649337191699296) {
|
||||
H_31(state, unused, out_9134649337191699296);
|
||||
}
|
||||
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_4387195160971747702);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115);
|
||||
void car_H_mod_fun(double *state, double *out_7178605602671138983);
|
||||
void car_f_fun(double *state, double dt, double *out_2416599425795193412);
|
||||
void car_F_fun(double *state, double dt, double *out_6761986232654706930);
|
||||
void car_h_25(double *state, double *unused, double *out_319074315832850169);
|
||||
void car_H_25(double *state, double *unused, double *out_3655624952681647306);
|
||||
void car_h_24(double *state, double *unused, double *out_5734853090591072423);
|
||||
void car_H_24(double *state, double *unused, double *out_1429917168702778744);
|
||||
void car_h_30(double *state, double *unused, double *out_8212013810717373528);
|
||||
void car_H_30(double *state, double *unused, double *out_3784963899824887376);
|
||||
void car_h_26(double *state, double *unused, double *out_5233056995331332443);
|
||||
void car_H_26(double *state, double *unused, double *out_2998770888571335402);
|
||||
void car_h_27(double *state, double *unused, double *out_3355860367277182854);
|
||||
void car_H_27(double *state, double *unused, double *out_5959727211625312287);
|
||||
void car_h_29(double *state, double *unused, double *out_7630324942183480437);
|
||||
void car_H_29(double *state, double *unused, double *out_3274732555510495192);
|
||||
void car_h_28(double *state, double *unused, double *out_4299370220178845289);
|
||||
void car_H_28(double *state, double *unused, double *out_1311102283945168941);
|
||||
void car_h_31(double *state, double *unused, double *out_594268378117356058);
|
||||
void car_H_31(double *state, double *unused, double *out_3624978990804686878);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438);
|
||||
void car_H_mod_fun(double *state, double *out_149552472482575511);
|
||||
void car_f_fun(double *state, double dt, double *out_4547528931834134293);
|
||||
void car_F_fun(double *state, double dt, double *out_3560058468708809121);
|
||||
void car_h_25(double *state, double *unused, double *out_1132582605868503102);
|
||||
void car_H_25(double *state, double *unused, double *out_9104003375314738868);
|
||||
void car_h_24(double *state, double *unused, double *out_8381470974523360630);
|
||||
void car_H_24(double *state, double *unused, double *out_4357494048348229834);
|
||||
void car_h_30(double *state, double *unused, double *out_975395937517373957);
|
||||
void car_H_30(double *state, double *unused, double *out_2426050356903195993);
|
||||
void car_h_26(double *state, double *unused, double *out_4026277330143471688);
|
||||
void car_H_26(double *state, double *unused, double *out_5362500056440682644);
|
||||
void car_h_27(double *state, double *unused, double *out_990245959502812892);
|
||||
void car_H_27(double *state, double *unused, double *out_4600813668703620904);
|
||||
void car_h_29(double *state, double *unused, double *out_8450804723751701217);
|
||||
void car_H_29(double *state, double *unused, double *out_1915819012588803809);
|
||||
void car_h_28(double *state, double *unused, double *out_2847713298477492018);
|
||||
void car_H_28(double *state, double *unused, double *out_7050168661066849105);
|
||||
void car_h_31(double *state, double *unused, double *out_857388543583997213);
|
||||
void car_H_31(double *state, double *unused, double *out_9134649337191699296);
|
||||
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_8207359146711228947);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353);
|
||||
void pose_H_mod_fun(double *state, double *out_7202306802530511060);
|
||||
void pose_f_fun(double *state, double dt, double *out_4961001892384611064);
|
||||
void pose_F_fun(double *state, double dt, double *out_4383751150204030030);
|
||||
void pose_h_4(double *state, double *unused, double *out_878843833946471253);
|
||||
void pose_H_4(double *state, double *unused, double *out_7970215192388091303);
|
||||
void pose_h_10(double *state, double *unused, double *out_1191517594273352576);
|
||||
void pose_H_10(double *state, double *unused, double *out_7914634342859917414);
|
||||
void pose_h_13(double *state, double *unused, double *out_5300060524255047032);
|
||||
void pose_H_13(double *state, double *unused, double *out_7264255055989127512);
|
||||
void pose_h_14(double *state, double *unused, double *out_3965460404426586337);
|
||||
void pose_H_14(double *state, double *unused, double *out_6513288024981975784);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_6169503642516981710);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2781558835044378693);
|
||||
void pose_H_mod_fun(double *state, double *out_4498457996676133449);
|
||||
void pose_f_fun(double *state, double dt, double *out_3471362747861957887);
|
||||
void pose_F_fun(double *state, double dt, double *out_2324287403941072643);
|
||||
void pose_h_4(double *state, double *unused, double *out_3295127809911230665);
|
||||
void pose_H_4(double *state, double *unused, double *out_8270265206520041021);
|
||||
void pose_h_10(double *state, double *unused, double *out_3863591942336910664);
|
||||
void pose_H_10(double *state, double *unused, double *out_6343727255885996060);
|
||||
void pose_h_13(double *state, double *unused, double *out_3880954183827223653);
|
||||
void pose_H_13(double *state, double *unused, double *out_6964205041857177794);
|
||||
void pose_h_14(double *state, double *unused, double *out_8209348319732836111);
|
||||
void pose_H_14(double *state, double *unused, double *out_5187476774224668725);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
@@ -357,7 +357,7 @@ class StarPilotLongitudinalTuneLayout(_SettingsPage):
|
||||
set_state=lambda s: self._params.put_bool("HumanAcceleration", s),
|
||||
visible=self._longitudinal_enabled),
|
||||
SettingRow("CoastUpToLeads", "toggle", tr_noop("Coast Up To Leads"),
|
||||
subtitle=tr_noop("Briefly coast toward far leads before applying normal throttle again."),
|
||||
subtitle=tr_noop("Keep optional far-lead comfort logic on. Recommended unless your car shows lead-follow stutter or springy gas/brake behavior."),
|
||||
get_state=lambda: self._params.get_bool("CoastUpToLeads"),
|
||||
set_state=lambda s: self._params.put_bool("CoastUpToLeads", s),
|
||||
visible=self._longitudinal_enabled),
|
||||
|
||||
@@ -414,8 +414,8 @@ class AugmentedRoadView(CameraView):
|
||||
self._offroad_label.set_text("start the car to\nuse openpilot")
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
# Don't trigger click callback if bookmark was triggered
|
||||
if not self._bookmark_icon.interacting():
|
||||
# Don't trigger click callback if bookmark or HUD widgets consumed the tap.
|
||||
if not self._bookmark_icon.interacting() and not self._hud_renderer.user_interacting():
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
def _render(self, _):
|
||||
|
||||
@@ -260,6 +260,9 @@ class HudRenderer(Widget):
|
||||
self._draw_steering_wheel(self._rect)
|
||||
self._draw_speed_limit_prompt(self._rect)
|
||||
|
||||
def user_interacting(self) -> bool:
|
||||
return self._navigation_card.is_pressed
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
"""Render HUD elements to the screen."""
|
||||
self.prepare(rect)
|
||||
|
||||
@@ -128,7 +128,7 @@ class HudRenderer(Widget):
|
||||
self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size))
|
||||
|
||||
def user_interacting(self) -> bool:
|
||||
return self._exp_button.is_pressed
|
||||
return self._exp_button.is_pressed or self._navigation_card.is_pressed
|
||||
|
||||
def _draw_set_speed(self, rect: rl.Rectangle) -> None:
|
||||
"""Draw the MAX speed indicator box."""
|
||||
|
||||
@@ -6,6 +6,7 @@ from pathlib import Path
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.common.params import UnknownKeyName
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
@@ -71,6 +72,31 @@ class NavigationCardRenderer(Widget):
|
||||
self._has_next = False
|
||||
self._next_maneuver_type = "turn"
|
||||
self._next_modifier = "straight"
|
||||
self._collapsed = False
|
||||
self._collapsed_fallback = False
|
||||
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)
|
||||
|
||||
@property
|
||||
def _hit_rect(self) -> rl.Rectangle:
|
||||
if self._interactive_rect.width > 0 and self._interactive_rect.height > 0:
|
||||
return self._interactive_rect
|
||||
return rl.Rectangle(-1, -1, 0, 0)
|
||||
|
||||
def _toggle_collapsed(self) -> None:
|
||||
if not self._valid:
|
||||
return
|
||||
new_state = not self._collapsed
|
||||
try:
|
||||
ui_state.params_memory.put_bool("NavInstructionCollapsed", new_state)
|
||||
self._collapsed_param_supported = True
|
||||
except UnknownKeyName:
|
||||
self._collapsed_param_supported = False
|
||||
self._collapsed_fallback = new_state
|
||||
self._collapsed = new_state
|
||||
|
||||
def _icon_filename(self, maneuver_type: str, modifier: str) -> str:
|
||||
normalized_type = _normalize_maneuver_type(maneuver_type)
|
||||
@@ -98,6 +124,7 @@ class NavigationCardRenderer(Widget):
|
||||
def _update_state(self) -> None:
|
||||
self._enabled = ui_state.params.get_bool("NavigationUI")
|
||||
self._valid = False
|
||||
self._interactive_rect = rl.Rectangle(0, 0, 0, 0)
|
||||
|
||||
if not self._enabled:
|
||||
return
|
||||
@@ -127,6 +154,10 @@ class NavigationCardRenderer(Widget):
|
||||
self._next_maneuver_type = str(nav_state.get("nextManeuverType") or "turn")
|
||||
self._next_modifier = str(nav_state.get("nextManeuverModifier") or "straight")
|
||||
self._has_next = bool(nav_state.get("nextManeuverType") or nav_state.get("nextManeuverModifier"))
|
||||
if self._collapsed_param_supported is False:
|
||||
self._collapsed = self._collapsed_fallback
|
||||
else:
|
||||
self._collapsed = ui_state.params_memory.get_bool("NavInstructionCollapsed")
|
||||
|
||||
self._valid = True
|
||||
|
||||
@@ -220,6 +251,7 @@ class NavigationCardRenderer(Widget):
|
||||
distance_font_size = 48 if container_height == 225 else 40
|
||||
|
||||
container = rl.Rectangle(container_x, container_y, container_width, container_height)
|
||||
self._interactive_rect = container
|
||||
rl.draw_rectangle_rounded(container, border_radius / container_height, 10, rl.Color(0, 0, 0, 192))
|
||||
|
||||
icon_x = container_x + icon_padding
|
||||
@@ -284,6 +316,29 @@ class NavigationCardRenderer(Widget):
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _render_collapsed_default(self, rect: rl.Rectangle) -> None:
|
||||
chip_size = 94
|
||||
chip_x = int(rect.x + rect.width - chip_size - 32)
|
||||
chip_y = int(rect.y + 82)
|
||||
chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size)
|
||||
self._interactive_rect = chip_rect
|
||||
|
||||
rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(7, 11, 18, 228))
|
||||
rl.draw_rectangle_rounded_lines_ex(chip_rect, 0.34, 10, 2, rl.Color(255, 255, 255, 40))
|
||||
|
||||
icon = self._get_icon(self._maneuver_type, self._modifier)
|
||||
icon_size = 58
|
||||
icon_x = chip_x + (chip_size - icon_size) / 2
|
||||
icon_y = chip_y + (chip_size - icon_size) / 2
|
||||
rl.draw_texture_pro(
|
||||
icon,
|
||||
rl.Rectangle(0, 0, icon.width, icon.height),
|
||||
rl.Rectangle(icon_x, icon_y, icon_size, icon_size),
|
||||
rl.Vector2(0, 0),
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _render_mici(self, rect: rl.Rectangle) -> None:
|
||||
left_safe = 96
|
||||
right_margin = 12
|
||||
@@ -293,6 +348,7 @@ class NavigationCardRenderer(Widget):
|
||||
container_x = int(rect.x + left_safe)
|
||||
container_y = int(rect.y + 16)
|
||||
container = rl.Rectangle(container_x, container_y, container_width, container_height)
|
||||
self._interactive_rect = container
|
||||
rl.draw_rectangle_rounded(container, 0.2, 10, rl.Color(7, 11, 18, 228))
|
||||
rl.draw_rectangle_rounded_lines_ex(container, 0.2, 10, 2, rl.Color(255, 255, 255, 34))
|
||||
|
||||
@@ -378,10 +434,40 @@ class NavigationCardRenderer(Widget):
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _render_collapsed_mici(self, rect: rl.Rectangle) -> None:
|
||||
chip_size = 72
|
||||
right_reserved = 160
|
||||
chip_x = int(rect.x + rect.width - chip_size - right_reserved)
|
||||
chip_y = int(rect.y + 18)
|
||||
chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size)
|
||||
self._interactive_rect = chip_rect
|
||||
|
||||
rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(7, 11, 18, 232))
|
||||
rl.draw_rectangle_rounded_lines_ex(chip_rect, 0.34, 10, 2, rl.Color(255, 255, 255, 40))
|
||||
|
||||
icon = self._get_icon(self._maneuver_type, self._modifier)
|
||||
icon_size = 44
|
||||
icon_x = chip_x + (chip_size - icon_size) / 2
|
||||
icon_y = chip_y + (chip_size - icon_size) / 2
|
||||
rl.draw_texture_pro(
|
||||
icon,
|
||||
rl.Rectangle(0, 0, icon.width, icon.height),
|
||||
rl.Rectangle(icon_x, icon_y, icon_size, icon_size),
|
||||
rl.Vector2(0, 0),
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _render(self, rect: rl.Rectangle) -> None:
|
||||
self._update_state()
|
||||
if not self._valid:
|
||||
return
|
||||
if self._collapsed:
|
||||
if self._layout_variant == "mici":
|
||||
self._render_collapsed_mici(rect)
|
||||
else:
|
||||
self._render_collapsed_default(rect)
|
||||
return
|
||||
if self._layout_variant == "mici":
|
||||
self._render_mici(rect)
|
||||
else:
|
||||
|
||||
@@ -27,6 +27,10 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par
|
||||
screen_recorder->setVisible(false);
|
||||
}
|
||||
|
||||
bool AnnotatedCameraWidget::handleHudTap(const QPoint &pos) {
|
||||
return hud.handleNavigationTap(pos);
|
||||
}
|
||||
|
||||
void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState &fs) {
|
||||
// update engageability/experimental mode button
|
||||
experimental_btn->updateState(s, fs);
|
||||
|
||||
@@ -17,6 +17,7 @@ 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);
|
||||
|
||||
double fps;
|
||||
|
||||
|
||||
@@ -12,6 +12,15 @@ constexpr int SET_SPEED_NA = 255;
|
||||
|
||||
HudRenderer::HudRenderer() {}
|
||||
|
||||
bool HudRenderer::handleNavigationTap(const QPoint &pos) {
|
||||
if (!navigation_valid || !nav_hit_rect.contains(pos)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
params_memory.putBool("NavInstructionCollapsed", !navigation_collapsed);
|
||||
return true;
|
||||
}
|
||||
|
||||
void HudRenderer::updateState(const UIState &s) {
|
||||
is_metric = s.scene.is_metric;
|
||||
status = s.status;
|
||||
@@ -48,6 +57,8 @@ void HudRenderer::updateState(const UIState &s) {
|
||||
navigation_enabled = params.getBool("NavigationUI");
|
||||
navigation_valid = false;
|
||||
navigation_has_next = false;
|
||||
navigation_collapsed = false;
|
||||
nav_hit_rect = QRect();
|
||||
nav_primary_text.clear();
|
||||
nav_secondary_text.clear();
|
||||
nav_distance.clear();
|
||||
@@ -88,6 +99,7 @@ void HudRenderer::updateState(const UIState &s) {
|
||||
nav_next_maneuver_type = nav.value("nextManeuverType").toString().trimmed();
|
||||
nav_next_modifier = nav.value("nextManeuverModifier").toString().trimmed();
|
||||
navigation_has_next = !nav_next_maneuver_type.isEmpty() || !nav_next_modifier.isEmpty();
|
||||
navigation_collapsed = params_memory.getBool("NavInstructionCollapsed");
|
||||
}
|
||||
|
||||
void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
|
||||
@@ -273,6 +285,27 @@ QStringList HudRenderer::wrapNavigationText(int max_width, int font_size, const
|
||||
}
|
||||
|
||||
void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) {
|
||||
if (navigation_collapsed) {
|
||||
const int chip_size = 94;
|
||||
const int chip_x = surface_rect.right() - chip_size - 32;
|
||||
const int chip_y = surface_rect.width() >= 1200 ? 232 : 204;
|
||||
nav_hit_rect = QRect(chip_x, chip_y, chip_size, chip_size);
|
||||
|
||||
p.setPen(Qt::NoPen);
|
||||
p.setBrush(QColor(7, 11, 18, 228));
|
||||
p.drawRoundedRect(nav_hit_rect, 28, 28);
|
||||
p.setPen(QPen(QColor(255, 255, 255, 40), 2));
|
||||
p.setBrush(Qt::NoBrush);
|
||||
p.drawRoundedRect(nav_hit_rect, 28, 28);
|
||||
|
||||
const int icon_size = 58;
|
||||
const QPixmap icon = getNavigationIcon(nav_maneuver_type, nav_modifier, icon_size);
|
||||
const int icon_x = chip_x + (chip_size - icon_size) / 2;
|
||||
const int icon_y = chip_y + (chip_size - icon_size) / 2;
|
||||
p.drawPixmap(icon_x, icon_y, icon);
|
||||
return;
|
||||
}
|
||||
|
||||
const int container_width = std::clamp(surface_rect.width() - 120, 760, 1080);
|
||||
const int container_height = surface_rect.width() >= 1200 ? 238 : 206;
|
||||
const int container_x = (surface_rect.width() - container_width) / 2;
|
||||
@@ -291,6 +324,7 @@ void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) {
|
||||
const int secondary_gap = container_height == 238 ? 10 : 8;
|
||||
|
||||
QRect container(container_x, container_y, container_width, container_height);
|
||||
nav_hit_rect = container;
|
||||
p.setPen(Qt::NoPen);
|
||||
p.setBrush(QColor(0, 0, 0, 180));
|
||||
p.drawRoundedRect(container, border_radius, border_radius);
|
||||
|
||||
@@ -15,6 +15,7 @@ public:
|
||||
HudRenderer();
|
||||
void updateState(const UIState &s);
|
||||
void draw(QPainter &p, const QRect &surface_rect);
|
||||
bool handleNavigationTap(const QPoint &pos);
|
||||
|
||||
StarPilotAnnotatedCameraWidget *starpilot_nvg;
|
||||
|
||||
@@ -39,6 +40,7 @@ private:
|
||||
bool navigation_enabled = false;
|
||||
bool navigation_valid = false;
|
||||
bool navigation_has_next = false;
|
||||
bool navigation_collapsed = false;
|
||||
int status = STATUS_DISENGAGED;
|
||||
QString nav_distance;
|
||||
QString nav_primary_text;
|
||||
@@ -47,6 +49,7 @@ private:
|
||||
QString nav_modifier;
|
||||
QString nav_next_maneuver_type;
|
||||
QString nav_next_modifier;
|
||||
QRect nav_hit_rect;
|
||||
QHash<QString, QPixmap> nav_icon_cache;
|
||||
Params params;
|
||||
Params params_memory{"", true};
|
||||
|
||||
@@ -113,6 +113,11 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
|
||||
}
|
||||
|
||||
void OnroadWindow::mousePressEvent(QMouseEvent* mouseEvent) {
|
||||
if (nvg->handleHudTap(mouseEvent->pos())) {
|
||||
mouseEvent->accept();
|
||||
return;
|
||||
}
|
||||
|
||||
starpilot_nvg->mousePressEvent(mouseEvent);
|
||||
|
||||
if (mouseEvent->isAccepted()) {
|
||||
|
||||
Binary file not shown.
@@ -14,7 +14,7 @@ from openpilot.starpilot.navigation.destination_store import parse_destination_j
|
||||
from openpilot.starpilot.navigation.route_engine import Coordinate, MapboxRouteEngine, NavigationRoute, RouteProgress
|
||||
|
||||
NAVIGATIOND_HZ = 1
|
||||
REROUTE_TRIGGER_SECONDS = 3.0
|
||||
REROUTE_TRIGGER_SECONDS = 2.0
|
||||
ARRIVAL_CLEAR_SECONDS = 5.0
|
||||
LOCATION_STATE_STALE_SECONDS = 2.5
|
||||
|
||||
@@ -71,6 +71,7 @@ class Navigationd:
|
||||
self._bearing_misaligned_started_at = None
|
||||
self._arrival_started_at = None
|
||||
self._publish_nav_state(None, None, False)
|
||||
self.params_memory.remove("NavInstructionCollapsed")
|
||||
|
||||
if remove_destination:
|
||||
self.params.remove("NavDestination")
|
||||
@@ -180,7 +181,7 @@ class Navigationd:
|
||||
|
||||
now = monotonic()
|
||||
off_route = route.off_route_distance_exceeded(progress, v_ego)
|
||||
misaligned = route.route_bearing_misaligned(progress.closest_index, self._last_bearing, v_ego)
|
||||
misaligned = route.route_bearing_misaligned(progress.closest_segment_index, self._last_bearing, v_ego)
|
||||
arrived = route.arrived(progress, v_ego)
|
||||
|
||||
self._off_route_started_at = self._bump_timer(self._off_route_started_at, off_route and not arrived, now)
|
||||
|
||||
@@ -20,7 +20,7 @@ SPEED_CONVERSIONS = {
|
||||
}
|
||||
|
||||
OFF_ROUTE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 20.0, 40.0]
|
||||
OFF_ROUTE_DISTANCE_BREAKPOINTS = [100.0, 125.0, 150.0, 200.0, 250.0]
|
||||
OFF_ROUTE_DISTANCE_BREAKPOINTS = [40.0, 50.0, 60.0, 80.0, 100.0]
|
||||
UPCOMING_TURN_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0]
|
||||
UPCOMING_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0]
|
||||
UTURN_MODIFIER = "uturn"
|
||||
@@ -79,6 +79,7 @@ class RouteStep:
|
||||
@dataclass(frozen=True)
|
||||
class RouteProgress:
|
||||
closest_index: int
|
||||
closest_segment_index: int
|
||||
distance_from_route: float
|
||||
current_step: RouteStep
|
||||
next_step: RouteStep | None
|
||||
@@ -112,23 +113,24 @@ def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float:
|
||||
projection = a + ab * t
|
||||
return projection.distance_to(p)
|
||||
|
||||
def project_onto_segment(a: Coordinate, b: Coordinate, p: Coordinate) -> tuple[float, float]:
|
||||
lat_scale = EARTH_MEAN_RADIUS * math.pi / 180.0
|
||||
ref_lat = math.radians((a.latitude + b.latitude + p.latitude) / 3.0)
|
||||
lon_scale = lat_scale * math.cos(ref_lat)
|
||||
|
||||
def distance_along_geometry(geometry: list[Coordinate], position: Coordinate) -> float:
|
||||
if len(geometry) <= 2:
|
||||
return geometry[0].distance_to(position)
|
||||
ab_x = (b.longitude - a.longitude) * lon_scale
|
||||
ab_y = (b.latitude - a.latitude) * lat_scale
|
||||
ap_x = (p.longitude - a.longitude) * lon_scale
|
||||
ap_y = (p.latitude - a.latitude) * lat_scale
|
||||
|
||||
total_distance = 0.0
|
||||
closest_total_distance = 0.0
|
||||
closest_distance = 1e9
|
||||
segment_length_sq = ab_x * ab_x + ab_y * ab_y
|
||||
if segment_length_sq <= 1e-6:
|
||||
return 0.0, math.hypot(ap_x, ap_y)
|
||||
|
||||
for index in range(len(geometry) - 1):
|
||||
segment_distance = minimum_distance(geometry[index], geometry[index + 1], position)
|
||||
if segment_distance < closest_distance:
|
||||
closest_distance = segment_distance
|
||||
closest_total_distance = total_distance + geometry[index].distance_to(position)
|
||||
total_distance += geometry[index].distance_to(geometry[index + 1])
|
||||
|
||||
return closest_total_distance
|
||||
t = float(np.clip((ap_x * ab_x + ap_y * ab_y) / segment_length_sq, 0.0, 1.0))
|
||||
proj_x = ab_x * t
|
||||
proj_y = ab_y * t
|
||||
return t, math.hypot(ap_x - proj_x, ap_y - proj_y)
|
||||
|
||||
|
||||
def string_to_direction(direction: str) -> str:
|
||||
@@ -207,6 +209,7 @@ def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -
|
||||
@dataclass
|
||||
class NavigationRoute:
|
||||
geometry: list[Coordinate]
|
||||
geometry_cumulative_distances: list[float]
|
||||
bearings: list[float]
|
||||
steps: list[RouteStep]
|
||||
total_distance: float
|
||||
@@ -243,38 +246,54 @@ class NavigationRoute:
|
||||
instruction=str(step.get("instruction", "")),
|
||||
))
|
||||
|
||||
bearings = [bearing_between_two_points(geometry[index], geometry[index + 2]) for index in range(len(geometry) - 2)]
|
||||
bearings = [bearing_between_two_points(geometry[index], geometry[index + 1]) for index in range(len(geometry) - 1)]
|
||||
return cls(
|
||||
geometry=geometry,
|
||||
geometry_cumulative_distances=cumulative_distances,
|
||||
bearings=bearings,
|
||||
steps=steps,
|
||||
total_distance=float(route_data.get("totalDistance", 0.0)),
|
||||
total_duration=float(route_data.get("totalDuration", 0.0)),
|
||||
)
|
||||
|
||||
def route_bearing_misaligned(self, closest_index: int, current_bearing: float | None, v_ego: float) -> bool:
|
||||
if current_bearing is None or v_ego < 5.0 or closest_index <= 0 or closest_index >= len(self.geometry) - 1:
|
||||
return False
|
||||
if closest_index - 1 >= len(self.bearings):
|
||||
def route_bearing_misaligned(self, closest_segment_index: int, current_bearing: float | None, v_ego: float) -> bool:
|
||||
if current_bearing is None or v_ego < 2.5 or closest_segment_index < 0 or closest_segment_index >= len(self.bearings):
|
||||
return False
|
||||
|
||||
route_bearing = self.bearings[closest_index - 1]
|
||||
route_bearing = self.bearings[closest_segment_index]
|
||||
normalized_bearing = (current_bearing + 360.0) % 360.0
|
||||
bearing_difference = abs(normalized_bearing - route_bearing)
|
||||
return min(bearing_difference, 360.0 - bearing_difference) > 110.0
|
||||
return min(bearing_difference, 360.0 - bearing_difference) > 75.0
|
||||
|
||||
def get_progress(self, position: Coordinate) -> RouteProgress | None:
|
||||
if not self.geometry or not self.steps:
|
||||
return None
|
||||
if len(self.geometry) == 1:
|
||||
closest_index = 0
|
||||
closest_segment_index = 0
|
||||
min_distance = position.distance_to(self.geometry[0])
|
||||
closest_cumulative = 0.0
|
||||
else:
|
||||
best_segment_index = 0
|
||||
best_distance = float("inf")
|
||||
best_t = 0.0
|
||||
|
||||
closest_index, min_distance = min(
|
||||
((idx, position.distance_to(coord)) for idx, coord in enumerate(self.geometry)),
|
||||
key=lambda item: item[1],
|
||||
)
|
||||
closest_cumulative = distance_along_geometry(self.geometry, position)
|
||||
for index in range(len(self.geometry) - 1):
|
||||
t, segment_distance = project_onto_segment(self.geometry[index], self.geometry[index + 1], position)
|
||||
if segment_distance < best_distance:
|
||||
best_distance = segment_distance
|
||||
best_segment_index = index
|
||||
best_t = t
|
||||
|
||||
closest_segment_index = best_segment_index
|
||||
segment_start = self.geometry_cumulative_distances[closest_segment_index]
|
||||
segment_end = self.geometry_cumulative_distances[closest_segment_index + 1]
|
||||
closest_cumulative = segment_start + (segment_end - segment_start) * best_t
|
||||
min_distance = best_distance
|
||||
closest_index = min(closest_segment_index + (1 if best_t >= 0.5 else 0), len(self.geometry) - 1)
|
||||
|
||||
current_step_index = max(
|
||||
(idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= closest_cumulative),
|
||||
(idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= (closest_cumulative + 1e-3)),
|
||||
default=-1,
|
||||
)
|
||||
current_step = self.steps[current_step_index if current_step_index >= 0 else 0]
|
||||
@@ -303,6 +322,7 @@ class NavigationRoute:
|
||||
|
||||
return RouteProgress(
|
||||
closest_index=closest_index,
|
||||
closest_segment_index=closest_segment_index,
|
||||
distance_from_route=min_distance,
|
||||
current_step=current_step,
|
||||
next_step=next_step,
|
||||
@@ -328,10 +348,13 @@ class NavigationRoute:
|
||||
return progress.distance_from_route > distance_threshold
|
||||
|
||||
def arrived(self, progress: RouteProgress, v_ego: float) -> bool:
|
||||
if v_ego >= 1.0 or not progress.all_maneuvers:
|
||||
if v_ego >= 2.0 or not progress.all_maneuvers:
|
||||
return False
|
||||
current = progress.all_maneuvers[0]
|
||||
return current["type"] == "arrive" or progress.current_step.instruction.startswith("Your destination")
|
||||
destination_step = current["type"] == "arrive" or progress.current_step.maneuver == "arrive" or progress.current_step.instruction.startswith("Your destination")
|
||||
if not destination_step and progress.next_step is not None:
|
||||
destination_step = progress.next_step.maneuver == "arrive" and progress.distance_to_end_of_step <= max(15.0, v_ego * 8.0)
|
||||
return destination_step and progress.distance_remaining <= 40.0
|
||||
|
||||
def build_instruction_payload(self, progress: RouteProgress, *, use_vienna_sign: bool = False) -> dict[str, Any]:
|
||||
parsed = parse_banner_instructions(progress.current_step.banner_instructions, progress.distance_to_end_of_step) or {}
|
||||
|
||||
@@ -90,10 +90,29 @@ def test_route_off_route_and_arrival_detection():
|
||||
assert misaligned_progress is not None
|
||||
assert arrive_progress is not None
|
||||
assert route.off_route_distance_exceeded(off_route_progress, 10.0)
|
||||
assert route.route_bearing_misaligned(misaligned_progress.closest_index, 270.0, 10.0)
|
||||
assert route.route_bearing_misaligned(misaligned_progress.closest_segment_index, 270.0, 10.0)
|
||||
assert route.arrived(arrive_progress, 0.5)
|
||||
|
||||
|
||||
def test_route_progress_projects_onto_segments_for_destination_step():
|
||||
route = make_route()
|
||||
progress = route.get_progress(Coordinate(0.00098, 0.0020))
|
||||
|
||||
assert progress is not None
|
||||
assert progress.next_step is not None
|
||||
assert progress.next_step.maneuver == "arrive"
|
||||
assert progress.distance_remaining <= 5.0
|
||||
assert route.arrived(progress, 0.5)
|
||||
|
||||
|
||||
def test_route_bearing_misaligned_catches_ninety_degree_wrong_turn():
|
||||
route = make_route()
|
||||
progress = route.get_progress(Coordinate(0.0, 0.0015))
|
||||
|
||||
assert progress is not None
|
||||
assert route.route_bearing_misaligned(progress.closest_segment_index, 0.0, 6.0)
|
||||
|
||||
|
||||
def test_lane_payload_uses_capnp_enum_names():
|
||||
route_data = {
|
||||
"geometry": [
|
||||
|
||||
@@ -1000,7 +1000,7 @@
|
||||
{
|
||||
"key": "CoastUpToLeads",
|
||||
"label": "Coast Up To Leads",
|
||||
"description": "Allow openpilot to briefly coast toward far leads before resuming normal throttle. Disable this if your vehicle shows gas/brake alternation while approaching distant traffic.",
|
||||
"description": "Keep the optional far-lead comfort logic enabled, including gentle coasting and matched-follow smoothing for distant leads. Recommended unless your car shows lead-follow stutter or springy gas/brake behavior.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle",
|
||||
"parent_key": "LongitudinalTune"
|
||||
|
||||
@@ -147,7 +147,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
{"AccelerationProfile", tr("Acceleration Profile"), tr("<b>How quickly openpilot speeds up.</b> \"Eco\" is gentle and efficient, \"Sport\" is firmer and more responsive, and \"Sport+\" accelerates at the maximum rate allowed."), ""},
|
||||
{"DecelerationProfile", tr("Deceleration Profile"), tr("<b>How firmly openpilot slows down.</b> \"Eco\" favors coasting, \"Sport\" applies stronger braking."), ""},
|
||||
{"HumanAcceleration", tr("Human-Like Acceleration"), tr("<b>Acceleration that mimics human behavior</b> by easing the throttle at low speeds and adding extra power when taking off from a stop."), ""},
|
||||
{"CoastUpToLeads", tr("Coast Up To Leads"), tr("<b>Allow openpilot to coast toward far leads before resuming normal throttle.</b> Disable this if your vehicle shows noticeable gas/brake alternation while approaching distant traffic."), ""},
|
||||
{"CoastUpToLeads", tr("Coast Up To Leads"), tr("<b>Keep the optional far-lead comfort logic enabled, including gentle coasting and matched-follow smoothing for distant leads.</b> Recommended to leave on unless your vehicle shows lead-follow stutter or springy gas/brake behavior."), ""},
|
||||
{"HumanLaneChanges", tr("Human-Like Lane Changes"), tr("<b>Lane-change behavior that mimics human drivers</b> by anticipating and tracking adjacent vehicles during lane changes."), ""},
|
||||
{"LeadDetectionThreshold", tr("Lead Detection Sensitivity"), tr("<b>How sensitive openpilot is to detecting vehicles.</b> Higher sensitivity allows quicker detection at longer distances but may react to non-vehicle objects; lower sensitivity is more conservative and reduces false detections."), ""},
|
||||
{"TacoTune", tr("\"Taco Bell Run\" Turn Speed Hack"), tr("<b>The turn-speed hack from comma's 2022 \"Taco Bell Run\".</b> Designed to slow down for left and right turns."), ""},
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -5,6 +5,7 @@ import time
|
||||
from itertools import cycle
|
||||
|
||||
from cereal import log, messaging
|
||||
from openpilot.common.params import Params, UnknownKeyName
|
||||
|
||||
|
||||
ROUTE_COORDS = [
|
||||
@@ -108,6 +109,20 @@ SCENARIOS = [
|
||||
]
|
||||
|
||||
|
||||
def build_nav_state(params_memory: Params, scenario: dict) -> None:
|
||||
params_memory.put_nonblocking("NavInstructionState", {
|
||||
"valid": True,
|
||||
"maneuverModifier": str(scenario["modifier"]),
|
||||
"maneuverType": str(scenario["type"]),
|
||||
"maneuverPrimaryText": str(scenario["primary"]),
|
||||
"maneuverSecondaryText": str(scenario["secondary"]),
|
||||
"maneuverDistance": float(scenario["distance"]),
|
||||
"nextManeuverType": str(scenario["next_type"]),
|
||||
"nextManeuverModifier": str(scenario["next_modifier"]),
|
||||
"nextManeuverDistance": float(scenario["distance"] + 220.0),
|
||||
})
|
||||
|
||||
|
||||
def build_instruction(pm: messaging.PubMaster, scenario: dict) -> None:
|
||||
msg = messaging.new_message("navInstruction")
|
||||
msg.valid = True
|
||||
@@ -146,22 +161,35 @@ def main() -> None:
|
||||
args = parser.parse_args()
|
||||
|
||||
pm = messaging.PubMaster(["navInstruction", "navRoute"])
|
||||
params_memory = Params(memory=True)
|
||||
try:
|
||||
params_memory.put_bool("NavInstructionCollapsed", False)
|
||||
except UnknownKeyName:
|
||||
pass
|
||||
|
||||
scenario_cycle = cycle(SCENARIOS)
|
||||
current = next(scenario_cycle)
|
||||
next_switch = time.monotonic() + args.hold_seconds
|
||||
print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True)
|
||||
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
if now >= next_switch:
|
||||
current = next(scenario_cycle)
|
||||
next_switch = now + args.hold_seconds
|
||||
print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True)
|
||||
try:
|
||||
while True:
|
||||
now = time.monotonic()
|
||||
if now >= next_switch:
|
||||
current = next(scenario_cycle)
|
||||
next_switch = now + args.hold_seconds
|
||||
print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True)
|
||||
|
||||
build_instruction(pm, current)
|
||||
build_route(pm)
|
||||
time.sleep(args.publish_interval)
|
||||
build_nav_state(params_memory, current)
|
||||
build_instruction(pm, current)
|
||||
build_route(pm)
|
||||
time.sleep(args.publish_interval)
|
||||
finally:
|
||||
params_memory.remove("NavInstructionState")
|
||||
try:
|
||||
params_memory.remove("NavInstructionCollapsed")
|
||||
except UnknownKeyName:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
Reference in New Issue
Block a user