mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-09-18 13:33:57 +08:00
nav
This commit is contained in:
firestar5683
@@ -367,6 +367,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LoudBlindspotAlert", {PERSISTENT, BOOL, "0", "0", 0}},
|
||||
{"LowVoltageShutdown", {PERSISTENT, FLOAT, "11.8", "11.8", 3}},
|
||||
{"ManualUpdateInitiated", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}},
|
||||
{"AMapKey1", {PERSISTENT | DONT_LOG, STRING, "", "", 0}},
|
||||
{"AMapKey2", {PERSISTENT | DONT_LOG, STRING, "", "", 0}},
|
||||
{"ApiCache_NavDestinations", {PERSISTENT, JSON, "[]", "[]"}},
|
||||
{"MapAcceleration", {PERSISTENT, BOOL, "0", "0", 1}},
|
||||
{"MapboxPublicKey", {PERSISTENT | DONT_LOG, STRING, "", "", 0}},
|
||||
{"MapBoxRequests", {PERSISTENT, JSON, "{}", "{}"}},
|
||||
@@ -376,6 +379,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"MapGears", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"MapsSelected", {PERSISTENT, STRING, "", "", 0}},
|
||||
{"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
|
||||
{"NavDesiresAllowed", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"NavDestination", {PERSISTENT, STRING, "", ""}},
|
||||
{"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"}},
|
||||
{"VisionSpeedLimitConfidence", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
|
||||
|
||||
@@ -27,18 +27,20 @@ env_var_truthy() {
|
||||
usage() {
|
||||
cat <<'EOF'
|
||||
Usage:
|
||||
./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [--prefix name] <route-or-replay-args...>
|
||||
./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [-nav] [--prefix name] <route-or-replay-args...>
|
||||
|
||||
Examples:
|
||||
./onroad --c3 f08912a233c1584f/2022-08-11--18-02-41/1
|
||||
./onroad --c4 f08912a233c1584f/2022-08-11--18-02-41/1 --start 30
|
||||
./onroad --all f08912a233c1584f/2022-08-11--18-02-41/1
|
||||
./onroad --c3 <route>
|
||||
./onroad --c4 <route> --start 30
|
||||
./onroad --c4 -nav <route>
|
||||
./onroad --all <route>
|
||||
./onroad --replay-only --demo --no-vipc --no-loop
|
||||
|
||||
Notes:
|
||||
- This is host/dev only. It uses the isolated host worktree and does not touch the device path.
|
||||
- A private comma connect route still requires tools/lib/auth.py before replay can download it.
|
||||
- Use multiple UI flags together if you want more than one desktop UI at once.
|
||||
- -nav injects a fake navigation demo stream and blocks replay from publishing navInstruction/navRoute.
|
||||
EOF
|
||||
}
|
||||
|
||||
@@ -53,7 +55,9 @@ REPLAY_ARGS=()
|
||||
UI_TARGETS=()
|
||||
LEGACY_UI_SELECTION=""
|
||||
REPLAY_ONLY=0
|
||||
NAV_DEMO=0
|
||||
REPLAY_PID=""
|
||||
NAV_PID=""
|
||||
UI_PIDS=()
|
||||
|
||||
parse_args() {
|
||||
@@ -83,6 +87,10 @@ parse_args() {
|
||||
REPLAY_ONLY=1
|
||||
shift
|
||||
;;
|
||||
-nav|--nav)
|
||||
NAV_DEMO=1
|
||||
shift
|
||||
;;
|
||||
--ui)
|
||||
if [[ $# -lt 2 ]]; then
|
||||
echo "Missing value for --ui" >&2
|
||||
@@ -194,6 +202,9 @@ cleanup() {
|
||||
if [[ -n "${REPLAY_PID}" ]]; then
|
||||
kill "${REPLAY_PID}" >/dev/null 2>&1 || true
|
||||
fi
|
||||
if [[ -n "${NAV_PID}" ]]; then
|
||||
kill "${NAV_PID}" >/dev/null 2>&1 || true
|
||||
fi
|
||||
|
||||
for pid in "${UI_PIDS[@]-}"; do
|
||||
if [[ -n "${pid}" ]]; then
|
||||
@@ -203,6 +214,9 @@ cleanup() {
|
||||
if [[ -n "${REPLAY_PID}" ]]; then
|
||||
wait "${REPLAY_PID}" >/dev/null 2>&1 || true
|
||||
fi
|
||||
if [[ -n "${NAV_PID}" ]]; then
|
||||
wait "${NAV_PID}" >/dev/null 2>&1 || true
|
||||
fi
|
||||
|
||||
if [[ -n "${OPENPILOT_PREFIX:-}" && "${OPENPILOT_PREFIX}" == desktop-onroad-* ]]; then
|
||||
echo "Cleaning up temporary prefix environment (${OPENPILOT_PREFIX})..."
|
||||
@@ -214,6 +228,68 @@ cleanup() {
|
||||
exit "${exit_code}"
|
||||
}
|
||||
|
||||
append_blocked_service_names() {
|
||||
local existing="$1"
|
||||
local required="$2"
|
||||
local combined="${existing}"
|
||||
|
||||
if [[ -n "${combined}" ]]; then
|
||||
combined+=",${required}"
|
||||
else
|
||||
combined="${required}"
|
||||
fi
|
||||
|
||||
local deduped=()
|
||||
local seen=","
|
||||
local part=""
|
||||
local parts=()
|
||||
IFS=',' read -r -a parts <<< "${combined}"
|
||||
for part in "${parts[@]-}"; do
|
||||
part="${part// /}"
|
||||
if [[ -z "${part}" ]]; then
|
||||
continue
|
||||
fi
|
||||
if [[ "${seen}" != *",${part},"* ]]; then
|
||||
deduped+=("${part}")
|
||||
seen+="${part},"
|
||||
fi
|
||||
done
|
||||
|
||||
local joined=""
|
||||
for part in "${deduped[@]-}"; do
|
||||
if [[ -n "${joined}" ]]; then
|
||||
joined+=","
|
||||
fi
|
||||
joined+="${part}"
|
||||
done
|
||||
|
||||
printf '%s' "${joined}"
|
||||
}
|
||||
|
||||
ensure_nav_demo_replay_blocklist() {
|
||||
local nav_services="navInstruction,navRoute"
|
||||
local idx=0
|
||||
|
||||
for ((idx=0; idx<${#REPLAY_ARGS[@]}; idx++)); do
|
||||
case "${REPLAY_ARGS[$idx]}" in
|
||||
-b|--block)
|
||||
if (( idx + 1 >= ${#REPLAY_ARGS[@]} )); then
|
||||
echo "Missing value for ${REPLAY_ARGS[$idx]}" >&2
|
||||
exit 1
|
||||
fi
|
||||
REPLAY_ARGS[$((idx + 1))]="$(append_blocked_service_names "${REPLAY_ARGS[$((idx + 1))]}" "${nav_services}")"
|
||||
return
|
||||
;;
|
||||
--block=*)
|
||||
REPLAY_ARGS[$idx]="--block=$(append_blocked_service_names "${REPLAY_ARGS[$idx]#*=}" "${nav_services}")"
|
||||
return
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
REPLAY_ARGS=(-b "${nav_services}" "${REPLAY_ARGS[@]}")
|
||||
}
|
||||
|
||||
prepare_env() {
|
||||
source .venv/bin/activate
|
||||
|
||||
@@ -238,6 +314,7 @@ prepare_env() {
|
||||
export SP_C4_FAKE_WIFI=0
|
||||
export SP_RAYBIG_FAKE_WIFI=0
|
||||
export SP_ALLOW_DESKTOP_FAKE_WIFI=0
|
||||
export SP_ONROAD_NAV_DEMO="${NAV_DEMO}"
|
||||
|
||||
if [[ "$(uname -s)" == "Darwin" ]] || env_var_truthy "${ZMQ:-0}"; then
|
||||
export OPENPILOT_ZMQ_NAMESPACE="${PREFIX_ARG:-${OPENPILOT_ZMQ_NAMESPACE:-desktop-onroad-$$}}"
|
||||
@@ -250,6 +327,8 @@ prepare_env() {
|
||||
|
||||
seed_params() {
|
||||
"${ROOT_DIR}/.venv/bin/python3" - <<'PY'
|
||||
import os
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.version import terms_version, training_version
|
||||
|
||||
@@ -260,6 +339,8 @@ params.put_bool("OpenpilotEnabledToggle", True)
|
||||
params.put_bool("IsDriverViewEnabled", False)
|
||||
params.put_bool("ForceOnroad", False)
|
||||
params.put_bool("ForceOffroad", False)
|
||||
if os.getenv("SP_ONROAD_NAV_DEMO", "").lower() in {"1", "true", "yes", "on"}:
|
||||
params.put_bool("NavigationUI", True)
|
||||
PY
|
||||
}
|
||||
|
||||
@@ -302,6 +383,18 @@ launch_replay() {
|
||||
fi
|
||||
}
|
||||
|
||||
launch_nav_demo() {
|
||||
echo "Starting fake nav demo publisher..."
|
||||
"${ROOT_DIR}/.venv/bin/python3" "${ROOT_DIR}/tools/replay/fake_nav_demo.py" &
|
||||
NAV_PID=$!
|
||||
|
||||
sleep 0.5
|
||||
if ! kill -0 "${NAV_PID}" >/dev/null 2>&1; then
|
||||
wait "${NAV_PID}"
|
||||
return 1
|
||||
fi
|
||||
}
|
||||
|
||||
launch_c3_ui() {
|
||||
local os_ext="linux"
|
||||
if [[ "$(uname -s)" == "Darwin" ]]; then
|
||||
@@ -339,6 +432,10 @@ else
|
||||
dedupe_ui_targets
|
||||
fi
|
||||
|
||||
if [[ "${NAV_DEMO}" == "1" ]]; then
|
||||
ensure_nav_demo_replay_blocklist
|
||||
fi
|
||||
|
||||
if [[ ${#REPLAY_ARGS[@]} -eq 0 ]]; then
|
||||
usage >&2
|
||||
exit 1
|
||||
@@ -378,6 +475,10 @@ seed_starpilot_theme
|
||||
echo "Starting replay: ${REPLAY_ARGS[*]}"
|
||||
launch_replay
|
||||
|
||||
if [[ "${NAV_DEMO}" == "1" ]]; then
|
||||
launch_nav_demo
|
||||
fi
|
||||
|
||||
if [[ ${#UI_TARGETS[@]} -eq 0 ]]; then
|
||||
echo "Replay is running without UI windows. Press Ctrl-C to stop."
|
||||
wait "${REPLAY_PID}"
|
||||
|
||||
@@ -1,5 +1,8 @@
|
||||
import json
|
||||
|
||||
from cereal import log
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
|
||||
LaneChangeState = log.LaneChangeState
|
||||
@@ -40,6 +43,8 @@ TURN_DESIRES = {
|
||||
|
||||
class DesireHelper:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.params_memory = Params(memory=True)
|
||||
self.lane_change_state = LaneChangeState.off
|
||||
self.lane_change_direction = LaneChangeDirection.none
|
||||
self.lane_change_timer = 0.0
|
||||
@@ -51,6 +56,84 @@ class DesireHelper:
|
||||
self.lane_change_completed = False
|
||||
|
||||
self.lane_change_wait_timer = 0.0
|
||||
self.nav_desires_allowed = False
|
||||
self._nav_param_counter = -1
|
||||
self._nav_instruction_state_raw: object = None
|
||||
self._nav_instruction_state: dict[str, object] = {}
|
||||
|
||||
def _update_nav_params(self):
|
||||
self._nav_param_counter += 1
|
||||
if self._nav_param_counter % 60 == 0:
|
||||
self.nav_desires_allowed = self.params.get_bool("NavDesiresAllowed")
|
||||
|
||||
raw = self.params_memory.get("NavInstructionState") or {}
|
||||
if raw == self._nav_instruction_state_raw:
|
||||
return
|
||||
|
||||
self._nav_instruction_state_raw = raw
|
||||
if not raw:
|
||||
self._nav_instruction_state = {}
|
||||
return
|
||||
|
||||
if isinstance(raw, dict):
|
||||
self._nav_instruction_state = raw
|
||||
return
|
||||
|
||||
if isinstance(raw, str):
|
||||
try:
|
||||
parsed = json.loads(raw)
|
||||
self._nav_instruction_state = parsed if isinstance(parsed, dict) else {}
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
self._nav_instruction_state = {}
|
||||
|
||||
@staticmethod
|
||||
def _nav_keep_direction_is_clear(carstate, lane_change_direction):
|
||||
return not (
|
||||
(lane_change_direction == LaneChangeDirection.left and carstate.leftBlindspot) or
|
||||
(lane_change_direction == LaneChangeDirection.right and carstate.rightBlindspot)
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _nav_torque_applied(carstate, lane_change_direction):
|
||||
return carstate.steeringPressed and (
|
||||
(lane_change_direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
|
||||
(lane_change_direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
|
||||
)
|
||||
|
||||
def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles):
|
||||
self._update_nav_params()
|
||||
if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)):
|
||||
return log.Desire.none
|
||||
|
||||
modifier = str(self._nav_instruction_state.get("maneuverModifier", ""))
|
||||
if modifier == "":
|
||||
return log.Desire.none
|
||||
|
||||
if modifier == "slightLeft":
|
||||
lane_change_direction = LaneChangeDirection.left
|
||||
desired_lane_width = starpilotPlan.laneWidthLeft
|
||||
nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
|
||||
if not carstate.rightBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction):
|
||||
if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed:
|
||||
return log.Desire.keepLeft
|
||||
elif modifier == "slightRight":
|
||||
lane_change_direction = LaneChangeDirection.right
|
||||
desired_lane_width = starpilotPlan.laneWidthRight
|
||||
nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width
|
||||
if not carstate.leftBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction):
|
||||
if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed:
|
||||
return log.Desire.keepRight
|
||||
elif modifier in ("left", "sharpLeft"):
|
||||
if not carstate.rightBlinker and not carstate.leftBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill:
|
||||
return log.Desire.turnLeft
|
||||
elif modifier in ("right", "sharpRight"):
|
||||
if not carstate.leftBlinker and not carstate.rightBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill:
|
||||
return log.Desire.turnRight
|
||||
|
||||
return log.Desire.none
|
||||
|
||||
@staticmethod
|
||||
def get_lane_change_direction(CS):
|
||||
@@ -155,3 +238,7 @@ class DesireHelper:
|
||||
self.lane_change_completed = False
|
||||
|
||||
self.lane_change_wait_timer = 0.0
|
||||
|
||||
nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles)
|
||||
if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off:
|
||||
self.desire = nav_desire
|
||||
|
||||
@@ -0,0 +1,113 @@
|
||||
from types import SimpleNamespace
|
||||
|
||||
from cereal import log
|
||||
|
||||
from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeDirection, LaneChangeState
|
||||
|
||||
|
||||
def make_car_state(**overrides):
|
||||
defaults = {
|
||||
"vEgo": 20.0,
|
||||
"leftBlinker": False,
|
||||
"rightBlinker": False,
|
||||
"leftBlindspot": False,
|
||||
"rightBlindspot": False,
|
||||
"steeringPressed": False,
|
||||
"steeringTorque": 0.0,
|
||||
"standstill": False,
|
||||
}
|
||||
defaults.update(overrides)
|
||||
return SimpleNamespace(**defaults)
|
||||
|
||||
|
||||
def make_toggles(**overrides):
|
||||
defaults = {
|
||||
"lane_changes": True,
|
||||
"lane_change_delay": 0.0,
|
||||
"lane_detection_width": 3.0,
|
||||
"minimum_lane_change_speed": 10.0,
|
||||
"nudgeless": True,
|
||||
"one_lane_change": False,
|
||||
"use_turn_desires": False,
|
||||
}
|
||||
defaults.update(overrides)
|
||||
return SimpleNamespace(**defaults)
|
||||
|
||||
|
||||
def make_plan(**overrides):
|
||||
defaults = {
|
||||
"laneWidthLeft": 4.0,
|
||||
"laneWidthRight": 4.0,
|
||||
}
|
||||
defaults.update(overrides)
|
||||
return SimpleNamespace(**defaults)
|
||||
|
||||
def test_nav_desires_keep_left_when_route_requests_it():
|
||||
helper = DesireHelper()
|
||||
helper.nav_desires_allowed = True
|
||||
helper._update_nav_params = lambda: None
|
||||
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"}
|
||||
|
||||
helper.update(
|
||||
make_car_state(vEgo=20.0),
|
||||
True,
|
||||
0.0,
|
||||
make_plan(laneWidthLeft=4.2),
|
||||
make_toggles(nudgeless=True),
|
||||
)
|
||||
|
||||
assert helper.desire == log.Desire.keepLeft
|
||||
|
||||
|
||||
def test_nav_desires_turn_right_below_lane_change_speed():
|
||||
helper = DesireHelper()
|
||||
helper.nav_desires_allowed = True
|
||||
helper._update_nav_params = lambda: None
|
||||
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right"}
|
||||
|
||||
helper.update(
|
||||
make_car_state(vEgo=5.0),
|
||||
True,
|
||||
0.0,
|
||||
make_plan(),
|
||||
make_toggles(minimum_lane_change_speed=10.0),
|
||||
)
|
||||
|
||||
assert helper.desire == log.Desire.turnRight
|
||||
|
||||
|
||||
def test_nav_desires_do_not_override_lane_change_state_machine():
|
||||
helper = DesireHelper()
|
||||
helper.nav_desires_allowed = True
|
||||
helper._update_nav_params = lambda: None
|
||||
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
|
||||
helper.lane_change_state = LaneChangeState.laneChangeStarting
|
||||
helper.lane_change_direction = LaneChangeDirection.left
|
||||
helper.lane_change_ll_prob = 0.5
|
||||
|
||||
helper.update(
|
||||
make_car_state(vEgo=25.0, leftBlinker=True),
|
||||
True,
|
||||
0.5,
|
||||
make_plan(),
|
||||
make_toggles(),
|
||||
)
|
||||
|
||||
assert helper.desire == log.Desire.laneChangeLeft
|
||||
|
||||
|
||||
def test_nav_desires_disabled_leave_desire_unchanged():
|
||||
helper = DesireHelper()
|
||||
helper.nav_desires_allowed = False
|
||||
helper._update_nav_params = lambda: None
|
||||
helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left"}
|
||||
|
||||
helper.update(
|
||||
make_car_state(vEgo=5.0),
|
||||
True,
|
||||
0.0,
|
||||
make_plan(),
|
||||
make_toggles(minimum_lane_change_speed=10.0),
|
||||
)
|
||||
|
||||
assert helper.desire == log.Desire.none
|
||||
@@ -283,6 +283,7 @@ def main():
|
||||
filter_initialized = False
|
||||
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
|
||||
observation_input_invalid = defaultdict(int)
|
||||
last_inputs_diagnostics = None
|
||||
|
||||
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
|
||||
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
|
||||
@@ -335,6 +336,31 @@ def main():
|
||||
inputs_valid = sm.all_valid() and critical_service_inputs_valid
|
||||
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
|
||||
|
||||
invalid_services = [s for s, valid in sm.valid.items() if not valid]
|
||||
over_threshold = {
|
||||
s: round(float(observation_input_invalid[s]), 3)
|
||||
for s in critcal_services
|
||||
if observation_input_invalid[s] >= input_invalid_threshold[s]
|
||||
}
|
||||
inputs_diagnostics = {
|
||||
"inputs_valid": inputs_valid,
|
||||
"sensors_valid": sensors_valid,
|
||||
"filter_initialized": filter_initialized,
|
||||
"invalid_services": invalid_services,
|
||||
"over_threshold": over_threshold,
|
||||
}
|
||||
if not inputs_valid and inputs_diagnostics != last_inputs_diagnostics:
|
||||
cloudlog.event(
|
||||
"locationdInputsInvalid",
|
||||
error=True,
|
||||
**inputs_diagnostics,
|
||||
observation_input_invalid={s: round(float(observation_input_invalid[s]), 3) for s in critcal_services},
|
||||
observation_input_thresholds={s: float(input_invalid_threshold[s]) for s in critcal_services},
|
||||
)
|
||||
elif inputs_valid and last_inputs_diagnostics is not None:
|
||||
cloudlog.event("locationdInputsRecovered", **inputs_diagnostics)
|
||||
last_inputs_diagnostics = None if inputs_valid else inputs_diagnostics
|
||||
|
||||
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
|
||||
pm.send("livePose", msg)
|
||||
|
||||
|
||||
@@ -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_5406094264205448922) {
|
||||
out_5406094264205448922[0] = delta_x[0] + nom_x[0];
|
||||
out_5406094264205448922[1] = delta_x[1] + nom_x[1];
|
||||
out_5406094264205448922[2] = delta_x[2] + nom_x[2];
|
||||
out_5406094264205448922[3] = delta_x[3] + nom_x[3];
|
||||
out_5406094264205448922[4] = delta_x[4] + nom_x[4];
|
||||
out_5406094264205448922[5] = delta_x[5] + nom_x[5];
|
||||
out_5406094264205448922[6] = delta_x[6] + nom_x[6];
|
||||
out_5406094264205448922[7] = delta_x[7] + nom_x[7];
|
||||
out_5406094264205448922[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713) {
|
||||
out_1976458697034392713[0] = delta_x[0] + nom_x[0];
|
||||
out_1976458697034392713[1] = delta_x[1] + nom_x[1];
|
||||
out_1976458697034392713[2] = delta_x[2] + nom_x[2];
|
||||
out_1976458697034392713[3] = delta_x[3] + nom_x[3];
|
||||
out_1976458697034392713[4] = delta_x[4] + nom_x[4];
|
||||
out_1976458697034392713[5] = delta_x[5] + nom_x[5];
|
||||
out_1976458697034392713[6] = delta_x[6] + nom_x[6];
|
||||
out_1976458697034392713[7] = delta_x[7] + nom_x[7];
|
||||
out_1976458697034392713[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) {
|
||||
out_7302786751934147898[0] = -nom_x[0] + true_x[0];
|
||||
out_7302786751934147898[1] = -nom_x[1] + true_x[1];
|
||||
out_7302786751934147898[2] = -nom_x[2] + true_x[2];
|
||||
out_7302786751934147898[3] = -nom_x[3] + true_x[3];
|
||||
out_7302786751934147898[4] = -nom_x[4] + true_x[4];
|
||||
out_7302786751934147898[5] = -nom_x[5] + true_x[5];
|
||||
out_7302786751934147898[6] = -nom_x[6] + true_x[6];
|
||||
out_7302786751934147898[7] = -nom_x[7] + true_x[7];
|
||||
out_7302786751934147898[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588) {
|
||||
out_8774680380747325588[0] = -nom_x[0] + true_x[0];
|
||||
out_8774680380747325588[1] = -nom_x[1] + true_x[1];
|
||||
out_8774680380747325588[2] = -nom_x[2] + true_x[2];
|
||||
out_8774680380747325588[3] = -nom_x[3] + true_x[3];
|
||||
out_8774680380747325588[4] = -nom_x[4] + true_x[4];
|
||||
out_8774680380747325588[5] = -nom_x[5] + true_x[5];
|
||||
out_8774680380747325588[6] = -nom_x[6] + true_x[6];
|
||||
out_8774680380747325588[7] = -nom_x[7] + true_x[7];
|
||||
out_8774680380747325588[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_8564810489090289784) {
|
||||
out_8564810489090289784[0] = 1.0;
|
||||
out_8564810489090289784[1] = 0.0;
|
||||
out_8564810489090289784[2] = 0.0;
|
||||
out_8564810489090289784[3] = 0.0;
|
||||
out_8564810489090289784[4] = 0.0;
|
||||
out_8564810489090289784[5] = 0.0;
|
||||
out_8564810489090289784[6] = 0.0;
|
||||
out_8564810489090289784[7] = 0.0;
|
||||
out_8564810489090289784[8] = 0.0;
|
||||
out_8564810489090289784[9] = 0.0;
|
||||
out_8564810489090289784[10] = 1.0;
|
||||
out_8564810489090289784[11] = 0.0;
|
||||
out_8564810489090289784[12] = 0.0;
|
||||
out_8564810489090289784[13] = 0.0;
|
||||
out_8564810489090289784[14] = 0.0;
|
||||
out_8564810489090289784[15] = 0.0;
|
||||
out_8564810489090289784[16] = 0.0;
|
||||
out_8564810489090289784[17] = 0.0;
|
||||
out_8564810489090289784[18] = 0.0;
|
||||
out_8564810489090289784[19] = 0.0;
|
||||
out_8564810489090289784[20] = 1.0;
|
||||
out_8564810489090289784[21] = 0.0;
|
||||
out_8564810489090289784[22] = 0.0;
|
||||
out_8564810489090289784[23] = 0.0;
|
||||
out_8564810489090289784[24] = 0.0;
|
||||
out_8564810489090289784[25] = 0.0;
|
||||
out_8564810489090289784[26] = 0.0;
|
||||
out_8564810489090289784[27] = 0.0;
|
||||
out_8564810489090289784[28] = 0.0;
|
||||
out_8564810489090289784[29] = 0.0;
|
||||
out_8564810489090289784[30] = 1.0;
|
||||
out_8564810489090289784[31] = 0.0;
|
||||
out_8564810489090289784[32] = 0.0;
|
||||
out_8564810489090289784[33] = 0.0;
|
||||
out_8564810489090289784[34] = 0.0;
|
||||
out_8564810489090289784[35] = 0.0;
|
||||
out_8564810489090289784[36] = 0.0;
|
||||
out_8564810489090289784[37] = 0.0;
|
||||
out_8564810489090289784[38] = 0.0;
|
||||
out_8564810489090289784[39] = 0.0;
|
||||
out_8564810489090289784[40] = 1.0;
|
||||
out_8564810489090289784[41] = 0.0;
|
||||
out_8564810489090289784[42] = 0.0;
|
||||
out_8564810489090289784[43] = 0.0;
|
||||
out_8564810489090289784[44] = 0.0;
|
||||
out_8564810489090289784[45] = 0.0;
|
||||
out_8564810489090289784[46] = 0.0;
|
||||
out_8564810489090289784[47] = 0.0;
|
||||
out_8564810489090289784[48] = 0.0;
|
||||
out_8564810489090289784[49] = 0.0;
|
||||
out_8564810489090289784[50] = 1.0;
|
||||
out_8564810489090289784[51] = 0.0;
|
||||
out_8564810489090289784[52] = 0.0;
|
||||
out_8564810489090289784[53] = 0.0;
|
||||
out_8564810489090289784[54] = 0.0;
|
||||
out_8564810489090289784[55] = 0.0;
|
||||
out_8564810489090289784[56] = 0.0;
|
||||
out_8564810489090289784[57] = 0.0;
|
||||
out_8564810489090289784[58] = 0.0;
|
||||
out_8564810489090289784[59] = 0.0;
|
||||
out_8564810489090289784[60] = 1.0;
|
||||
out_8564810489090289784[61] = 0.0;
|
||||
out_8564810489090289784[62] = 0.0;
|
||||
out_8564810489090289784[63] = 0.0;
|
||||
out_8564810489090289784[64] = 0.0;
|
||||
out_8564810489090289784[65] = 0.0;
|
||||
out_8564810489090289784[66] = 0.0;
|
||||
out_8564810489090289784[67] = 0.0;
|
||||
out_8564810489090289784[68] = 0.0;
|
||||
out_8564810489090289784[69] = 0.0;
|
||||
out_8564810489090289784[70] = 1.0;
|
||||
out_8564810489090289784[71] = 0.0;
|
||||
out_8564810489090289784[72] = 0.0;
|
||||
out_8564810489090289784[73] = 0.0;
|
||||
out_8564810489090289784[74] = 0.0;
|
||||
out_8564810489090289784[75] = 0.0;
|
||||
out_8564810489090289784[76] = 0.0;
|
||||
out_8564810489090289784[77] = 0.0;
|
||||
out_8564810489090289784[78] = 0.0;
|
||||
out_8564810489090289784[79] = 0.0;
|
||||
out_8564810489090289784[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_1571472922248018296) {
|
||||
out_1571472922248018296[0] = 1.0;
|
||||
out_1571472922248018296[1] = 0.0;
|
||||
out_1571472922248018296[2] = 0.0;
|
||||
out_1571472922248018296[3] = 0.0;
|
||||
out_1571472922248018296[4] = 0.0;
|
||||
out_1571472922248018296[5] = 0.0;
|
||||
out_1571472922248018296[6] = 0.0;
|
||||
out_1571472922248018296[7] = 0.0;
|
||||
out_1571472922248018296[8] = 0.0;
|
||||
out_1571472922248018296[9] = 0.0;
|
||||
out_1571472922248018296[10] = 1.0;
|
||||
out_1571472922248018296[11] = 0.0;
|
||||
out_1571472922248018296[12] = 0.0;
|
||||
out_1571472922248018296[13] = 0.0;
|
||||
out_1571472922248018296[14] = 0.0;
|
||||
out_1571472922248018296[15] = 0.0;
|
||||
out_1571472922248018296[16] = 0.0;
|
||||
out_1571472922248018296[17] = 0.0;
|
||||
out_1571472922248018296[18] = 0.0;
|
||||
out_1571472922248018296[19] = 0.0;
|
||||
out_1571472922248018296[20] = 1.0;
|
||||
out_1571472922248018296[21] = 0.0;
|
||||
out_1571472922248018296[22] = 0.0;
|
||||
out_1571472922248018296[23] = 0.0;
|
||||
out_1571472922248018296[24] = 0.0;
|
||||
out_1571472922248018296[25] = 0.0;
|
||||
out_1571472922248018296[26] = 0.0;
|
||||
out_1571472922248018296[27] = 0.0;
|
||||
out_1571472922248018296[28] = 0.0;
|
||||
out_1571472922248018296[29] = 0.0;
|
||||
out_1571472922248018296[30] = 1.0;
|
||||
out_1571472922248018296[31] = 0.0;
|
||||
out_1571472922248018296[32] = 0.0;
|
||||
out_1571472922248018296[33] = 0.0;
|
||||
out_1571472922248018296[34] = 0.0;
|
||||
out_1571472922248018296[35] = 0.0;
|
||||
out_1571472922248018296[36] = 0.0;
|
||||
out_1571472922248018296[37] = 0.0;
|
||||
out_1571472922248018296[38] = 0.0;
|
||||
out_1571472922248018296[39] = 0.0;
|
||||
out_1571472922248018296[40] = 1.0;
|
||||
out_1571472922248018296[41] = 0.0;
|
||||
out_1571472922248018296[42] = 0.0;
|
||||
out_1571472922248018296[43] = 0.0;
|
||||
out_1571472922248018296[44] = 0.0;
|
||||
out_1571472922248018296[45] = 0.0;
|
||||
out_1571472922248018296[46] = 0.0;
|
||||
out_1571472922248018296[47] = 0.0;
|
||||
out_1571472922248018296[48] = 0.0;
|
||||
out_1571472922248018296[49] = 0.0;
|
||||
out_1571472922248018296[50] = 1.0;
|
||||
out_1571472922248018296[51] = 0.0;
|
||||
out_1571472922248018296[52] = 0.0;
|
||||
out_1571472922248018296[53] = 0.0;
|
||||
out_1571472922248018296[54] = 0.0;
|
||||
out_1571472922248018296[55] = 0.0;
|
||||
out_1571472922248018296[56] = 0.0;
|
||||
out_1571472922248018296[57] = 0.0;
|
||||
out_1571472922248018296[58] = 0.0;
|
||||
out_1571472922248018296[59] = 0.0;
|
||||
out_1571472922248018296[60] = 1.0;
|
||||
out_1571472922248018296[61] = 0.0;
|
||||
out_1571472922248018296[62] = 0.0;
|
||||
out_1571472922248018296[63] = 0.0;
|
||||
out_1571472922248018296[64] = 0.0;
|
||||
out_1571472922248018296[65] = 0.0;
|
||||
out_1571472922248018296[66] = 0.0;
|
||||
out_1571472922248018296[67] = 0.0;
|
||||
out_1571472922248018296[68] = 0.0;
|
||||
out_1571472922248018296[69] = 0.0;
|
||||
out_1571472922248018296[70] = 1.0;
|
||||
out_1571472922248018296[71] = 0.0;
|
||||
out_1571472922248018296[72] = 0.0;
|
||||
out_1571472922248018296[73] = 0.0;
|
||||
out_1571472922248018296[74] = 0.0;
|
||||
out_1571472922248018296[75] = 0.0;
|
||||
out_1571472922248018296[76] = 0.0;
|
||||
out_1571472922248018296[77] = 0.0;
|
||||
out_1571472922248018296[78] = 0.0;
|
||||
out_1571472922248018296[79] = 0.0;
|
||||
out_1571472922248018296[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_7344896958720977953) {
|
||||
out_7344896958720977953[0] = state[0];
|
||||
out_7344896958720977953[1] = state[1];
|
||||
out_7344896958720977953[2] = state[2];
|
||||
out_7344896958720977953[3] = state[3];
|
||||
out_7344896958720977953[4] = state[4];
|
||||
out_7344896958720977953[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_7344896958720977953[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_7344896958720977953[7] = state[7];
|
||||
out_7344896958720977953[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_3070664500164102760) {
|
||||
out_3070664500164102760[0] = state[0];
|
||||
out_3070664500164102760[1] = state[1];
|
||||
out_3070664500164102760[2] = state[2];
|
||||
out_3070664500164102760[3] = state[3];
|
||||
out_3070664500164102760[4] = state[4];
|
||||
out_3070664500164102760[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_3070664500164102760[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_3070664500164102760[7] = state[7];
|
||||
out_3070664500164102760[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_4577316669100378715) {
|
||||
out_4577316669100378715[0] = 1;
|
||||
out_4577316669100378715[1] = 0;
|
||||
out_4577316669100378715[2] = 0;
|
||||
out_4577316669100378715[3] = 0;
|
||||
out_4577316669100378715[4] = 0;
|
||||
out_4577316669100378715[5] = 0;
|
||||
out_4577316669100378715[6] = 0;
|
||||
out_4577316669100378715[7] = 0;
|
||||
out_4577316669100378715[8] = 0;
|
||||
out_4577316669100378715[9] = 0;
|
||||
out_4577316669100378715[10] = 1;
|
||||
out_4577316669100378715[11] = 0;
|
||||
out_4577316669100378715[12] = 0;
|
||||
out_4577316669100378715[13] = 0;
|
||||
out_4577316669100378715[14] = 0;
|
||||
out_4577316669100378715[15] = 0;
|
||||
out_4577316669100378715[16] = 0;
|
||||
out_4577316669100378715[17] = 0;
|
||||
out_4577316669100378715[18] = 0;
|
||||
out_4577316669100378715[19] = 0;
|
||||
out_4577316669100378715[20] = 1;
|
||||
out_4577316669100378715[21] = 0;
|
||||
out_4577316669100378715[22] = 0;
|
||||
out_4577316669100378715[23] = 0;
|
||||
out_4577316669100378715[24] = 0;
|
||||
out_4577316669100378715[25] = 0;
|
||||
out_4577316669100378715[26] = 0;
|
||||
out_4577316669100378715[27] = 0;
|
||||
out_4577316669100378715[28] = 0;
|
||||
out_4577316669100378715[29] = 0;
|
||||
out_4577316669100378715[30] = 1;
|
||||
out_4577316669100378715[31] = 0;
|
||||
out_4577316669100378715[32] = 0;
|
||||
out_4577316669100378715[33] = 0;
|
||||
out_4577316669100378715[34] = 0;
|
||||
out_4577316669100378715[35] = 0;
|
||||
out_4577316669100378715[36] = 0;
|
||||
out_4577316669100378715[37] = 0;
|
||||
out_4577316669100378715[38] = 0;
|
||||
out_4577316669100378715[39] = 0;
|
||||
out_4577316669100378715[40] = 1;
|
||||
out_4577316669100378715[41] = 0;
|
||||
out_4577316669100378715[42] = 0;
|
||||
out_4577316669100378715[43] = 0;
|
||||
out_4577316669100378715[44] = 0;
|
||||
out_4577316669100378715[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_4577316669100378715[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_4577316669100378715[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4577316669100378715[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4577316669100378715[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_4577316669100378715[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_4577316669100378715[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_4577316669100378715[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4577316669100378715[53] = -9.8100000000000005*dt;
|
||||
out_4577316669100378715[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_4577316669100378715[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_4577316669100378715[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4577316669100378715[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4577316669100378715[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_4577316669100378715[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_4577316669100378715[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_4577316669100378715[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4577316669100378715[62] = 0;
|
||||
out_4577316669100378715[63] = 0;
|
||||
out_4577316669100378715[64] = 0;
|
||||
out_4577316669100378715[65] = 0;
|
||||
out_4577316669100378715[66] = 0;
|
||||
out_4577316669100378715[67] = 0;
|
||||
out_4577316669100378715[68] = 0;
|
||||
out_4577316669100378715[69] = 0;
|
||||
out_4577316669100378715[70] = 1;
|
||||
out_4577316669100378715[71] = 0;
|
||||
out_4577316669100378715[72] = 0;
|
||||
out_4577316669100378715[73] = 0;
|
||||
out_4577316669100378715[74] = 0;
|
||||
out_4577316669100378715[75] = 0;
|
||||
out_4577316669100378715[76] = 0;
|
||||
out_4577316669100378715[77] = 0;
|
||||
out_4577316669100378715[78] = 0;
|
||||
out_4577316669100378715[79] = 0;
|
||||
out_4577316669100378715[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2365857006052744488) {
|
||||
out_2365857006052744488[0] = 1;
|
||||
out_2365857006052744488[1] = 0;
|
||||
out_2365857006052744488[2] = 0;
|
||||
out_2365857006052744488[3] = 0;
|
||||
out_2365857006052744488[4] = 0;
|
||||
out_2365857006052744488[5] = 0;
|
||||
out_2365857006052744488[6] = 0;
|
||||
out_2365857006052744488[7] = 0;
|
||||
out_2365857006052744488[8] = 0;
|
||||
out_2365857006052744488[9] = 0;
|
||||
out_2365857006052744488[10] = 1;
|
||||
out_2365857006052744488[11] = 0;
|
||||
out_2365857006052744488[12] = 0;
|
||||
out_2365857006052744488[13] = 0;
|
||||
out_2365857006052744488[14] = 0;
|
||||
out_2365857006052744488[15] = 0;
|
||||
out_2365857006052744488[16] = 0;
|
||||
out_2365857006052744488[17] = 0;
|
||||
out_2365857006052744488[18] = 0;
|
||||
out_2365857006052744488[19] = 0;
|
||||
out_2365857006052744488[20] = 1;
|
||||
out_2365857006052744488[21] = 0;
|
||||
out_2365857006052744488[22] = 0;
|
||||
out_2365857006052744488[23] = 0;
|
||||
out_2365857006052744488[24] = 0;
|
||||
out_2365857006052744488[25] = 0;
|
||||
out_2365857006052744488[26] = 0;
|
||||
out_2365857006052744488[27] = 0;
|
||||
out_2365857006052744488[28] = 0;
|
||||
out_2365857006052744488[29] = 0;
|
||||
out_2365857006052744488[30] = 1;
|
||||
out_2365857006052744488[31] = 0;
|
||||
out_2365857006052744488[32] = 0;
|
||||
out_2365857006052744488[33] = 0;
|
||||
out_2365857006052744488[34] = 0;
|
||||
out_2365857006052744488[35] = 0;
|
||||
out_2365857006052744488[36] = 0;
|
||||
out_2365857006052744488[37] = 0;
|
||||
out_2365857006052744488[38] = 0;
|
||||
out_2365857006052744488[39] = 0;
|
||||
out_2365857006052744488[40] = 1;
|
||||
out_2365857006052744488[41] = 0;
|
||||
out_2365857006052744488[42] = 0;
|
||||
out_2365857006052744488[43] = 0;
|
||||
out_2365857006052744488[44] = 0;
|
||||
out_2365857006052744488[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_2365857006052744488[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2365857006052744488[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2365857006052744488[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2365857006052744488[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_2365857006052744488[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2365857006052744488[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2365857006052744488[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2365857006052744488[53] = -9.8100000000000005*dt;
|
||||
out_2365857006052744488[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_2365857006052744488[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2365857006052744488[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2365857006052744488[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2365857006052744488[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_2365857006052744488[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2365857006052744488[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_2365857006052744488[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2365857006052744488[62] = 0;
|
||||
out_2365857006052744488[63] = 0;
|
||||
out_2365857006052744488[64] = 0;
|
||||
out_2365857006052744488[65] = 0;
|
||||
out_2365857006052744488[66] = 0;
|
||||
out_2365857006052744488[67] = 0;
|
||||
out_2365857006052744488[68] = 0;
|
||||
out_2365857006052744488[69] = 0;
|
||||
out_2365857006052744488[70] = 1;
|
||||
out_2365857006052744488[71] = 0;
|
||||
out_2365857006052744488[72] = 0;
|
||||
out_2365857006052744488[73] = 0;
|
||||
out_2365857006052744488[74] = 0;
|
||||
out_2365857006052744488[75] = 0;
|
||||
out_2365857006052744488[76] = 0;
|
||||
out_2365857006052744488[77] = 0;
|
||||
out_2365857006052744488[78] = 0;
|
||||
out_2365857006052744488[79] = 0;
|
||||
out_2365857006052744488[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_6986192086353969465) {
|
||||
out_6986192086353969465[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_7208997417211176832) {
|
||||
out_7208997417211176832[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_628377736821947453) {
|
||||
out_628377736821947453[0] = 0;
|
||||
out_628377736821947453[1] = 0;
|
||||
out_628377736821947453[2] = 0;
|
||||
out_628377736821947453[3] = 0;
|
||||
out_628377736821947453[4] = 0;
|
||||
out_628377736821947453[5] = 0;
|
||||
out_628377736821947453[6] = 1;
|
||||
out_628377736821947453[7] = 0;
|
||||
out_628377736821947453[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_3987544096248591949) {
|
||||
out_3987544096248591949[0] = 0;
|
||||
out_3987544096248591949[1] = 0;
|
||||
out_3987544096248591949[2] = 0;
|
||||
out_3987544096248591949[3] = 0;
|
||||
out_3987544096248591949[4] = 0;
|
||||
out_3987544096248591949[5] = 0;
|
||||
out_3987544096248591949[6] = 1;
|
||||
out_3987544096248591949[7] = 0;
|
||||
out_3987544096248591949[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_434665502247836674) {
|
||||
out_434665502247836674[0] = state[4];
|
||||
out_434665502247836674[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_7481715524482624299) {
|
||||
out_7481715524482624299[0] = state[4];
|
||||
out_7481715524482624299[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_2801027335827447019) {
|
||||
out_2801027335827447019[0] = 0;
|
||||
out_2801027335827447019[1] = 0;
|
||||
out_2801027335827447019[2] = 0;
|
||||
out_2801027335827447019[3] = 0;
|
||||
out_2801027335827447019[4] = 1;
|
||||
out_2801027335827447019[5] = 0;
|
||||
out_2801027335827447019[6] = 0;
|
||||
out_2801027335827447019[7] = 0;
|
||||
out_2801027335827447019[8] = 0;
|
||||
out_2801027335827447019[9] = 0;
|
||||
out_2801027335827447019[10] = 0;
|
||||
out_2801027335827447019[11] = 0;
|
||||
out_2801027335827447019[12] = 0;
|
||||
out_2801027335827447019[13] = 0;
|
||||
out_2801027335827447019[14] = 1;
|
||||
out_2801027335827447019[15] = 0;
|
||||
out_2801027335827447019[16] = 0;
|
||||
out_2801027335827447019[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_1810329672641441976) {
|
||||
out_1810329672641441976[0] = 0;
|
||||
out_1810329672641441976[1] = 0;
|
||||
out_1810329672641441976[2] = 0;
|
||||
out_1810329672641441976[3] = 0;
|
||||
out_1810329672641441976[4] = 1;
|
||||
out_1810329672641441976[5] = 0;
|
||||
out_1810329672641441976[6] = 0;
|
||||
out_1810329672641441976[7] = 0;
|
||||
out_1810329672641441976[8] = 0;
|
||||
out_1810329672641441976[9] = 0;
|
||||
out_1810329672641441976[10] = 0;
|
||||
out_1810329672641441976[11] = 0;
|
||||
out_1810329672641441976[12] = 0;
|
||||
out_1810329672641441976[13] = 0;
|
||||
out_1810329672641441976[14] = 1;
|
||||
out_1810329672641441976[15] = 0;
|
||||
out_1810329672641441976[16] = 0;
|
||||
out_1810329672641441976[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_4915078315896353947) {
|
||||
out_4915078315896353947[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_438162190860825896) {
|
||||
out_438162190860825896[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6288312604669669302) {
|
||||
out_6288312604669669302[0] = 0;
|
||||
out_6288312604669669302[1] = 0;
|
||||
out_6288312604669669302[2] = 0;
|
||||
out_6288312604669669302[3] = 0;
|
||||
out_6288312604669669302[4] = 1;
|
||||
out_6288312604669669302[5] = 0;
|
||||
out_6288312604669669302[6] = 0;
|
||||
out_6288312604669669302[7] = 0;
|
||||
out_6288312604669669302[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_1469211137741343322) {
|
||||
out_1469211137741343322[0] = 0;
|
||||
out_1469211137741343322[1] = 0;
|
||||
out_1469211137741343322[2] = 0;
|
||||
out_1469211137741343322[3] = 0;
|
||||
out_1469211137741343322[4] = 1;
|
||||
out_1469211137741343322[5] = 0;
|
||||
out_1469211137741343322[6] = 0;
|
||||
out_1469211137741343322[7] = 0;
|
||||
out_1469211137741343322[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_4943503076008356261) {
|
||||
out_4943503076008356261[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4563511076800019749) {
|
||||
out_4563511076800019749[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_4369881055696003677) {
|
||||
out_4369881055696003677[0] = 0;
|
||||
out_4369881055696003677[1] = 0;
|
||||
out_4369881055696003677[2] = 0;
|
||||
out_4369881055696003677[3] = 0;
|
||||
out_4369881055696003677[4] = 0;
|
||||
out_4369881055696003677[5] = 0;
|
||||
out_4369881055696003677[6] = 0;
|
||||
out_4369881055696003677[7] = 1;
|
||||
out_4369881055696003677[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_7729047415122648173) {
|
||||
out_7729047415122648173[0] = 0;
|
||||
out_7729047415122648173[1] = 0;
|
||||
out_7729047415122648173[2] = 0;
|
||||
out_7729047415122648173[3] = 0;
|
||||
out_7729047415122648173[4] = 0;
|
||||
out_7729047415122648173[5] = 0;
|
||||
out_7729047415122648173[6] = 0;
|
||||
out_7729047415122648173[7] = 1;
|
||||
out_7729047415122648173[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_1984518818693123820) {
|
||||
out_1984518818693123820[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_6265487564309514397) {
|
||||
out_6265487564309514397[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_2932479995765612434) {
|
||||
out_2932479995765612434[0] = 0;
|
||||
out_2932479995765612434[1] = 0;
|
||||
out_2932479995765612434[2] = 0;
|
||||
out_2932479995765612434[3] = 1;
|
||||
out_2932479995765612434[4] = 0;
|
||||
out_2932479995765612434[5] = 0;
|
||||
out_2932479995765612434[6] = 0;
|
||||
out_2932479995765612434[7] = 0;
|
||||
out_2932479995765612434[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_3643974449541768233) {
|
||||
out_3643974449541768233[0] = 0;
|
||||
out_3643974449541768233[1] = 0;
|
||||
out_3643974449541768233[2] = 0;
|
||||
out_3643974449541768233[3] = 1;
|
||||
out_3643974449541768233[4] = 0;
|
||||
out_3643974449541768233[5] = 0;
|
||||
out_3643974449541768233[6] = 0;
|
||||
out_3643974449541768233[7] = 0;
|
||||
out_3643974449541768233[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_963207171662361141) {
|
||||
out_963207171662361141[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_6316447678419540984) {
|
||||
out_6316447678419540984[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_4645842722635163467) {
|
||||
out_4645842722635163467[0] = 0;
|
||||
out_4645842722635163467[1] = 1;
|
||||
out_4645842722635163467[2] = 0;
|
||||
out_4645842722635163467[3] = 0;
|
||||
out_4645842722635163467[4] = 0;
|
||||
out_4645842722635163467[5] = 0;
|
||||
out_4645842722635163467[6] = 0;
|
||||
out_4645842722635163467[7] = 0;
|
||||
out_4645842722635163467[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_958979793426951138) {
|
||||
out_958979793426951138[0] = 0;
|
||||
out_958979793426951138[1] = 1;
|
||||
out_958979793426951138[2] = 0;
|
||||
out_958979793426951138[3] = 0;
|
||||
out_958979793426951138[4] = 0;
|
||||
out_958979793426951138[5] = 0;
|
||||
out_958979793426951138[6] = 0;
|
||||
out_958979793426951138[7] = 0;
|
||||
out_958979793426951138[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_1002434725357825708) {
|
||||
out_1002434725357825708[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_3384947045134040079) {
|
||||
out_3384947045134040079[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_2682212451069837216) {
|
||||
out_2682212451069837216[0] = 1;
|
||||
out_2682212451069837216[1] = 0;
|
||||
out_2682212451069837216[2] = 0;
|
||||
out_2682212451069837216[3] = 0;
|
||||
out_2682212451069837216[4] = 0;
|
||||
out_2682212451069837216[5] = 0;
|
||||
out_2682212451069837216[6] = 0;
|
||||
out_2682212451069837216[7] = 0;
|
||||
out_2682212451069837216[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_6041378810496481712) {
|
||||
out_6041378810496481712[0] = 1;
|
||||
out_6041378810496481712[1] = 0;
|
||||
out_6041378810496481712[2] = 0;
|
||||
out_6041378810496481712[3] = 0;
|
||||
out_6041378810496481712[4] = 0;
|
||||
out_6041378810496481712[5] = 0;
|
||||
out_6041378810496481712[6] = 0;
|
||||
out_6041378810496481712[7] = 0;
|
||||
out_6041378810496481712[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_7261386148638475354) {
|
||||
out_7261386148638475354[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_2597869179779832900) {
|
||||
out_2597869179779832900[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_597731774944987025) {
|
||||
out_597731774944987025[0] = 0;
|
||||
out_597731774944987025[1] = 0;
|
||||
out_597731774944987025[2] = 0;
|
||||
out_597731774944987025[3] = 0;
|
||||
out_597731774944987025[4] = 0;
|
||||
out_597731774944987025[5] = 0;
|
||||
out_597731774944987025[6] = 0;
|
||||
out_597731774944987025[7] = 0;
|
||||
out_597731774944987025[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_8355255517355999649) {
|
||||
out_8355255517355999649[0] = 0;
|
||||
out_8355255517355999649[1] = 0;
|
||||
out_8355255517355999649[2] = 0;
|
||||
out_8355255517355999649[3] = 0;
|
||||
out_8355255517355999649[4] = 0;
|
||||
out_8355255517355999649[5] = 0;
|
||||
out_8355255517355999649[6] = 0;
|
||||
out_8355255517355999649[7] = 0;
|
||||
out_8355255517355999649[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_5406094264205448922) {
|
||||
err_fun(nom_x, delta_x, out_5406094264205448922);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713) {
|
||||
err_fun(nom_x, delta_x, out_1976458697034392713);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) {
|
||||
inv_err_fun(nom_x, true_x, out_7302786751934147898);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588) {
|
||||
inv_err_fun(nom_x, true_x, out_8774680380747325588);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_8564810489090289784) {
|
||||
H_mod_fun(state, out_8564810489090289784);
|
||||
void car_H_mod_fun(double *state, double *out_1571472922248018296) {
|
||||
H_mod_fun(state, out_1571472922248018296);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_7344896958720977953) {
|
||||
f_fun(state, dt, out_7344896958720977953);
|
||||
void car_f_fun(double *state, double dt, double *out_3070664500164102760) {
|
||||
f_fun(state, dt, out_3070664500164102760);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_4577316669100378715) {
|
||||
F_fun(state, dt, out_4577316669100378715);
|
||||
void car_F_fun(double *state, double dt, double *out_2365857006052744488) {
|
||||
F_fun(state, dt, out_2365857006052744488);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_6986192086353969465) {
|
||||
h_25(state, unused, out_6986192086353969465);
|
||||
void car_h_25(double *state, double *unused, double *out_7208997417211176832) {
|
||||
h_25(state, unused, out_7208997417211176832);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_628377736821947453) {
|
||||
H_25(state, unused, out_628377736821947453);
|
||||
void car_H_25(double *state, double *unused, double *out_3987544096248591949) {
|
||||
H_25(state, unused, out_3987544096248591949);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_434665502247836674) {
|
||||
h_24(state, unused, out_434665502247836674);
|
||||
void car_h_24(double *state, double *unused, double *out_7481715524482624299) {
|
||||
h_24(state, unused, out_7481715524482624299);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_2801027335827447019) {
|
||||
H_24(state, unused, out_2801027335827447019);
|
||||
void car_H_24(double *state, double *unused, double *out_1810329672641441976) {
|
||||
H_24(state, unused, out_1810329672641441976);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_4915078315896353947) {
|
||||
h_30(state, unused, out_4915078315896353947);
|
||||
void car_h_30(double *state, double *unused, double *out_438162190860825896) {
|
||||
h_30(state, unused, out_438162190860825896);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6288312604669669302) {
|
||||
H_30(state, unused, out_6288312604669669302);
|
||||
void car_H_30(double *state, double *unused, double *out_1469211137741343322) {
|
||||
H_30(state, unused, out_1469211137741343322);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_4943503076008356261) {
|
||||
h_26(state, unused, out_4943503076008356261);
|
||||
void car_h_26(double *state, double *unused, double *out_4563511076800019749) {
|
||||
h_26(state, unused, out_4563511076800019749);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_4369881055696003677) {
|
||||
H_26(state, unused, out_4369881055696003677);
|
||||
void car_H_26(double *state, double *unused, double *out_7729047415122648173) {
|
||||
H_26(state, unused, out_7729047415122648173);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_1984518818693123820) {
|
||||
h_27(state, unused, out_1984518818693123820);
|
||||
void car_h_27(double *state, double *unused, double *out_6265487564309514397) {
|
||||
h_27(state, unused, out_6265487564309514397);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_2932479995765612434) {
|
||||
H_27(state, unused, out_2932479995765612434);
|
||||
void car_H_27(double *state, double *unused, double *out_3643974449541768233) {
|
||||
H_27(state, unused, out_3643974449541768233);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_963207171662361141) {
|
||||
h_29(state, unused, out_963207171662361141);
|
||||
void car_h_29(double *state, double *unused, double *out_6316447678419540984) {
|
||||
h_29(state, unused, out_6316447678419540984);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_4645842722635163467) {
|
||||
H_29(state, unused, out_4645842722635163467);
|
||||
void car_H_29(double *state, double *unused, double *out_958979793426951138) {
|
||||
H_29(state, unused, out_958979793426951138);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_1002434725357825708) {
|
||||
h_28(state, unused, out_1002434725357825708);
|
||||
void car_h_28(double *state, double *unused, double *out_3384947045134040079) {
|
||||
h_28(state, unused, out_3384947045134040079);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_2682212451069837216) {
|
||||
H_28(state, unused, out_2682212451069837216);
|
||||
void car_H_28(double *state, double *unused, double *out_6041378810496481712) {
|
||||
H_28(state, unused, out_6041378810496481712);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_7261386148638475354) {
|
||||
h_31(state, unused, out_7261386148638475354);
|
||||
void car_h_31(double *state, double *unused, double *out_2597869179779832900) {
|
||||
h_31(state, unused, out_2597869179779832900);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_597731774944987025) {
|
||||
H_31(state, unused, out_597731774944987025);
|
||||
void car_H_31(double *state, double *unused, double *out_8355255517355999649) {
|
||||
H_31(state, unused, out_8355255517355999649);
|
||||
}
|
||||
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_5406094264205448922);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898);
|
||||
void car_H_mod_fun(double *state, double *out_8564810489090289784);
|
||||
void car_f_fun(double *state, double dt, double *out_7344896958720977953);
|
||||
void car_F_fun(double *state, double dt, double *out_4577316669100378715);
|
||||
void car_h_25(double *state, double *unused, double *out_6986192086353969465);
|
||||
void car_H_25(double *state, double *unused, double *out_628377736821947453);
|
||||
void car_h_24(double *state, double *unused, double *out_434665502247836674);
|
||||
void car_H_24(double *state, double *unused, double *out_2801027335827447019);
|
||||
void car_h_30(double *state, double *unused, double *out_4915078315896353947);
|
||||
void car_H_30(double *state, double *unused, double *out_6288312604669669302);
|
||||
void car_h_26(double *state, double *unused, double *out_4943503076008356261);
|
||||
void car_H_26(double *state, double *unused, double *out_4369881055696003677);
|
||||
void car_h_27(double *state, double *unused, double *out_1984518818693123820);
|
||||
void car_H_27(double *state, double *unused, double *out_2932479995765612434);
|
||||
void car_h_29(double *state, double *unused, double *out_963207171662361141);
|
||||
void car_H_29(double *state, double *unused, double *out_4645842722635163467);
|
||||
void car_h_28(double *state, double *unused, double *out_1002434725357825708);
|
||||
void car_H_28(double *state, double *unused, double *out_2682212451069837216);
|
||||
void car_h_31(double *state, double *unused, double *out_7261386148638475354);
|
||||
void car_H_31(double *state, double *unused, double *out_597731774944987025);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588);
|
||||
void car_H_mod_fun(double *state, double *out_1571472922248018296);
|
||||
void car_f_fun(double *state, double dt, double *out_3070664500164102760);
|
||||
void car_F_fun(double *state, double dt, double *out_2365857006052744488);
|
||||
void car_h_25(double *state, double *unused, double *out_7208997417211176832);
|
||||
void car_H_25(double *state, double *unused, double *out_3987544096248591949);
|
||||
void car_h_24(double *state, double *unused, double *out_7481715524482624299);
|
||||
void car_H_24(double *state, double *unused, double *out_1810329672641441976);
|
||||
void car_h_30(double *state, double *unused, double *out_438162190860825896);
|
||||
void car_H_30(double *state, double *unused, double *out_1469211137741343322);
|
||||
void car_h_26(double *state, double *unused, double *out_4563511076800019749);
|
||||
void car_H_26(double *state, double *unused, double *out_7729047415122648173);
|
||||
void car_h_27(double *state, double *unused, double *out_6265487564309514397);
|
||||
void car_H_27(double *state, double *unused, double *out_3643974449541768233);
|
||||
void car_h_29(double *state, double *unused, double *out_6316447678419540984);
|
||||
void car_H_29(double *state, double *unused, double *out_958979793426951138);
|
||||
void car_h_28(double *state, double *unused, double *out_3384947045134040079);
|
||||
void car_H_28(double *state, double *unused, double *out_6041378810496481712);
|
||||
void car_h_31(double *state, double *unused, double *out_2597869179779832900);
|
||||
void car_H_31(double *state, double *unused, double *out_8355255517355999649);
|
||||
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_6497612872484174174);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204);
|
||||
void pose_H_mod_fun(double *state, double *out_4617418789543565628);
|
||||
void pose_f_fun(double *state, double dt, double *out_9094503671864348345);
|
||||
void pose_F_fun(double *state, double dt, double *out_1312959600226932432);
|
||||
void pose_h_4(double *state, double *unused, double *out_6400948089178504221);
|
||||
void pose_H_4(double *state, double *unused, double *out_3419471307660667524);
|
||||
void pose_h_10(double *state, double *unused, double *out_8540054287512262719);
|
||||
void pose_H_10(double *state, double *unused, double *out_5231659216258162963);
|
||||
void pose_h_13(double *state, double *unused, double *out_7953441679753433349);
|
||||
void pose_H_13(double *state, double *unused, double *out_207197482328334723);
|
||||
void pose_h_14(double *state, double *unused, double *out_8785755176393893631);
|
||||
void pose_H_14(double *state, double *unused, double *out_543769548678817005);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_812796242528121648);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2630998712382937796);
|
||||
void pose_H_mod_fun(double *state, double *out_8952125250503643232);
|
||||
void pose_f_fun(double *state, double dt, double *out_3519678704568830866);
|
||||
void pose_F_fun(double *state, double dt, double *out_6750093296288889328);
|
||||
void pose_h_4(double *state, double *unused, double *out_1078897140345555822);
|
||||
void pose_H_4(double *state, double *unused, double *out_850583411531230682);
|
||||
void pose_h_10(double *state, double *unused, double *out_3641708848879017741);
|
||||
void pose_H_10(double *state, double *unused, double *out_7494820718027871329);
|
||||
void pose_h_13(double *state, double *unused, double *out_515642654693887905);
|
||||
void pose_H_13(double *state, double *unused, double *out_4062857236863563483);
|
||||
void pose_h_14(double *state, double *unused, double *out_1617663395634793849);
|
||||
void pose_H_14(double *state, double *unused, double *out_4813824267870715211);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -125,6 +125,7 @@ class SelfdriveD:
|
||||
self.last_functional_fan_frame = 0
|
||||
self.events_prev = []
|
||||
self.logged_comm_issue = None
|
||||
self.logged_locationd_issue = None
|
||||
self.not_running_prev = None
|
||||
self.experimental_mode = False
|
||||
self.safe_mode = self.params.get_bool("SafeMode")
|
||||
@@ -161,7 +162,6 @@ class SelfdriveD:
|
||||
|
||||
self.cancel_pressed_previously = False
|
||||
self.distance_pressed_previously = False
|
||||
|
||||
self.display_timer = 0
|
||||
self.last_below_steer_speed_alert_time = -float("inf")
|
||||
self.last_steer_saturated_alert_time = -float("inf")
|
||||
@@ -172,6 +172,22 @@ class SelfdriveD:
|
||||
|
||||
self.FPCP = messaging.log_from_bytes(self.params.get("StarPilotCarParams", block=True), custom.StarPilotCarParams)
|
||||
|
||||
def _service_diagnostics(self, services: list[str]) -> dict[str, dict[str, object]]:
|
||||
diagnostics: dict[str, dict[str, object]] = {}
|
||||
for service in services:
|
||||
if service not in self.sm.services:
|
||||
continue
|
||||
|
||||
recv_age_ms = round(max(0.0, (self.sm.frame - self.sm.recv_frame[service]) * DT_CTRL * 1000.0), 1)
|
||||
diagnostics[service] = {
|
||||
"valid": self.sm.valid[service],
|
||||
"alive": self.sm.alive[service],
|
||||
"freq_ok": self.sm.freq_ok[service],
|
||||
"recv_age_ms": recv_age_ms,
|
||||
}
|
||||
|
||||
return diagnostics
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute onroadEvents from carState"""
|
||||
|
||||
@@ -434,7 +450,22 @@ class SelfdriveD:
|
||||
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
|
||||
}
|
||||
if logs != self.logged_comm_issue:
|
||||
cloudlog.event("commIssue", error=True, **logs)
|
||||
interesting_services = sorted(set(
|
||||
logs['invalid'] +
|
||||
logs['not_alive'] +
|
||||
logs['not_freq_ok'] +
|
||||
['liveCalibration', 'livePose', 'liveParameters', 'radarState', 'driverAssistance', 'longitudinalPlan']
|
||||
))
|
||||
cloudlog.event(
|
||||
"commIssue",
|
||||
error=True,
|
||||
**logs,
|
||||
service_diagnostics=self._service_diagnostics(interesting_services),
|
||||
nav_destination_active=self.params.get("NavDestination") is not None,
|
||||
navigation_ui=self.params.get_bool("NavigationUI"),
|
||||
can_timeout=CS.canTimeout,
|
||||
can_valid=CS.canValid,
|
||||
)
|
||||
self.logged_comm_issue = logs
|
||||
else:
|
||||
self.logged_comm_issue = None
|
||||
@@ -444,6 +475,18 @@ class SelfdriveD:
|
||||
self.events.add(EventName.posenetInvalid)
|
||||
if not self.sm['livePose'].inputsOK:
|
||||
self.events.add(EventName.locationdTemporaryError)
|
||||
locationd_issue = {
|
||||
"inputs_ok": self.sm['livePose'].inputsOK,
|
||||
"posenet_ok": self.sm['livePose'].posenetOK,
|
||||
"live_pose_valid": self.sm.valid['livePose'],
|
||||
"live_calibration_valid": self.sm.valid['liveCalibration'],
|
||||
"live_parameters_valid": self.sm.valid['liveParameters'],
|
||||
}
|
||||
if locationd_issue != self.logged_locationd_issue:
|
||||
cloudlog.event("locationdTemporaryError", error=True, **locationd_issue)
|
||||
self.logged_locationd_issue = locationd_issue
|
||||
else:
|
||||
self.logged_locationd_issue = None
|
||||
if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
|
||||
self.events.add(EventName.paramsdTemporaryError)
|
||||
|
||||
|
||||
@@ -242,6 +242,11 @@ class StarPilotLateralLayout(_SettingsPage):
|
||||
get_state=lambda: self._params.get_bool("TurnDesires"),
|
||||
set_state=lambda s: self._params.put_bool("TurnDesires", s),
|
||||
visible=lambda: self._params.get_bool("LateralTune")),
|
||||
SettingRow("NavDesiresAllowed", "toggle", tr_noop("Use Route Desires"),
|
||||
subtitle=tr_noop("Allow an active navigation route to request keep-left, keep-right, and low-speed turn desires."),
|
||||
get_state=lambda: self._params.get_bool("NavDesiresAllowed"),
|
||||
set_state=lambda s: self._params.put_bool("NavDesiresAllowed", s),
|
||||
visible=lambda: self._params.get_bool("LateralTune")),
|
||||
SettingRow("NNFF", "toggle", tr_noop("Neural Network Feedforward (NNFF)"),
|
||||
subtitle=tr_noop("Use the full neural-network feedforward steering controller when available."),
|
||||
get_state=lambda: self._params.get_bool("NNFF"),
|
||||
|
||||
@@ -3,6 +3,7 @@ from dataclasses import dataclass
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar
|
||||
from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
@@ -139,6 +140,7 @@ class HudRenderer(Widget):
|
||||
|
||||
self._turn_intent = TurnIntent()
|
||||
self._torque_bar = TorqueBar()
|
||||
self._navigation_card = NavigationCardRenderer("mici")
|
||||
|
||||
self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50)
|
||||
self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50)
|
||||
@@ -254,6 +256,7 @@ class HudRenderer(Widget):
|
||||
if self.is_cruise_set:
|
||||
self._draw_set_speed(self._rect)
|
||||
|
||||
self._navigation_card.render(self._rect)
|
||||
self._draw_steering_wheel(self._rect)
|
||||
self._draw_speed_limit_prompt(self._rect)
|
||||
|
||||
|
||||
@@ -2,6 +2,7 @@ import pyray as rl
|
||||
from dataclasses import dataclass
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.ui.onroad.exp_button import ExpButton
|
||||
from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus
|
||||
from openpilot.system.ui.lib.application import gui_app, FontWeight
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
@@ -71,6 +72,7 @@ class HudRenderer(Widget):
|
||||
self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
|
||||
|
||||
self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size)
|
||||
self._navigation_card = NavigationCardRenderer()
|
||||
|
||||
def _update_state(self) -> None:
|
||||
"""Update HUD state based on car state and controls state."""
|
||||
@@ -119,6 +121,8 @@ class HudRenderer(Widget):
|
||||
if not ui_state.starpilot_toggles.get("hide_speed", False):
|
||||
self._draw_current_speed(rect)
|
||||
|
||||
self._navigation_card.render(rect)
|
||||
|
||||
button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size
|
||||
button_y = rect.y + UI_CONFIG.border_size
|
||||
self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size))
|
||||
|
||||
@@ -0,0 +1,384 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import pyray as rl
|
||||
|
||||
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
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
ASSETS_PATH = Path(__file__).resolve().parents[4] / "starpilot" / "assets" / "navigation"
|
||||
FALLBACK_ICON = "direction_turn_straight.png"
|
||||
|
||||
|
||||
def _format_distance(distance_m: float, is_metric: bool) -> str:
|
||||
if is_metric:
|
||||
if distance_m < 1000:
|
||||
step = 25 if distance_m < 500 else 50
|
||||
return f"{round(distance_m / step) * step} m"
|
||||
distance_km = distance_m / 1000.0
|
||||
return f"{math.floor(distance_km)} km" if distance_km >= 10.0 else f"{distance_km:.1f} km"
|
||||
|
||||
distance_ft = distance_m * 3.28084
|
||||
if distance_ft < 1000:
|
||||
step = 10 if distance_ft <= 100 else 50
|
||||
return f"{round(distance_ft / step) * step} ft"
|
||||
distance_mi = distance_ft / 5280.0
|
||||
return f"{math.floor(distance_mi)} mi" if distance_mi >= 10.0 else f"{distance_mi:.1f} mi"
|
||||
|
||||
|
||||
def _modifier_suffix(modifier: str) -> str:
|
||||
suffix_map = {
|
||||
"left": "left",
|
||||
"right": "right",
|
||||
"straight": "straight",
|
||||
"slightLeft": "slight_left",
|
||||
"slightRight": "slight_right",
|
||||
"sharpLeft": "sharp_left",
|
||||
"sharpRight": "sharp_right",
|
||||
}
|
||||
return suffix_map.get(modifier, "")
|
||||
|
||||
|
||||
def _normalize_maneuver_type(maneuver_type: str) -> str:
|
||||
normalized = maneuver_type or "turn"
|
||||
if normalized == "rotary":
|
||||
normalized = "roundabout"
|
||||
elif normalized in ("new name", "continue"):
|
||||
normalized = "turn"
|
||||
return normalized.replace(" ", "_")
|
||||
|
||||
|
||||
class NavigationCardRenderer(Widget):
|
||||
def __init__(self, layout_variant: str = "default"):
|
||||
super().__init__()
|
||||
self._font_bold = gui_app.font(FontWeight.BOLD)
|
||||
self._font_medium = gui_app.font(FontWeight.MEDIUM)
|
||||
self._icons: dict[str, rl.Texture2D] = {}
|
||||
self._layout_variant = layout_variant
|
||||
|
||||
self._enabled = False
|
||||
self._valid = False
|
||||
self._distance = ""
|
||||
self._primary_text = ""
|
||||
self._secondary_text = ""
|
||||
self._maneuver_type = "turn"
|
||||
self._modifier = "straight"
|
||||
self._has_next = False
|
||||
self._next_maneuver_type = "turn"
|
||||
self._next_modifier = "straight"
|
||||
|
||||
def _icon_filename(self, maneuver_type: str, modifier: str) -> str:
|
||||
normalized_type = _normalize_maneuver_type(maneuver_type)
|
||||
if modifier == "uturn":
|
||||
return "direction_uturn.png"
|
||||
|
||||
suffix = _modifier_suffix(modifier)
|
||||
candidate = f"direction_{normalized_type}.png" if suffix == "" else f"direction_{normalized_type}_{suffix}.png"
|
||||
return candidate if (ASSETS_PATH / candidate).exists() else FALLBACK_ICON
|
||||
|
||||
def _get_icon(self, maneuver_type: str, modifier: str):
|
||||
icon_name = self._icon_filename(maneuver_type, modifier)
|
||||
if icon_name not in self._icons:
|
||||
icon_path = ASSETS_PATH / icon_name
|
||||
if not icon_path.exists():
|
||||
icon_path = ASSETS_PATH / FALLBACK_ICON
|
||||
image = rl.load_image(str(icon_path))
|
||||
texture = rl.load_texture_from_image(image)
|
||||
rl.set_texture_filter(texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR)
|
||||
rl.set_texture_wrap(texture, rl.TextureWrap.TEXTURE_WRAP_CLAMP)
|
||||
rl.unload_image(image)
|
||||
self._icons[icon_name] = texture
|
||||
return self._icons[icon_name]
|
||||
|
||||
def _update_state(self) -> None:
|
||||
sm = ui_state.sm
|
||||
self._enabled = ui_state.params.get_bool("NavigationUI")
|
||||
self._valid = False
|
||||
|
||||
if not self._enabled:
|
||||
return
|
||||
if sm.recv_frame["navInstruction"] < ui_state.started_frame:
|
||||
return
|
||||
if not sm.valid.get("navInstruction", False):
|
||||
return
|
||||
|
||||
nav_instruction = sm["navInstruction"]
|
||||
if nav_instruction.maneuverPrimaryText == "":
|
||||
return
|
||||
|
||||
self._distance = _format_distance(float(nav_instruction.maneuverDistance), ui_state.is_metric)
|
||||
self._primary_text = nav_instruction.maneuverPrimaryText
|
||||
self._secondary_text = nav_instruction.maneuverSecondaryText
|
||||
self._maneuver_type = nav_instruction.maneuverType or "turn"
|
||||
self._modifier = nav_instruction.maneuverModifier or "straight"
|
||||
|
||||
maneuvers = nav_instruction.allManeuvers
|
||||
if len(maneuvers) > 1:
|
||||
self._has_next = True
|
||||
self._next_maneuver_type = maneuvers[1].type or "turn"
|
||||
self._next_modifier = maneuvers[1].modifier or "straight"
|
||||
else:
|
||||
self._has_next = False
|
||||
|
||||
self._valid = True
|
||||
|
||||
def _wrap_text_lines(self, text: str, max_width: float, font_size: int, max_lines: int = 2) -> tuple[list[str], bool]:
|
||||
words = text.split()
|
||||
if not words:
|
||||
return [], False
|
||||
|
||||
lines: list[str] = []
|
||||
current_words: list[str] = []
|
||||
word_index = 0
|
||||
|
||||
while word_index < len(words):
|
||||
candidate_words = current_words + [words[word_index]]
|
||||
candidate_line = " ".join(candidate_words)
|
||||
text_width = measure_text_cached(self._font_bold, candidate_line, font_size).x
|
||||
if text_width <= max_width or not current_words:
|
||||
current_words = candidate_words
|
||||
word_index += 1
|
||||
else:
|
||||
lines.append(" ".join(current_words))
|
||||
current_words = []
|
||||
if len(lines) >= max_lines:
|
||||
break
|
||||
|
||||
if current_words and len(lines) < max_lines:
|
||||
lines.append(" ".join(current_words))
|
||||
|
||||
truncated = word_index < len(words)
|
||||
if truncated and lines:
|
||||
shortened = lines[-1]
|
||||
candidate = f"{shortened}…"
|
||||
while shortened and measure_text_cached(self._font_bold, candidate, font_size).x > max_width:
|
||||
if " " in shortened:
|
||||
shortened = shortened.rsplit(" ", 1)[0]
|
||||
else:
|
||||
shortened = shortened[:-1]
|
||||
candidate = f"{shortened}…" if shortened else "…"
|
||||
lines[-1] = candidate
|
||||
|
||||
return lines[:max_lines], truncated
|
||||
|
||||
def _fit_title(self, text: str, max_width: float, preferred_size: int, minimum_size: int) -> tuple[list[str], int]:
|
||||
font_size = preferred_size
|
||||
while font_size > minimum_size:
|
||||
lines, truncated = self._wrap_text_lines(text, max_width, font_size)
|
||||
widest = max((measure_text_cached(self._font_bold, line, font_size).x for line in lines), default=0)
|
||||
if not truncated and widest <= max_width:
|
||||
return lines, font_size
|
||||
font_size -= 2
|
||||
lines, _ = self._wrap_text_lines(text, max_width, minimum_size)
|
||||
return lines, minimum_size
|
||||
|
||||
def _truncate_text(self, text: str, max_width: float, font_size: int) -> str:
|
||||
if measure_text_cached(self._font_medium, text, font_size).x <= max_width:
|
||||
return text
|
||||
|
||||
shortened = text.rstrip()
|
||||
candidate = f"{shortened}…"
|
||||
while shortened and measure_text_cached(self._font_medium, candidate, font_size).x > max_width:
|
||||
shortened = shortened[:-1].rstrip()
|
||||
candidate = f"{shortened}…" if shortened else "…"
|
||||
return candidate
|
||||
|
||||
def _draw_wrapped_lines(self, lines: list[str], x: float, y: float, font_size: int) -> None:
|
||||
if not lines:
|
||||
return
|
||||
|
||||
text_y = y if len(lines) == 1 else y - (font_size * 0.35)
|
||||
for index, line in enumerate(lines[:2]):
|
||||
rl.draw_text_ex(self._font_bold, line, rl.Vector2(x, text_y + index * font_size), font_size, 0, rl.WHITE)
|
||||
|
||||
def _draw_top_aligned_lines(self, lines: list[str], x: float, y: float, font_size: int, line_gap: int = 0) -> None:
|
||||
for index, line in enumerate(lines[:2]):
|
||||
line_y = y + index * (font_size + line_gap)
|
||||
rl.draw_text_ex(self._font_bold, line, rl.Vector2(x, line_y), font_size, 0, rl.WHITE)
|
||||
|
||||
def _render_default(self, rect: rl.Rectangle) -> None:
|
||||
container_width = min(int(rect.width - 120), 1080)
|
||||
container_width = max(container_width, 760)
|
||||
container_height = 225 if rect.width >= 1200 else 195
|
||||
container_x = int(rect.x + (rect.width - container_width) / 2)
|
||||
container_y = int(rect.y + 62)
|
||||
border_radius = 42 if container_height == 225 else 34
|
||||
icon_size = 150 if container_height == 225 else 122
|
||||
icon_padding = 30 if container_height == 225 else 24
|
||||
then_section_width = 180 if container_height == 225 else 144
|
||||
then_icon_size = 105 if container_height == 225 else 82
|
||||
preferred_font_size = 75 if container_height == 225 else 58
|
||||
minimum_font_size = 60 if container_height == 225 else 46
|
||||
distance_font_size = 48 if container_height == 225 else 40
|
||||
|
||||
container = rl.Rectangle(container_x, container_y, container_width, container_height)
|
||||
rl.draw_rectangle_rounded(container, border_radius / container_height, 10, rl.Color(0, 0, 0, 192))
|
||||
|
||||
icon_x = container_x + icon_padding
|
||||
icon_y = container_y + (container_height - icon_size - 38) // 2
|
||||
icon = self._get_icon(self._maneuver_type, self._modifier)
|
||||
dest_rect = rl.Rectangle(icon_x, icon_y, icon_size, icon_size)
|
||||
rl.draw_texture_pro(icon, rl.Rectangle(0, 0, icon.width, icon.height), dest_rect, rl.Vector2(0, 0), 0, rl.WHITE)
|
||||
|
||||
distance_size = measure_text_cached(self._font_bold, self._distance, distance_font_size)
|
||||
distance_x = icon_x + (icon_size - distance_size.x) / 2
|
||||
distance_y = icon_y + icon_size + 5
|
||||
rl.draw_text_ex(self._font_bold, self._distance, rl.Vector2(distance_x, distance_y), distance_font_size, 0, rl.WHITE)
|
||||
|
||||
show_next = self._has_next
|
||||
text_x = icon_x + icon_size + 53
|
||||
right_gutter = icon_padding + (then_section_width if show_next else 0)
|
||||
text_area_width = container_width - (text_x - container_x) - right_gutter
|
||||
title_lines, title_font_size = self._fit_title(self._primary_text, text_area_width, preferred_font_size, minimum_font_size)
|
||||
title_y = container_y + (container_height - title_font_size) / 2
|
||||
self._draw_wrapped_lines(title_lines, text_x, title_y, title_font_size)
|
||||
|
||||
if self._secondary_text:
|
||||
secondary_font_size = 38 if container_height == 225 else 30
|
||||
secondary_text = self._truncate_text(self._secondary_text, text_area_width, secondary_font_size)
|
||||
secondary_y = container_y + container_height - (42 if container_height == 225 else 30)
|
||||
rl.draw_text_ex(
|
||||
self._font_medium,
|
||||
secondary_text,
|
||||
rl.Vector2(text_x, secondary_y),
|
||||
secondary_font_size,
|
||||
0,
|
||||
rl.Color(255, 255, 255, 180),
|
||||
)
|
||||
|
||||
if not show_next:
|
||||
return
|
||||
|
||||
divider_x = container_x + container_width - then_section_width - 8
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(divider_x, container_y + 23),
|
||||
rl.Vector2(divider_x, container_y + container_height - 23),
|
||||
2,
|
||||
rl.Color(255, 255, 255, 50),
|
||||
)
|
||||
|
||||
then_x = divider_x + 15
|
||||
then_label = "Then"
|
||||
then_font_size = 53 if container_height == 225 else 40
|
||||
then_size = measure_text_cached(self._font_medium, then_label, then_font_size)
|
||||
then_label_x = then_x + (then_section_width - 23 - then_size.x) / 2
|
||||
rl.draw_text_ex(self._font_medium, then_label, rl.Vector2(then_label_x, container_y + 30), then_font_size, 0, rl.WHITE)
|
||||
|
||||
next_icon = self._get_icon(self._next_maneuver_type, self._next_modifier)
|
||||
then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2
|
||||
then_icon_y = container_y + (84 if container_height == 225 else 72)
|
||||
rl.draw_texture_pro(
|
||||
next_icon,
|
||||
rl.Rectangle(0, 0, next_icon.width, next_icon.height),
|
||||
rl.Rectangle(then_icon_x, then_icon_y, then_icon_size, then_icon_size),
|
||||
rl.Vector2(0, 0),
|
||||
0,
|
||||
rl.WHITE,
|
||||
)
|
||||
|
||||
def _render_mici(self, rect: rl.Rectangle) -> None:
|
||||
left_safe = 96
|
||||
right_margin = 12
|
||||
available_width = max(260, int(rect.width - left_safe - right_margin))
|
||||
container_width = min(available_width, 420)
|
||||
container_height = 142
|
||||
container_x = int(rect.x + left_safe)
|
||||
container_y = int(rect.y + 16)
|
||||
container = rl.Rectangle(container_x, container_y, container_width, container_height)
|
||||
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))
|
||||
|
||||
chip_size = 68
|
||||
chip_padding = 18
|
||||
chip_x = container_x + chip_padding
|
||||
chip_y = container_y + 16
|
||||
chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size)
|
||||
rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(70, 80, 94, 228))
|
||||
|
||||
icon = self._get_icon(self._maneuver_type, self._modifier)
|
||||
icon_size = 48
|
||||
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,
|
||||
)
|
||||
|
||||
distance_font_size = 24
|
||||
distance_size = measure_text_cached(self._font_bold, self._distance, distance_font_size)
|
||||
distance_x = chip_x + (chip_size - distance_size.x) / 2
|
||||
distance_y = container_y + container_height - 34
|
||||
rl.draw_text_ex(self._font_bold, self._distance, rl.Vector2(distance_x, distance_y), distance_font_size, 0, rl.WHITE)
|
||||
|
||||
show_next = self._has_next and available_width >= 660
|
||||
next_section_width = 92 if show_next else 0
|
||||
text_x = chip_x + chip_size + 18
|
||||
text_width = container_width - (text_x - container_x) - 18 - next_section_width
|
||||
title_lines, title_font_size = self._fit_title(self._primary_text, text_width, 32, 24)
|
||||
title_top = container_y + (26 if len(title_lines) == 1 else 16)
|
||||
self._draw_top_aligned_lines(title_lines, text_x, title_top, title_font_size, line_gap=2)
|
||||
|
||||
show_secondary = bool(self._secondary_text)
|
||||
if show_secondary:
|
||||
secondary_font_size = 20
|
||||
secondary_text = self._truncate_text(self._secondary_text, text_width, secondary_font_size)
|
||||
rl.draw_text_ex(
|
||||
self._font_medium,
|
||||
secondary_text,
|
||||
rl.Vector2(text_x, container_y + container_height - 40),
|
||||
secondary_font_size,
|
||||
0,
|
||||
rl.Color(255, 255, 255, 172),
|
||||
)
|
||||
|
||||
if not show_next:
|
||||
return
|
||||
|
||||
divider_x = container_x + container_width - next_section_width
|
||||
rl.draw_line_ex(
|
||||
rl.Vector2(divider_x, container_y + 16),
|
||||
rl.Vector2(divider_x, container_y + container_height - 16),
|
||||
2,
|
||||
rl.Color(255, 255, 255, 38),
|
||||
)
|
||||
|
||||
then_label = "Then"
|
||||
then_font_size = 20
|
||||
then_size = measure_text_cached(self._font_medium, then_label, then_font_size)
|
||||
then_x = divider_x + (next_section_width - then_size.x) / 2
|
||||
rl.draw_text_ex(self._font_medium, then_label, rl.Vector2(then_x, container_y + 16), then_font_size, 0, rl.Color(255, 255, 255, 196))
|
||||
|
||||
next_icon = self._get_icon(self._next_maneuver_type, self._next_modifier)
|
||||
next_chip_size = 54
|
||||
next_chip_x = divider_x + (next_section_width - next_chip_size) / 2
|
||||
next_chip_y = container_y + 46
|
||||
next_chip_rect = rl.Rectangle(next_chip_x, next_chip_y, next_chip_size, next_chip_size)
|
||||
rl.draw_rectangle_rounded(next_chip_rect, 0.34, 10, rl.Color(70, 80, 94, 210))
|
||||
next_icon_size = 38
|
||||
next_icon_x = next_chip_x + (next_chip_size - next_icon_size) / 2
|
||||
next_icon_y = next_chip_y + (next_chip_size - next_icon_size) / 2
|
||||
rl.draw_texture_pro(
|
||||
next_icon,
|
||||
rl.Rectangle(0, 0, next_icon.width, next_icon.height),
|
||||
rl.Rectangle(next_icon_x, next_icon_y, next_icon_size, next_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._layout_variant == "mici":
|
||||
self._render_mici(rect)
|
||||
else:
|
||||
self._render_default(rect)
|
||||
@@ -1,5 +1,7 @@
|
||||
#include "selfdrive/ui/qt/onroad/hud.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <QFileInfo>
|
||||
#include <cmath>
|
||||
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
@@ -40,6 +42,41 @@ void HudRenderer::updateState(const UIState &s) {
|
||||
v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
|
||||
float v_ego = v_ego_cluster_seen && !starpilot_toggles.value("use_wheel_speed").toBool() ? car_state.getVEgoCluster() : car_state.getVEgo();
|
||||
speed = std::max<float>(0.0f, v_ego * (is_metric ? MS_TO_KPH : MS_TO_MPH));
|
||||
|
||||
navigation_enabled = params.getBool("NavigationUI");
|
||||
navigation_valid = false;
|
||||
navigation_has_next = false;
|
||||
nav_primary_text.clear();
|
||||
nav_secondary_text.clear();
|
||||
nav_distance.clear();
|
||||
nav_maneuver_type.clear();
|
||||
nav_modifier.clear();
|
||||
nav_next_maneuver_type.clear();
|
||||
nav_next_modifier.clear();
|
||||
|
||||
if (!navigation_enabled || sm.rcv_frame("navInstruction") < s.scene.started_frame ||
|
||||
!sm.alive("navInstruction") || !sm.valid("navInstruction")) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto &nav = sm["navInstruction"].getNavInstruction();
|
||||
nav_primary_text = QString::fromUtf8(nav.getManeuverPrimaryText().cStr()).trimmed();
|
||||
if (nav_primary_text.isEmpty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
navigation_valid = true;
|
||||
nav_secondary_text = QString::fromUtf8(nav.getManeuverSecondaryText().cStr()).trimmed();
|
||||
nav_distance = formatNavDistance(nav.getManeuverDistance());
|
||||
nav_maneuver_type = QString::fromUtf8(nav.getManeuverType().cStr()).trimmed();
|
||||
nav_modifier = QString::fromUtf8(nav.getManeuverModifier().cStr()).trimmed();
|
||||
|
||||
const auto maneuvers = nav.getAllManeuvers();
|
||||
if (maneuvers.size() > 1) {
|
||||
navigation_has_next = true;
|
||||
nav_next_maneuver_type = QString::fromUtf8(maneuvers[1].getType().cStr()).trimmed();
|
||||
nav_next_modifier = QString::fromUtf8(maneuvers[1].getModifier().cStr()).trimmed();
|
||||
}
|
||||
}
|
||||
|
||||
void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
|
||||
@@ -61,6 +98,9 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
|
||||
if (starpilot_nvg->standstillDuration == 0 && !starpilot_toggles.value("hide_speed").toBool()) {
|
||||
drawCurrentSpeed(p, surface_rect);
|
||||
}
|
||||
if (navigation_valid) {
|
||||
drawNavigationCard(p, surface_rect);
|
||||
}
|
||||
|
||||
p.restore();
|
||||
}
|
||||
@@ -134,3 +174,169 @@ void HudRenderer::drawText(QPainter &p, int x, int y, const QString &text, int a
|
||||
p.setPen(QColor(0xff, 0xff, 0xff, alpha));
|
||||
p.drawText(real_rect.x(), real_rect.bottom(), text);
|
||||
}
|
||||
|
||||
QString HudRenderer::formatNavDistance(float distance_m) const {
|
||||
if (is_metric) {
|
||||
if (distance_m < 1000.0f) {
|
||||
int step = distance_m < 500.0f ? 25 : 50;
|
||||
return QString("%1 m").arg(qRound(distance_m / step) * step);
|
||||
}
|
||||
float km = distance_m / 1000.0f;
|
||||
return km >= 10.0f ? QString("%1 km").arg(static_cast<int>(std::floor(km))) : QString("%1 km").arg(km, 0, 'f', 1);
|
||||
}
|
||||
|
||||
float distance_ft = distance_m * METER_TO_FOOT;
|
||||
if (distance_ft < 1000.0f) {
|
||||
int step = distance_ft <= 100.0f ? 10 : 50;
|
||||
return QString("%1 ft").arg(qRound(distance_ft / step) * step);
|
||||
}
|
||||
|
||||
float miles = distance_ft / 5280.0f;
|
||||
return miles >= 10.0f ? QString("%1 mi").arg(static_cast<int>(std::floor(miles))) : QString("%1 mi").arg(miles, 0, 'f', 1);
|
||||
}
|
||||
|
||||
QString HudRenderer::navigationIconFilename(const QString &maneuver_type, const QString &modifier) const {
|
||||
QString normalized_type = maneuver_type.isEmpty() ? "turn" : maneuver_type;
|
||||
if (normalized_type == "rotary") {
|
||||
normalized_type = "roundabout";
|
||||
} else if (normalized_type == "new name" || normalized_type == "continue") {
|
||||
normalized_type = "turn";
|
||||
}
|
||||
|
||||
if (modifier == "uturn") {
|
||||
return "direction_uturn.png";
|
||||
}
|
||||
|
||||
static const QHash<QString, QString> suffixes = {
|
||||
{"left", "left"},
|
||||
{"right", "right"},
|
||||
{"straight", "straight"},
|
||||
{"slightLeft", "slight_left"},
|
||||
{"slightRight", "slight_right"},
|
||||
{"sharpLeft", "sharp_left"},
|
||||
{"sharpRight", "sharp_right"},
|
||||
};
|
||||
|
||||
normalized_type.replace(" ", "_");
|
||||
const QString suffix = suffixes.value(modifier, "");
|
||||
const QString candidate = suffix.isEmpty() ? QString("direction_%1.png").arg(normalized_type)
|
||||
: QString("direction_%1_%2.png").arg(normalized_type, suffix);
|
||||
const QString candidate_path = QString("../../starpilot/assets/navigation/%1").arg(candidate);
|
||||
return QFileInfo::exists(candidate_path) ? candidate : "direction_turn_straight.png";
|
||||
}
|
||||
|
||||
QPixmap HudRenderer::getNavigationIcon(const QString &maneuver_type, const QString &modifier, int size) {
|
||||
const QString icon_name = navigationIconFilename(maneuver_type, modifier);
|
||||
const QString cache_key = QString("%1|%2").arg(icon_name).arg(size);
|
||||
if (!nav_icon_cache.contains(cache_key)) {
|
||||
nav_icon_cache.insert(cache_key, loadPixmap(QString("../../starpilot/assets/navigation/%1").arg(icon_name), QSize(size, size)));
|
||||
}
|
||||
return nav_icon_cache.value(cache_key);
|
||||
}
|
||||
|
||||
QStringList HudRenderer::wrapNavigationText(int max_width, int font_size, const QString &text) const {
|
||||
QStringList words = text.split(' ', QString::SkipEmptyParts);
|
||||
QStringList lines;
|
||||
QString current_line;
|
||||
const QFontMetrics metrics(InterFont(font_size, QFont::Bold));
|
||||
|
||||
for (const QString &word : words) {
|
||||
QString test_line = current_line.isEmpty() ? word : current_line + " " + word;
|
||||
if (metrics.horizontalAdvance(test_line) <= max_width) {
|
||||
current_line = test_line;
|
||||
} else {
|
||||
if (!current_line.isEmpty()) {
|
||||
lines.append(current_line);
|
||||
}
|
||||
current_line = word;
|
||||
if (lines.size() >= 2) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!current_line.isEmpty() && lines.size() < 2) {
|
||||
lines.append(current_line);
|
||||
}
|
||||
return lines;
|
||||
}
|
||||
|
||||
void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) {
|
||||
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;
|
||||
const int container_y = surface_rect.width() >= 1200 ? 332 : 298;
|
||||
const int border_radius = container_height == 238 ? 42 : 34;
|
||||
const int icon_size = container_height == 238 ? 150 : 122;
|
||||
const int icon_padding = container_height == 238 ? 30 : 24;
|
||||
const int then_section_width = container_height == 238 ? 180 : 144;
|
||||
const int then_icon_size = container_height == 238 ? 105 : 82;
|
||||
const int title_font_size = container_height == 238 ? 75 : 58;
|
||||
const int secondary_font_size = container_height == 238 ? 34 : 28;
|
||||
const int distance_font_size = container_height == 238 ? 48 : 40;
|
||||
const int title_top_padding = container_height == 238 ? 10 : 10;
|
||||
const int title_line_spacing = container_height == 238 ? 68 : 56;
|
||||
const int secondary_bottom_padding = container_height == 238 ? 24 : 20;
|
||||
const int secondary_gap = container_height == 238 ? 10 : 8;
|
||||
|
||||
QRect container(container_x, container_y, container_width, container_height);
|
||||
p.setPen(Qt::NoPen);
|
||||
p.setBrush(QColor(0, 0, 0, 180));
|
||||
p.drawRoundedRect(container, border_radius, border_radius);
|
||||
|
||||
const int icon_x = container_x + icon_padding;
|
||||
const int icon_y = container_y + title_top_padding + 4;
|
||||
const QPixmap icon = getNavigationIcon(nav_maneuver_type, nav_modifier, icon_size);
|
||||
p.drawPixmap(icon_x, icon_y, icon);
|
||||
|
||||
p.setFont(InterFont(distance_font_size, QFont::Bold));
|
||||
p.setPen(Qt::white);
|
||||
const int distance_width = p.fontMetrics().horizontalAdvance(nav_distance);
|
||||
p.drawText(icon_x + (icon_size - distance_width) / 2, container_y + container_height - 18, nav_distance);
|
||||
|
||||
const int text_x = icon_x + icon_size + 53;
|
||||
const int right_gutter = icon_padding + (navigation_has_next ? then_section_width : 0);
|
||||
const int text_width = container_width - (text_x - container_x) - right_gutter;
|
||||
const QStringList title_lines = wrapNavigationText(text_width, title_font_size, nav_primary_text);
|
||||
const QFont title_font = InterFont(title_font_size, QFont::Bold);
|
||||
const QFontMetrics title_metrics(title_font);
|
||||
const int title_baseline = container_y + title_top_padding + title_metrics.ascent();
|
||||
|
||||
p.setFont(title_font);
|
||||
p.setPen(Qt::white);
|
||||
for (int index = 0; index < title_lines.size() && index < 2; ++index) {
|
||||
p.drawText(text_x, title_baseline + index * title_line_spacing, title_lines[index]);
|
||||
}
|
||||
|
||||
if (!nav_secondary_text.isEmpty()) {
|
||||
const QFont secondary_font = InterFont(secondary_font_size, QFont::Medium);
|
||||
const QFontMetrics secondary_metrics(secondary_font);
|
||||
const int title_bottom = title_baseline + (std::max(title_lines.size(), 1) - 1) * title_line_spacing + title_metrics.descent();
|
||||
const int secondary_max_baseline = container_y + container_height - secondary_bottom_padding - secondary_metrics.descent();
|
||||
const int secondary_preferred_baseline = title_bottom + secondary_gap + secondary_metrics.ascent();
|
||||
const int secondary_baseline = std::min(secondary_preferred_baseline, secondary_max_baseline);
|
||||
p.setFont(secondary_font);
|
||||
p.setPen(QColor(255, 255, 255, 180));
|
||||
p.drawText(text_x, secondary_baseline, nav_secondary_text);
|
||||
}
|
||||
|
||||
if (!navigation_has_next) {
|
||||
return;
|
||||
}
|
||||
|
||||
const int divider_x = container_x + container_width - then_section_width - 8;
|
||||
p.setPen(QPen(QColor(255, 255, 255, 50), 2));
|
||||
p.drawLine(divider_x, container_y + 23, divider_x, container_y + container_height - 23);
|
||||
|
||||
p.setFont(InterFont(container_height == 225 ? 53 : 40, QFont::Medium));
|
||||
p.setPen(Qt::white);
|
||||
const QString then_label = tr("Then");
|
||||
const int then_x = divider_x + 15;
|
||||
const int then_label_width = p.fontMetrics().horizontalAdvance(then_label);
|
||||
p.drawText(then_x + (then_section_width - 23 - then_label_width) / 2, container_y + (container_height == 238 ? 66 : 60), then_label);
|
||||
|
||||
const QPixmap next_icon = getNavigationIcon(nav_next_maneuver_type, nav_next_modifier, then_icon_size);
|
||||
const int then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2;
|
||||
const int then_icon_y = container_y + (container_height == 238 ? 96 : 78);
|
||||
p.drawPixmap(then_icon_x, then_icon_y, next_icon);
|
||||
}
|
||||
|
||||
@@ -1,6 +1,8 @@
|
||||
#pragma once
|
||||
|
||||
#include <QHash>
|
||||
#include <QPainter>
|
||||
#include <QPixmap>
|
||||
#include "selfdrive/ui/ui.h"
|
||||
|
||||
#include "starpilot/ui/qt/onroad/starpilot_annotated_camera.h"
|
||||
@@ -20,7 +22,12 @@ public:
|
||||
private:
|
||||
void drawSetSpeed(QPainter &p, const QRect &surface_rect);
|
||||
void drawCurrentSpeed(QPainter &p, const QRect &surface_rect);
|
||||
void drawNavigationCard(QPainter &p, const QRect &surface_rect);
|
||||
void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
|
||||
QStringList wrapNavigationText(int max_width, int font_size, const QString &text) const;
|
||||
QString formatNavDistance(float distance_m) const;
|
||||
QString navigationIconFilename(const QString &maneuver_type, const QString &modifier) const;
|
||||
QPixmap getNavigationIcon(const QString &maneuver_type, const QString &modifier, int size);
|
||||
|
||||
float speed = 0;
|
||||
float set_speed = 0;
|
||||
@@ -28,5 +35,17 @@ private:
|
||||
bool is_cruise_available = true;
|
||||
bool is_metric = false;
|
||||
bool v_ego_cluster_seen = false;
|
||||
bool navigation_enabled = false;
|
||||
bool navigation_valid = false;
|
||||
bool navigation_has_next = false;
|
||||
int status = STATUS_DISENGAGED;
|
||||
QString nav_distance;
|
||||
QString nav_primary_text;
|
||||
QString nav_secondary_text;
|
||||
QString nav_maneuver_type;
|
||||
QString nav_modifier;
|
||||
QString nav_next_maneuver_type;
|
||||
QString nav_next_modifier;
|
||||
QHash<QString, QPixmap> nav_icon_cache;
|
||||
Params params;
|
||||
};
|
||||
|
||||
@@ -367,6 +367,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
|
||||
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
|
||||
"pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
|
||||
"wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
|
||||
"navInstruction",
|
||||
});
|
||||
prime_state = new PrimeState(this);
|
||||
language = QString::fromStdString(Params().get("LanguageSetting"));
|
||||
|
||||
@@ -54,6 +54,7 @@ class UIState:
|
||||
"longitudinalPlan",
|
||||
"gpsLocationExternal",
|
||||
"mapdOut",
|
||||
"navInstruction",
|
||||
"carOutput",
|
||||
"carControl",
|
||||
"liveParameters",
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 865 B |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.6 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.2 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 658 B |
Binary file not shown.
|
After Width: | Height: | Size: 2.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 2.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 113 B |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.9 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.9 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.3 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user