mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-10-01 03:43:51 +08:00
Nav Long
This commit is contained in:
firestar5683
Binary file not shown.
@@ -380,6 +380,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"MapsSelected", {PERSISTENT, STRING, "", "", 0}},
|
||||
{"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}},
|
||||
{"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}},
|
||||
{"NavLongitudinalAllowed", {PERSISTENT, BOOL, "0", "0", 2}},
|
||||
{"NavDestination", {PERSISTENT, STRING, "", ""}},
|
||||
{"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}},
|
||||
{"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-c744f66b-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-01bd291b-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-c744f66b-DEBUG
|
||||
DEV-01bd291b-DEBUG
|
||||
@@ -6,8 +6,12 @@ from types import SimpleNamespace
|
||||
|
||||
|
||||
class FakeParams:
|
||||
def __init__(self, values=None):
|
||||
self.values = dict(values or {})
|
||||
|
||||
def get(self, *args, **kwargs):
|
||||
return None
|
||||
key = args[0] if args else None
|
||||
return self.values.get(key)
|
||||
|
||||
def get_float(self, *args, **kwargs):
|
||||
return 0.0
|
||||
@@ -16,10 +20,10 @@ class FakeParams:
|
||||
pass
|
||||
|
||||
|
||||
def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False):
|
||||
def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None):
|
||||
planner = SimpleNamespace(
|
||||
params=FakeParams(),
|
||||
params_memory=FakeParams(),
|
||||
params_memory=FakeParams({"NavInstructionState": nav_state or {}}),
|
||||
lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
|
||||
starpilot_cem=SimpleNamespace(stop_light_detected=red_light),
|
||||
tracking_lead=False,
|
||||
@@ -35,10 +39,11 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
|
||||
return planner, vcruise
|
||||
|
||||
|
||||
def make_sm(*, standstill=True):
|
||||
def make_sm(*, standstill=True, min_steer_speed=0.0):
|
||||
return {
|
||||
"carControl": SimpleNamespace(longActive=True),
|
||||
"carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0),
|
||||
"carParams": SimpleNamespace(minSteerSpeed=min_steer_speed),
|
||||
"starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0),
|
||||
}
|
||||
|
||||
@@ -48,6 +53,7 @@ def make_toggles():
|
||||
force_stops=True,
|
||||
force_standstill=False,
|
||||
curve_speed_controller=False,
|
||||
nav_longitudinal_allowed=False,
|
||||
speed_limit_controller=False,
|
||||
show_speed_limits=False,
|
||||
force_stop_distance_offset=0,
|
||||
@@ -133,3 +139,164 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
|
||||
assert result == pytest.approx(0.0)
|
||||
assert vcruise.force_stop_timer >= 0.5
|
||||
assert vcruise.forcing_stop
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_default_off():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "turn",
|
||||
"maneuverModifier": "right",
|
||||
"maneuverDistance": 30.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=20.0,
|
||||
v_ego=20.0,
|
||||
sm=make_sm(standstill=False),
|
||||
starpilot_toggles=make_toggles(),
|
||||
)
|
||||
|
||||
assert result == pytest.approx(20.0)
|
||||
assert vcruise.nav_turn_target == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_slows_for_imminent_turn():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "turn",
|
||||
"maneuverModifier": "right",
|
||||
"maneuverDistance": 30.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
toggles = make_toggles()
|
||||
toggles.nav_longitudinal_allowed = True
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=20.0,
|
||||
v_ego=20.0,
|
||||
sm=make_sm(standstill=False),
|
||||
starpilot_toggles=toggles,
|
||||
)
|
||||
|
||||
assert result < 20.0
|
||||
assert result == pytest.approx(vcruise.nav_turn_target)
|
||||
assert result > 0.0
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_ignores_distant_turn():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "turn",
|
||||
"maneuverModifier": "right",
|
||||
"maneuverDistance": 400.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
toggles = make_toggles()
|
||||
toggles.nav_longitudinal_allowed = True
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=20.0,
|
||||
v_ego=20.0,
|
||||
sm=make_sm(standstill=False),
|
||||
starpilot_toggles=toggles,
|
||||
)
|
||||
|
||||
assert result == pytest.approx(20.0)
|
||||
assert vcruise.nav_turn_target == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_ignores_off_ramp():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "off ramp",
|
||||
"maneuverModifier": "slightRight",
|
||||
"maneuverDistance": 40.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
toggles = make_toggles()
|
||||
toggles.nav_longitudinal_allowed = True
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=30.0,
|
||||
v_ego=30.0,
|
||||
sm=make_sm(standstill=False),
|
||||
starpilot_toggles=toggles,
|
||||
)
|
||||
|
||||
assert result == pytest.approx(30.0)
|
||||
assert vcruise.nav_turn_target == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_respects_car_min_steer_speed():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "turn",
|
||||
"maneuverModifier": "uturn",
|
||||
"maneuverDistance": 8.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
toggles = make_toggles()
|
||||
toggles.nav_longitudinal_allowed = True
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=20.0,
|
||||
v_ego=20.0,
|
||||
sm=make_sm(standstill=False, min_steer_speed=7.0 * CV.MPH_TO_MS),
|
||||
starpilot_toggles=toggles,
|
||||
)
|
||||
|
||||
assert result == pytest.approx(vcruise.nav_turn_target)
|
||||
assert vcruise.nav_turn_target >= 7.0 * CV.MPH_TO_MS
|
||||
|
||||
|
||||
def test_nav_turn_speed_control_does_not_floor_steer_to_zero_cars():
|
||||
_, vcruise = make_vcruise(nav_state={
|
||||
"valid": True,
|
||||
"maneuverType": "turn",
|
||||
"maneuverModifier": "uturn",
|
||||
"maneuverDistance": 8.0,
|
||||
"nextManeuverType": "",
|
||||
"nextManeuverModifier": "",
|
||||
"nextManeuverDistance": 0.0,
|
||||
})
|
||||
|
||||
toggles = make_toggles()
|
||||
toggles.nav_longitudinal_allowed = True
|
||||
result = vcruise.update(
|
||||
controls_enabled=True,
|
||||
now=0.0,
|
||||
time_validated=True,
|
||||
v_cruise=20.0,
|
||||
v_ego=20.0,
|
||||
sm=make_sm(standstill=False, min_steer_speed=0.0),
|
||||
starpilot_toggles=toggles,
|
||||
)
|
||||
|
||||
assert result == pytest.approx(vcruise.nav_turn_target)
|
||||
assert vcruise.nav_turn_target == pytest.approx(5.0 * CV.MPH_TO_MS)
|
||||
|
||||
@@ -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_284207352707246819) {
|
||||
out_284207352707246819[0] = delta_x[0] + nom_x[0];
|
||||
out_284207352707246819[1] = delta_x[1] + nom_x[1];
|
||||
out_284207352707246819[2] = delta_x[2] + nom_x[2];
|
||||
out_284207352707246819[3] = delta_x[3] + nom_x[3];
|
||||
out_284207352707246819[4] = delta_x[4] + nom_x[4];
|
||||
out_284207352707246819[5] = delta_x[5] + nom_x[5];
|
||||
out_284207352707246819[6] = delta_x[6] + nom_x[6];
|
||||
out_284207352707246819[7] = delta_x[7] + nom_x[7];
|
||||
out_284207352707246819[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) {
|
||||
out_575866575379183764[0] = delta_x[0] + nom_x[0];
|
||||
out_575866575379183764[1] = delta_x[1] + nom_x[1];
|
||||
out_575866575379183764[2] = delta_x[2] + nom_x[2];
|
||||
out_575866575379183764[3] = delta_x[3] + nom_x[3];
|
||||
out_575866575379183764[4] = delta_x[4] + nom_x[4];
|
||||
out_575866575379183764[5] = delta_x[5] + nom_x[5];
|
||||
out_575866575379183764[6] = delta_x[6] + nom_x[6];
|
||||
out_575866575379183764[7] = delta_x[7] + nom_x[7];
|
||||
out_575866575379183764[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) {
|
||||
out_494072413816411138[0] = -nom_x[0] + true_x[0];
|
||||
out_494072413816411138[1] = -nom_x[1] + true_x[1];
|
||||
out_494072413816411138[2] = -nom_x[2] + true_x[2];
|
||||
out_494072413816411138[3] = -nom_x[3] + true_x[3];
|
||||
out_494072413816411138[4] = -nom_x[4] + true_x[4];
|
||||
out_494072413816411138[5] = -nom_x[5] + true_x[5];
|
||||
out_494072413816411138[6] = -nom_x[6] + true_x[6];
|
||||
out_494072413816411138[7] = -nom_x[7] + true_x[7];
|
||||
out_494072413816411138[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) {
|
||||
out_5616167858322380498[0] = -nom_x[0] + true_x[0];
|
||||
out_5616167858322380498[1] = -nom_x[1] + true_x[1];
|
||||
out_5616167858322380498[2] = -nom_x[2] + true_x[2];
|
||||
out_5616167858322380498[3] = -nom_x[3] + true_x[3];
|
||||
out_5616167858322380498[4] = -nom_x[4] + true_x[4];
|
||||
out_5616167858322380498[5] = -nom_x[5] + true_x[5];
|
||||
out_5616167858322380498[6] = -nom_x[6] + true_x[6];
|
||||
out_5616167858322380498[7] = -nom_x[7] + true_x[7];
|
||||
out_5616167858322380498[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_4975994215663677500) {
|
||||
out_4975994215663677500[0] = 1.0;
|
||||
out_4975994215663677500[1] = 0.0;
|
||||
out_4975994215663677500[2] = 0.0;
|
||||
out_4975994215663677500[3] = 0.0;
|
||||
out_4975994215663677500[4] = 0.0;
|
||||
out_4975994215663677500[5] = 0.0;
|
||||
out_4975994215663677500[6] = 0.0;
|
||||
out_4975994215663677500[7] = 0.0;
|
||||
out_4975994215663677500[8] = 0.0;
|
||||
out_4975994215663677500[9] = 0.0;
|
||||
out_4975994215663677500[10] = 1.0;
|
||||
out_4975994215663677500[11] = 0.0;
|
||||
out_4975994215663677500[12] = 0.0;
|
||||
out_4975994215663677500[13] = 0.0;
|
||||
out_4975994215663677500[14] = 0.0;
|
||||
out_4975994215663677500[15] = 0.0;
|
||||
out_4975994215663677500[16] = 0.0;
|
||||
out_4975994215663677500[17] = 0.0;
|
||||
out_4975994215663677500[18] = 0.0;
|
||||
out_4975994215663677500[19] = 0.0;
|
||||
out_4975994215663677500[20] = 1.0;
|
||||
out_4975994215663677500[21] = 0.0;
|
||||
out_4975994215663677500[22] = 0.0;
|
||||
out_4975994215663677500[23] = 0.0;
|
||||
out_4975994215663677500[24] = 0.0;
|
||||
out_4975994215663677500[25] = 0.0;
|
||||
out_4975994215663677500[26] = 0.0;
|
||||
out_4975994215663677500[27] = 0.0;
|
||||
out_4975994215663677500[28] = 0.0;
|
||||
out_4975994215663677500[29] = 0.0;
|
||||
out_4975994215663677500[30] = 1.0;
|
||||
out_4975994215663677500[31] = 0.0;
|
||||
out_4975994215663677500[32] = 0.0;
|
||||
out_4975994215663677500[33] = 0.0;
|
||||
out_4975994215663677500[34] = 0.0;
|
||||
out_4975994215663677500[35] = 0.0;
|
||||
out_4975994215663677500[36] = 0.0;
|
||||
out_4975994215663677500[37] = 0.0;
|
||||
out_4975994215663677500[38] = 0.0;
|
||||
out_4975994215663677500[39] = 0.0;
|
||||
out_4975994215663677500[40] = 1.0;
|
||||
out_4975994215663677500[41] = 0.0;
|
||||
out_4975994215663677500[42] = 0.0;
|
||||
out_4975994215663677500[43] = 0.0;
|
||||
out_4975994215663677500[44] = 0.0;
|
||||
out_4975994215663677500[45] = 0.0;
|
||||
out_4975994215663677500[46] = 0.0;
|
||||
out_4975994215663677500[47] = 0.0;
|
||||
out_4975994215663677500[48] = 0.0;
|
||||
out_4975994215663677500[49] = 0.0;
|
||||
out_4975994215663677500[50] = 1.0;
|
||||
out_4975994215663677500[51] = 0.0;
|
||||
out_4975994215663677500[52] = 0.0;
|
||||
out_4975994215663677500[53] = 0.0;
|
||||
out_4975994215663677500[54] = 0.0;
|
||||
out_4975994215663677500[55] = 0.0;
|
||||
out_4975994215663677500[56] = 0.0;
|
||||
out_4975994215663677500[57] = 0.0;
|
||||
out_4975994215663677500[58] = 0.0;
|
||||
out_4975994215663677500[59] = 0.0;
|
||||
out_4975994215663677500[60] = 1.0;
|
||||
out_4975994215663677500[61] = 0.0;
|
||||
out_4975994215663677500[62] = 0.0;
|
||||
out_4975994215663677500[63] = 0.0;
|
||||
out_4975994215663677500[64] = 0.0;
|
||||
out_4975994215663677500[65] = 0.0;
|
||||
out_4975994215663677500[66] = 0.0;
|
||||
out_4975994215663677500[67] = 0.0;
|
||||
out_4975994215663677500[68] = 0.0;
|
||||
out_4975994215663677500[69] = 0.0;
|
||||
out_4975994215663677500[70] = 1.0;
|
||||
out_4975994215663677500[71] = 0.0;
|
||||
out_4975994215663677500[72] = 0.0;
|
||||
out_4975994215663677500[73] = 0.0;
|
||||
out_4975994215663677500[74] = 0.0;
|
||||
out_4975994215663677500[75] = 0.0;
|
||||
out_4975994215663677500[76] = 0.0;
|
||||
out_4975994215663677500[77] = 0.0;
|
||||
out_4975994215663677500[78] = 0.0;
|
||||
out_4975994215663677500[79] = 0.0;
|
||||
out_4975994215663677500[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_6639538403135995006) {
|
||||
out_6639538403135995006[0] = 1.0;
|
||||
out_6639538403135995006[1] = 0.0;
|
||||
out_6639538403135995006[2] = 0.0;
|
||||
out_6639538403135995006[3] = 0.0;
|
||||
out_6639538403135995006[4] = 0.0;
|
||||
out_6639538403135995006[5] = 0.0;
|
||||
out_6639538403135995006[6] = 0.0;
|
||||
out_6639538403135995006[7] = 0.0;
|
||||
out_6639538403135995006[8] = 0.0;
|
||||
out_6639538403135995006[9] = 0.0;
|
||||
out_6639538403135995006[10] = 1.0;
|
||||
out_6639538403135995006[11] = 0.0;
|
||||
out_6639538403135995006[12] = 0.0;
|
||||
out_6639538403135995006[13] = 0.0;
|
||||
out_6639538403135995006[14] = 0.0;
|
||||
out_6639538403135995006[15] = 0.0;
|
||||
out_6639538403135995006[16] = 0.0;
|
||||
out_6639538403135995006[17] = 0.0;
|
||||
out_6639538403135995006[18] = 0.0;
|
||||
out_6639538403135995006[19] = 0.0;
|
||||
out_6639538403135995006[20] = 1.0;
|
||||
out_6639538403135995006[21] = 0.0;
|
||||
out_6639538403135995006[22] = 0.0;
|
||||
out_6639538403135995006[23] = 0.0;
|
||||
out_6639538403135995006[24] = 0.0;
|
||||
out_6639538403135995006[25] = 0.0;
|
||||
out_6639538403135995006[26] = 0.0;
|
||||
out_6639538403135995006[27] = 0.0;
|
||||
out_6639538403135995006[28] = 0.0;
|
||||
out_6639538403135995006[29] = 0.0;
|
||||
out_6639538403135995006[30] = 1.0;
|
||||
out_6639538403135995006[31] = 0.0;
|
||||
out_6639538403135995006[32] = 0.0;
|
||||
out_6639538403135995006[33] = 0.0;
|
||||
out_6639538403135995006[34] = 0.0;
|
||||
out_6639538403135995006[35] = 0.0;
|
||||
out_6639538403135995006[36] = 0.0;
|
||||
out_6639538403135995006[37] = 0.0;
|
||||
out_6639538403135995006[38] = 0.0;
|
||||
out_6639538403135995006[39] = 0.0;
|
||||
out_6639538403135995006[40] = 1.0;
|
||||
out_6639538403135995006[41] = 0.0;
|
||||
out_6639538403135995006[42] = 0.0;
|
||||
out_6639538403135995006[43] = 0.0;
|
||||
out_6639538403135995006[44] = 0.0;
|
||||
out_6639538403135995006[45] = 0.0;
|
||||
out_6639538403135995006[46] = 0.0;
|
||||
out_6639538403135995006[47] = 0.0;
|
||||
out_6639538403135995006[48] = 0.0;
|
||||
out_6639538403135995006[49] = 0.0;
|
||||
out_6639538403135995006[50] = 1.0;
|
||||
out_6639538403135995006[51] = 0.0;
|
||||
out_6639538403135995006[52] = 0.0;
|
||||
out_6639538403135995006[53] = 0.0;
|
||||
out_6639538403135995006[54] = 0.0;
|
||||
out_6639538403135995006[55] = 0.0;
|
||||
out_6639538403135995006[56] = 0.0;
|
||||
out_6639538403135995006[57] = 0.0;
|
||||
out_6639538403135995006[58] = 0.0;
|
||||
out_6639538403135995006[59] = 0.0;
|
||||
out_6639538403135995006[60] = 1.0;
|
||||
out_6639538403135995006[61] = 0.0;
|
||||
out_6639538403135995006[62] = 0.0;
|
||||
out_6639538403135995006[63] = 0.0;
|
||||
out_6639538403135995006[64] = 0.0;
|
||||
out_6639538403135995006[65] = 0.0;
|
||||
out_6639538403135995006[66] = 0.0;
|
||||
out_6639538403135995006[67] = 0.0;
|
||||
out_6639538403135995006[68] = 0.0;
|
||||
out_6639538403135995006[69] = 0.0;
|
||||
out_6639538403135995006[70] = 1.0;
|
||||
out_6639538403135995006[71] = 0.0;
|
||||
out_6639538403135995006[72] = 0.0;
|
||||
out_6639538403135995006[73] = 0.0;
|
||||
out_6639538403135995006[74] = 0.0;
|
||||
out_6639538403135995006[75] = 0.0;
|
||||
out_6639538403135995006[76] = 0.0;
|
||||
out_6639538403135995006[77] = 0.0;
|
||||
out_6639538403135995006[78] = 0.0;
|
||||
out_6639538403135995006[79] = 0.0;
|
||||
out_6639538403135995006[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1721040043494422786) {
|
||||
out_1721040043494422786[0] = state[0];
|
||||
out_1721040043494422786[1] = state[1];
|
||||
out_1721040043494422786[2] = state[2];
|
||||
out_1721040043494422786[3] = state[3];
|
||||
out_1721040043494422786[4] = state[4];
|
||||
out_1721040043494422786[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_1721040043494422786[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_1721040043494422786[7] = state[7];
|
||||
out_1721040043494422786[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_6721417124249708482) {
|
||||
out_6721417124249708482[0] = state[0];
|
||||
out_6721417124249708482[1] = state[1];
|
||||
out_6721417124249708482[2] = state[2];
|
||||
out_6721417124249708482[3] = state[3];
|
||||
out_6721417124249708482[4] = state[4];
|
||||
out_6721417124249708482[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_6721417124249708482[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_6721417124249708482[7] = state[7];
|
||||
out_6721417124249708482[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_6408657319260780093) {
|
||||
out_6408657319260780093[0] = 1;
|
||||
out_6408657319260780093[1] = 0;
|
||||
out_6408657319260780093[2] = 0;
|
||||
out_6408657319260780093[3] = 0;
|
||||
out_6408657319260780093[4] = 0;
|
||||
out_6408657319260780093[5] = 0;
|
||||
out_6408657319260780093[6] = 0;
|
||||
out_6408657319260780093[7] = 0;
|
||||
out_6408657319260780093[8] = 0;
|
||||
out_6408657319260780093[9] = 0;
|
||||
out_6408657319260780093[10] = 1;
|
||||
out_6408657319260780093[11] = 0;
|
||||
out_6408657319260780093[12] = 0;
|
||||
out_6408657319260780093[13] = 0;
|
||||
out_6408657319260780093[14] = 0;
|
||||
out_6408657319260780093[15] = 0;
|
||||
out_6408657319260780093[16] = 0;
|
||||
out_6408657319260780093[17] = 0;
|
||||
out_6408657319260780093[18] = 0;
|
||||
out_6408657319260780093[19] = 0;
|
||||
out_6408657319260780093[20] = 1;
|
||||
out_6408657319260780093[21] = 0;
|
||||
out_6408657319260780093[22] = 0;
|
||||
out_6408657319260780093[23] = 0;
|
||||
out_6408657319260780093[24] = 0;
|
||||
out_6408657319260780093[25] = 0;
|
||||
out_6408657319260780093[26] = 0;
|
||||
out_6408657319260780093[27] = 0;
|
||||
out_6408657319260780093[28] = 0;
|
||||
out_6408657319260780093[29] = 0;
|
||||
out_6408657319260780093[30] = 1;
|
||||
out_6408657319260780093[31] = 0;
|
||||
out_6408657319260780093[32] = 0;
|
||||
out_6408657319260780093[33] = 0;
|
||||
out_6408657319260780093[34] = 0;
|
||||
out_6408657319260780093[35] = 0;
|
||||
out_6408657319260780093[36] = 0;
|
||||
out_6408657319260780093[37] = 0;
|
||||
out_6408657319260780093[38] = 0;
|
||||
out_6408657319260780093[39] = 0;
|
||||
out_6408657319260780093[40] = 1;
|
||||
out_6408657319260780093[41] = 0;
|
||||
out_6408657319260780093[42] = 0;
|
||||
out_6408657319260780093[43] = 0;
|
||||
out_6408657319260780093[44] = 0;
|
||||
out_6408657319260780093[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_6408657319260780093[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_6408657319260780093[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[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_6408657319260780093[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_6408657319260780093[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_6408657319260780093[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_6408657319260780093[53] = -9.8100000000000005*dt;
|
||||
out_6408657319260780093[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_6408657319260780093[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_6408657319260780093[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[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_6408657319260780093[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_6408657319260780093[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_6408657319260780093[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_6408657319260780093[62] = 0;
|
||||
out_6408657319260780093[63] = 0;
|
||||
out_6408657319260780093[64] = 0;
|
||||
out_6408657319260780093[65] = 0;
|
||||
out_6408657319260780093[66] = 0;
|
||||
out_6408657319260780093[67] = 0;
|
||||
out_6408657319260780093[68] = 0;
|
||||
out_6408657319260780093[69] = 0;
|
||||
out_6408657319260780093[70] = 1;
|
||||
out_6408657319260780093[71] = 0;
|
||||
out_6408657319260780093[72] = 0;
|
||||
out_6408657319260780093[73] = 0;
|
||||
out_6408657319260780093[74] = 0;
|
||||
out_6408657319260780093[75] = 0;
|
||||
out_6408657319260780093[76] = 0;
|
||||
out_6408657319260780093[77] = 0;
|
||||
out_6408657319260780093[78] = 0;
|
||||
out_6408657319260780093[79] = 0;
|
||||
out_6408657319260780093[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2916284287213904038) {
|
||||
out_2916284287213904038[0] = 1;
|
||||
out_2916284287213904038[1] = 0;
|
||||
out_2916284287213904038[2] = 0;
|
||||
out_2916284287213904038[3] = 0;
|
||||
out_2916284287213904038[4] = 0;
|
||||
out_2916284287213904038[5] = 0;
|
||||
out_2916284287213904038[6] = 0;
|
||||
out_2916284287213904038[7] = 0;
|
||||
out_2916284287213904038[8] = 0;
|
||||
out_2916284287213904038[9] = 0;
|
||||
out_2916284287213904038[10] = 1;
|
||||
out_2916284287213904038[11] = 0;
|
||||
out_2916284287213904038[12] = 0;
|
||||
out_2916284287213904038[13] = 0;
|
||||
out_2916284287213904038[14] = 0;
|
||||
out_2916284287213904038[15] = 0;
|
||||
out_2916284287213904038[16] = 0;
|
||||
out_2916284287213904038[17] = 0;
|
||||
out_2916284287213904038[18] = 0;
|
||||
out_2916284287213904038[19] = 0;
|
||||
out_2916284287213904038[20] = 1;
|
||||
out_2916284287213904038[21] = 0;
|
||||
out_2916284287213904038[22] = 0;
|
||||
out_2916284287213904038[23] = 0;
|
||||
out_2916284287213904038[24] = 0;
|
||||
out_2916284287213904038[25] = 0;
|
||||
out_2916284287213904038[26] = 0;
|
||||
out_2916284287213904038[27] = 0;
|
||||
out_2916284287213904038[28] = 0;
|
||||
out_2916284287213904038[29] = 0;
|
||||
out_2916284287213904038[30] = 1;
|
||||
out_2916284287213904038[31] = 0;
|
||||
out_2916284287213904038[32] = 0;
|
||||
out_2916284287213904038[33] = 0;
|
||||
out_2916284287213904038[34] = 0;
|
||||
out_2916284287213904038[35] = 0;
|
||||
out_2916284287213904038[36] = 0;
|
||||
out_2916284287213904038[37] = 0;
|
||||
out_2916284287213904038[38] = 0;
|
||||
out_2916284287213904038[39] = 0;
|
||||
out_2916284287213904038[40] = 1;
|
||||
out_2916284287213904038[41] = 0;
|
||||
out_2916284287213904038[42] = 0;
|
||||
out_2916284287213904038[43] = 0;
|
||||
out_2916284287213904038[44] = 0;
|
||||
out_2916284287213904038[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_2916284287213904038[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2916284287213904038[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[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_2916284287213904038[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2916284287213904038[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2916284287213904038[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2916284287213904038[53] = -9.8100000000000005*dt;
|
||||
out_2916284287213904038[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_2916284287213904038[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2916284287213904038[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[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_2916284287213904038[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2916284287213904038[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_2916284287213904038[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2916284287213904038[62] = 0;
|
||||
out_2916284287213904038[63] = 0;
|
||||
out_2916284287213904038[64] = 0;
|
||||
out_2916284287213904038[65] = 0;
|
||||
out_2916284287213904038[66] = 0;
|
||||
out_2916284287213904038[67] = 0;
|
||||
out_2916284287213904038[68] = 0;
|
||||
out_2916284287213904038[69] = 0;
|
||||
out_2916284287213904038[70] = 1;
|
||||
out_2916284287213904038[71] = 0;
|
||||
out_2916284287213904038[72] = 0;
|
||||
out_2916284287213904038[73] = 0;
|
||||
out_2916284287213904038[74] = 0;
|
||||
out_2916284287213904038[75] = 0;
|
||||
out_2916284287213904038[76] = 0;
|
||||
out_2916284287213904038[77] = 0;
|
||||
out_2916284287213904038[78] = 0;
|
||||
out_2916284287213904038[79] = 0;
|
||||
out_2916284287213904038[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_8679682106878424978) {
|
||||
out_8679682106878424978[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_2044795451145406261) {
|
||||
out_2044795451145406261[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_4470659773463380788) {
|
||||
out_4470659773463380788[0] = 0;
|
||||
out_4470659773463380788[1] = 0;
|
||||
out_4470659773463380788[2] = 0;
|
||||
out_4470659773463380788[3] = 0;
|
||||
out_4470659773463380788[4] = 0;
|
||||
out_4470659773463380788[5] = 0;
|
||||
out_4470659773463380788[6] = 1;
|
||||
out_4470659773463380788[7] = 0;
|
||||
out_4470659773463380788[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_8284225020584064933) {
|
||||
out_8284225020584064933[0] = 0;
|
||||
out_8284225020584064933[1] = 0;
|
||||
out_8284225020584064933[2] = 0;
|
||||
out_8284225020584064933[3] = 0;
|
||||
out_8284225020584064933[4] = 0;
|
||||
out_8284225020584064933[5] = 0;
|
||||
out_8284225020584064933[6] = 1;
|
||||
out_8284225020584064933[7] = 0;
|
||||
out_8284225020584064933[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_5665719925403943373) {
|
||||
out_5665719925403943373[0] = state[4];
|
||||
out_5665719925403943373[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_1441131931248447039) {
|
||||
out_1441131931248447039[0] = state[4];
|
||||
out_1441131931248447039[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7514976983467728980) {
|
||||
out_7514976983467728980[0] = 0;
|
||||
out_7514976983467728980[1] = 0;
|
||||
out_7514976983467728980[2] = 0;
|
||||
out_7514976983467728980[3] = 0;
|
||||
out_7514976983467728980[4] = 1;
|
||||
out_7514976983467728980[5] = 0;
|
||||
out_7514976983467728980[6] = 0;
|
||||
out_7514976983467728980[7] = 0;
|
||||
out_7514976983467728980[8] = 0;
|
||||
out_7514976983467728980[9] = 0;
|
||||
out_7514976983467728980[10] = 0;
|
||||
out_7514976983467728980[11] = 0;
|
||||
out_7514976983467728980[12] = 0;
|
||||
out_7514976983467728980[13] = 0;
|
||||
out_7514976983467728980[14] = 1;
|
||||
out_7514976983467728980[15] = 0;
|
||||
out_7514976983467728980[16] = 0;
|
||||
out_7514976983467728980[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_4726299421781741797) {
|
||||
out_4726299421781741797[0] = 0;
|
||||
out_4726299421781741797[1] = 0;
|
||||
out_4726299421781741797[2] = 0;
|
||||
out_4726299421781741797[3] = 0;
|
||||
out_4726299421781741797[4] = 1;
|
||||
out_4726299421781741797[5] = 0;
|
||||
out_4726299421781741797[6] = 0;
|
||||
out_4726299421781741797[7] = 0;
|
||||
out_4726299421781741797[8] = 0;
|
||||
out_4726299421781741797[9] = 0;
|
||||
out_4726299421781741797[10] = 0;
|
||||
out_4726299421781741797[11] = 0;
|
||||
out_4726299421781741797[12] = 0;
|
||||
out_4726299421781741797[13] = 0;
|
||||
out_4726299421781741797[14] = 1;
|
||||
out_4726299421781741797[15] = 0;
|
||||
out_4726299421781741797[16] = 0;
|
||||
out_4726299421781741797[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_6129612438577104288) {
|
||||
out_6129612438577104288[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_8638683365743472914) {
|
||||
out_8638683365743472914[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_4599998720606620858) {
|
||||
out_4599998720606620858[0] = 0;
|
||||
out_4599998720606620858[1] = 0;
|
||||
out_4599998720606620858[2] = 0;
|
||||
out_4599998720606620858[3] = 0;
|
||||
out_4599998720606620858[4] = 1;
|
||||
out_4599998720606620858[5] = 0;
|
||||
out_4599998720606620858[6] = 0;
|
||||
out_4599998720606620858[7] = 0;
|
||||
out_4599998720606620858[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_8413563967727305003) {
|
||||
out_8413563967727305003[0] = 0;
|
||||
out_8413563967727305003[1] = 0;
|
||||
out_8413563967727305003[2] = 0;
|
||||
out_8413563967727305003[3] = 0;
|
||||
out_8413563967727305003[4] = 1;
|
||||
out_8413563967727305003[5] = 0;
|
||||
out_8413563967727305003[6] = 0;
|
||||
out_8413563967727305003[7] = 0;
|
||||
out_8413563967727305003[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_6887462583737923098) {
|
||||
out_6887462583737923098[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_989338192432138572) {
|
||||
out_989338192432138572[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_3813805709353068884) {
|
||||
out_3813805709353068884[0] = 0;
|
||||
out_3813805709353068884[1] = 0;
|
||||
out_3813805709353068884[2] = 0;
|
||||
out_3813805709353068884[3] = 0;
|
||||
out_3813805709353068884[4] = 0;
|
||||
out_3813805709353068884[5] = 0;
|
||||
out_3813805709353068884[6] = 0;
|
||||
out_3813805709353068884[7] = 1;
|
||||
out_3813805709353068884[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_6421015734251430459) {
|
||||
out_6421015734251430459[0] = 0;
|
||||
out_6421015734251430459[1] = 0;
|
||||
out_6421015734251430459[2] = 0;
|
||||
out_6421015734251430459[3] = 0;
|
||||
out_6421015734251430459[4] = 0;
|
||||
out_6421015734251430459[5] = 0;
|
||||
out_6421015734251430459[6] = 0;
|
||||
out_6421015734251430459[7] = 1;
|
||||
out_6421015734251430459[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_3337814870992394415) {
|
||||
out_3337814870992394415[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_7375345222001897423) {
|
||||
out_7375345222001897423[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_6774762032407045769) {
|
||||
out_6774762032407045769[0] = 0;
|
||||
out_6774762032407045769[1] = 0;
|
||||
out_6774762032407045769[2] = 0;
|
||||
out_6774762032407045769[3] = 1;
|
||||
out_6774762032407045769[4] = 0;
|
||||
out_6774762032407045769[5] = 0;
|
||||
out_6774762032407045769[6] = 0;
|
||||
out_6774762032407045769[7] = 0;
|
||||
out_6774762032407045769[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_7858416794181821702) {
|
||||
out_7858416794181821702[0] = 0;
|
||||
out_7858416794181821702[1] = 0;
|
||||
out_7858416794181821702[2] = 0;
|
||||
out_7858416794181821702[3] = 1;
|
||||
out_7858416794181821702[4] = 0;
|
||||
out_7858416794181821702[5] = 0;
|
||||
out_7858416794181821702[6] = 0;
|
||||
out_7858416794181821702[7] = 0;
|
||||
out_7858416794181821702[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_5302062605726147469) {
|
||||
out_5302062605726147469[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_4806387440305233057) {
|
||||
out_4806387440305233057[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_4089767376292228674) {
|
||||
out_4089767376292228674[0] = 0;
|
||||
out_4089767376292228674[1] = 1;
|
||||
out_4089767376292228674[2] = 0;
|
||||
out_4089767376292228674[3] = 0;
|
||||
out_4089767376292228674[4] = 0;
|
||||
out_4089767376292228674[5] = 0;
|
||||
out_4089767376292228674[6] = 0;
|
||||
out_4089767376292228674[7] = 0;
|
||||
out_4089767376292228674[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_6145054067312270669) {
|
||||
out_6145054067312270669[0] = 0;
|
||||
out_6145054067312270669[1] = 1;
|
||||
out_6145054067312270669[2] = 0;
|
||||
out_6145054067312270669[3] = 0;
|
||||
out_6145054067312270669[4] = 0;
|
||||
out_6145054067312270669[5] = 0;
|
||||
out_6145054067312270669[6] = 0;
|
||||
out_6145054067312270669[7] = 0;
|
||||
out_6145054067312270669[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_301028819548061730) {
|
||||
out_301028819548061730[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_6025091355491401381) {
|
||||
out_6025091355491401381[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_2126137104726902423) {
|
||||
out_2126137104726902423[0] = 1;
|
||||
out_2126137104726902423[1] = 0;
|
||||
out_2126137104726902423[2] = 0;
|
||||
out_2126137104726902423[3] = 0;
|
||||
out_2126137104726902423[4] = 0;
|
||||
out_2126137104726902423[5] = 0;
|
||||
out_2126137104726902423[6] = 0;
|
||||
out_2126137104726902423[7] = 0;
|
||||
out_2126137104726902423[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_8108684338877596920) {
|
||||
out_8108684338877596920[0] = 1;
|
||||
out_8108684338877596920[1] = 0;
|
||||
out_8108684338877596920[2] = 0;
|
||||
out_8108684338877596920[3] = 0;
|
||||
out_8108684338877596920[4] = 0;
|
||||
out_8108684338877596920[5] = 0;
|
||||
out_8108684338877596920[6] = 0;
|
||||
out_8108684338877596920[7] = 0;
|
||||
out_8108684338877596920[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_213227004039568063) {
|
||||
out_213227004039568063[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_2208042107618306896) {
|
||||
out_2208042107618306896[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_4440013811586420360) {
|
||||
out_4440013811586420360[0] = 0;
|
||||
out_4440013811586420360[1] = 0;
|
||||
out_4440013811586420360[2] = 0;
|
||||
out_4440013811586420360[3] = 0;
|
||||
out_4440013811586420360[4] = 0;
|
||||
out_4440013811586420360[5] = 0;
|
||||
out_4440013811586420360[6] = 0;
|
||||
out_4440013811586420360[7] = 0;
|
||||
out_4440013811586420360[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_8253579058707104505) {
|
||||
out_8253579058707104505[0] = 0;
|
||||
out_8253579058707104505[1] = 0;
|
||||
out_8253579058707104505[2] = 0;
|
||||
out_8253579058707104505[3] = 0;
|
||||
out_8253579058707104505[4] = 0;
|
||||
out_8253579058707104505[5] = 0;
|
||||
out_8253579058707104505[6] = 0;
|
||||
out_8253579058707104505[7] = 0;
|
||||
out_8253579058707104505[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_284207352707246819) {
|
||||
err_fun(nom_x, delta_x, out_284207352707246819);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764) {
|
||||
err_fun(nom_x, delta_x, out_575866575379183764);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138) {
|
||||
inv_err_fun(nom_x, true_x, out_494072413816411138);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498) {
|
||||
inv_err_fun(nom_x, true_x, out_5616167858322380498);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_4975994215663677500) {
|
||||
H_mod_fun(state, out_4975994215663677500);
|
||||
void car_H_mod_fun(double *state, double *out_6639538403135995006) {
|
||||
H_mod_fun(state, out_6639538403135995006);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1721040043494422786) {
|
||||
f_fun(state, dt, out_1721040043494422786);
|
||||
void car_f_fun(double *state, double dt, double *out_6721417124249708482) {
|
||||
f_fun(state, dt, out_6721417124249708482);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_6408657319260780093) {
|
||||
F_fun(state, dt, out_6408657319260780093);
|
||||
void car_F_fun(double *state, double dt, double *out_2916284287213904038) {
|
||||
F_fun(state, dt, out_2916284287213904038);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_8679682106878424978) {
|
||||
h_25(state, unused, out_8679682106878424978);
|
||||
void car_h_25(double *state, double *unused, double *out_2044795451145406261) {
|
||||
h_25(state, unused, out_2044795451145406261);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_4470659773463380788) {
|
||||
H_25(state, unused, out_4470659773463380788);
|
||||
void car_H_25(double *state, double *unused, double *out_8284225020584064933) {
|
||||
H_25(state, unused, out_8284225020584064933);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_5665719925403943373) {
|
||||
h_24(state, unused, out_5665719925403943373);
|
||||
void car_h_24(double *state, double *unused, double *out_1441131931248447039) {
|
||||
h_24(state, unused, out_1441131931248447039);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7514976983467728980) {
|
||||
H_24(state, unused, out_7514976983467728980);
|
||||
void car_H_24(double *state, double *unused, double *out_4726299421781741797) {
|
||||
H_24(state, unused, out_4726299421781741797);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_6129612438577104288) {
|
||||
h_30(state, unused, out_6129612438577104288);
|
||||
void car_h_30(double *state, double *unused, double *out_8638683365743472914) {
|
||||
h_30(state, unused, out_8638683365743472914);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_4599998720606620858) {
|
||||
H_30(state, unused, out_4599998720606620858);
|
||||
void car_H_30(double *state, double *unused, double *out_8413563967727305003) {
|
||||
H_30(state, unused, out_8413563967727305003);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_6887462583737923098) {
|
||||
h_26(state, unused, out_6887462583737923098);
|
||||
void car_h_26(double *state, double *unused, double *out_989338192432138572) {
|
||||
h_26(state, unused, out_989338192432138572);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_3813805709353068884) {
|
||||
H_26(state, unused, out_3813805709353068884);
|
||||
void car_H_26(double *state, double *unused, double *out_6421015734251430459) {
|
||||
H_26(state, unused, out_6421015734251430459);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_3337814870992394415) {
|
||||
h_27(state, unused, out_3337814870992394415);
|
||||
void car_h_27(double *state, double *unused, double *out_7375345222001897423) {
|
||||
h_27(state, unused, out_7375345222001897423);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_6774762032407045769) {
|
||||
H_27(state, unused, out_6774762032407045769);
|
||||
void car_H_27(double *state, double *unused, double *out_7858416794181821702) {
|
||||
H_27(state, unused, out_7858416794181821702);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_5302062605726147469) {
|
||||
h_29(state, unused, out_5302062605726147469);
|
||||
void car_h_29(double *state, double *unused, double *out_4806387440305233057) {
|
||||
h_29(state, unused, out_4806387440305233057);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_4089767376292228674) {
|
||||
H_29(state, unused, out_4089767376292228674);
|
||||
void car_H_29(double *state, double *unused, double *out_6145054067312270669) {
|
||||
H_29(state, unused, out_6145054067312270669);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_301028819548061730) {
|
||||
h_28(state, unused, out_301028819548061730);
|
||||
void car_h_28(double *state, double *unused, double *out_6025091355491401381) {
|
||||
h_28(state, unused, out_6025091355491401381);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_2126137104726902423) {
|
||||
H_28(state, unused, out_2126137104726902423);
|
||||
void car_H_28(double *state, double *unused, double *out_8108684338877596920) {
|
||||
H_28(state, unused, out_8108684338877596920);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_213227004039568063) {
|
||||
h_31(state, unused, out_213227004039568063);
|
||||
void car_h_31(double *state, double *unused, double *out_2208042107618306896) {
|
||||
h_31(state, unused, out_2208042107618306896);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_4440013811586420360) {
|
||||
H_31(state, unused, out_4440013811586420360);
|
||||
void car_H_31(double *state, double *unused, double *out_8253579058707104505) {
|
||||
H_31(state, unused, out_8253579058707104505);
|
||||
}
|
||||
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_284207352707246819);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_494072413816411138);
|
||||
void car_H_mod_fun(double *state, double *out_4975994215663677500);
|
||||
void car_f_fun(double *state, double dt, double *out_1721040043494422786);
|
||||
void car_F_fun(double *state, double dt, double *out_6408657319260780093);
|
||||
void car_h_25(double *state, double *unused, double *out_8679682106878424978);
|
||||
void car_H_25(double *state, double *unused, double *out_4470659773463380788);
|
||||
void car_h_24(double *state, double *unused, double *out_5665719925403943373);
|
||||
void car_H_24(double *state, double *unused, double *out_7514976983467728980);
|
||||
void car_h_30(double *state, double *unused, double *out_6129612438577104288);
|
||||
void car_H_30(double *state, double *unused, double *out_4599998720606620858);
|
||||
void car_h_26(double *state, double *unused, double *out_6887462583737923098);
|
||||
void car_H_26(double *state, double *unused, double *out_3813805709353068884);
|
||||
void car_h_27(double *state, double *unused, double *out_3337814870992394415);
|
||||
void car_H_27(double *state, double *unused, double *out_6774762032407045769);
|
||||
void car_h_29(double *state, double *unused, double *out_5302062605726147469);
|
||||
void car_H_29(double *state, double *unused, double *out_4089767376292228674);
|
||||
void car_h_28(double *state, double *unused, double *out_301028819548061730);
|
||||
void car_H_28(double *state, double *unused, double *out_2126137104726902423);
|
||||
void car_h_31(double *state, double *unused, double *out_213227004039568063);
|
||||
void car_H_31(double *state, double *unused, double *out_4440013811586420360);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_575866575379183764);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5616167858322380498);
|
||||
void car_H_mod_fun(double *state, double *out_6639538403135995006);
|
||||
void car_f_fun(double *state, double dt, double *out_6721417124249708482);
|
||||
void car_F_fun(double *state, double dt, double *out_2916284287213904038);
|
||||
void car_h_25(double *state, double *unused, double *out_2044795451145406261);
|
||||
void car_H_25(double *state, double *unused, double *out_8284225020584064933);
|
||||
void car_h_24(double *state, double *unused, double *out_1441131931248447039);
|
||||
void car_H_24(double *state, double *unused, double *out_4726299421781741797);
|
||||
void car_h_30(double *state, double *unused, double *out_8638683365743472914);
|
||||
void car_H_30(double *state, double *unused, double *out_8413563967727305003);
|
||||
void car_h_26(double *state, double *unused, double *out_989338192432138572);
|
||||
void car_H_26(double *state, double *unused, double *out_6421015734251430459);
|
||||
void car_h_27(double *state, double *unused, double *out_7375345222001897423);
|
||||
void car_H_27(double *state, double *unused, double *out_7858416794181821702);
|
||||
void car_h_29(double *state, double *unused, double *out_4806387440305233057);
|
||||
void car_H_29(double *state, double *unused, double *out_6145054067312270669);
|
||||
void car_h_28(double *state, double *unused, double *out_6025091355491401381);
|
||||
void car_H_28(double *state, double *unused, double *out_8108684338877596920);
|
||||
void car_h_31(double *state, double *unused, double *out_2208042107618306896);
|
||||
void car_H_31(double *state, double *unused, double *out_8253579058707104505);
|
||||
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_628539194751276084);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5785405288888399451);
|
||||
void pose_H_mod_fun(double *state, double *out_1308901075637477725);
|
||||
void pose_f_fun(double *state, double dt, double *out_8416334481406024670);
|
||||
void pose_F_fun(double *state, double dt, double *out_5288884962144890875);
|
||||
void pose_h_4(double *state, double *unused, double *out_90872324407998874);
|
||||
void pose_H_4(double *state, double *unused, double *out_7635460590420343617);
|
||||
void pose_h_10(double *state, double *unused, double *out_729559215867171711);
|
||||
void pose_H_10(double *state, double *unused, double *out_4724811786445925588);
|
||||
void pose_h_13(double *state, double *unused, double *out_6088791521295825946);
|
||||
void pose_H_13(double *state, double *unused, double *out_4423186765088010816);
|
||||
void pose_h_14(double *state, double *unused, double *out_1848639786756728796);
|
||||
void pose_H_14(double *state, double *unused, double *out_7728495050993835703);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_8315489890310503187);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6008596051390826841);
|
||||
void pose_H_mod_fun(double *state, double *out_5634677350720166788);
|
||||
void pose_f_fun(double *state, double dt, double *out_5401222257802876036);
|
||||
void pose_F_fun(double *state, double dt, double *out_1313183186567530475);
|
||||
void pose_h_4(double *state, double *unused, double *out_717943751669182438);
|
||||
void pose_H_4(double *state, double *unused, double *out_7659548337950304093);
|
||||
void pose_h_10(double *state, double *unused, double *out_5351201218378086589);
|
||||
void pose_H_10(double *state, double *unused, double *out_5063285185285945841);
|
||||
void pose_h_13(double *state, double *unused, double *out_8511287818621351769);
|
||||
void pose_H_13(double *state, double *unused, double *out_4447274512617971292);
|
||||
void pose_h_14(double *state, double *unused, double *out_7796170631529448638);
|
||||
void pose_H_14(double *state, double *unused, double *out_3696307481610819564);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
@@ -382,6 +382,11 @@ class StarPilotLongitudinalTuneLayout(_SettingsPage):
|
||||
get_state=lambda: self._params.get_bool("TacoTune"),
|
||||
set_state=lambda s: self._params.put_bool("TacoTune", s),
|
||||
visible=self._longitudinal_enabled),
|
||||
SettingRow("NavLongitudinalAllowed", "toggle", tr_noop("Use Route Speed Control"),
|
||||
subtitle=tr_noop("Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts."),
|
||||
get_state=lambda: self._params.get_bool("NavLongitudinalAllowed"),
|
||||
set_state=lambda s: self._params.put_bool("NavLongitudinalAllowed", s),
|
||||
visible=self._longitudinal_enabled),
|
||||
], column_pair="detection_tune"),
|
||||
]
|
||||
self._manager_view = AetherSettingsView(
|
||||
|
||||
Binary file not shown.
@@ -42,6 +42,7 @@ SAFE_MODE_MANAGED_KEYS = (
|
||||
"NNFFLite",
|
||||
"TurnDesires",
|
||||
"NavDesiresAllowed",
|
||||
"NavLongitudinalAllowed",
|
||||
"QOLLateral",
|
||||
"PauseLateralSpeed",
|
||||
"PauseLateralOnSignal",
|
||||
|
||||
@@ -1050,6 +1050,7 @@ class StarPilotVariables:
|
||||
if longitudinal_tuning and self.params.get("CoastUpToLeads") is None:
|
||||
toggle.coast_up_to_leads = True
|
||||
toggle.human_lane_changes = has_radar and self.get_value("HumanLaneChanges", condition=longitudinal_tuning)
|
||||
toggle.nav_longitudinal_allowed = toggle.openpilot_longitudinal and self.get_value("NavLongitudinalAllowed", condition=longitudinal_tuning)
|
||||
# Keep lead detection sensitivity normalized even when longitudinal tuning is disabled.
|
||||
# Some branches can return raw integer defaults (e.g. 35) when condition=False.
|
||||
lead_detection_probability = self.get_value("LeadDetectionThreshold", cast=float, condition=toggle.openpilot_longitudinal,
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
import json
|
||||
import math
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
@@ -10,6 +11,16 @@ from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitCo
|
||||
|
||||
CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
|
||||
OVERRIDE_FORCE_STOP_TIMER = 10
|
||||
NAV_TURN_COMFORT_DECEL = 1.25
|
||||
NAV_TURN_DISTANCE_BUFFER = 8.0
|
||||
NAV_TURN_MIN_TARGET_DELTA = 0.25
|
||||
NAV_TURN_TARGET_SPEEDS = {
|
||||
"uturn": 5.0 * CV.MPH_TO_MS,
|
||||
"sharpLeft": 10.0 * CV.MPH_TO_MS,
|
||||
"sharpRight": 10.0 * CV.MPH_TO_MS,
|
||||
"left": 14.0 * CV.MPH_TO_MS,
|
||||
"right": 14.0 * CV.MPH_TO_MS,
|
||||
}
|
||||
|
||||
# Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset,
|
||||
# in feet); positive = stop later/longer, negative = stop sooner/shorter. All other
|
||||
@@ -58,6 +69,99 @@ class StarPilotVCruise:
|
||||
self.tracked_model_length = 0.0
|
||||
|
||||
self.stop_sign_confirmed = False
|
||||
self.nav_turn_target = 0.0
|
||||
self._nav_instruction_state_raw = None
|
||||
self._nav_instruction_state = {}
|
||||
|
||||
def _update_nav_instruction_state(self):
|
||||
raw = self.starpilot_planner.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_maneuver_target_speed(maneuver_type, maneuver_modifier):
|
||||
maneuver_type = str(maneuver_type or "").strip().lower()
|
||||
maneuver_modifier = str(maneuver_modifier or "").strip()
|
||||
|
||||
if not maneuver_modifier and not maneuver_type:
|
||||
return None
|
||||
|
||||
if maneuver_modifier == "uturn" or "uturn" in maneuver_type or "u-turn" in maneuver_type:
|
||||
return NAV_TURN_TARGET_SPEEDS["uturn"]
|
||||
|
||||
if "roundabout" in maneuver_type or "rotary" in maneuver_type:
|
||||
return 12.0 * CV.MPH_TO_MS
|
||||
|
||||
if maneuver_type == "turn":
|
||||
return NAV_TURN_TARGET_SPEEDS.get(maneuver_modifier)
|
||||
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _nav_target_for_distance(target_speed, maneuver_distance):
|
||||
try:
|
||||
remaining_distance = max(float(maneuver_distance) - NAV_TURN_DISTANCE_BUFFER, 0.0)
|
||||
except (TypeError, ValueError):
|
||||
return 0.0
|
||||
|
||||
return math.sqrt(max(target_speed * target_speed + (2.0 * NAV_TURN_COMFORT_DECEL * remaining_distance), 0.0))
|
||||
|
||||
@staticmethod
|
||||
def _get_nav_long_min_target_speed(sm):
|
||||
car_params = sm.get("carParams")
|
||||
return max(float(getattr(car_params, "minSteerSpeed", 0.0) or 0.0), 0.0)
|
||||
|
||||
def _get_nav_turn_control_target(self, v_cruise, sm, starpilot_toggles):
|
||||
self._update_nav_instruction_state()
|
||||
if not getattr(starpilot_toggles, "nav_longitudinal_allowed", False):
|
||||
return 0.0
|
||||
if not bool(self._nav_instruction_state.get("valid", False)):
|
||||
return 0.0
|
||||
|
||||
nav_long_min_target_speed = self._get_nav_long_min_target_speed(sm)
|
||||
|
||||
candidates = [
|
||||
(
|
||||
self._nav_instruction_state.get("maneuverType"),
|
||||
self._nav_instruction_state.get("maneuverModifier"),
|
||||
self._nav_instruction_state.get("maneuverDistance"),
|
||||
),
|
||||
(
|
||||
self._nav_instruction_state.get("nextManeuverType"),
|
||||
self._nav_instruction_state.get("nextManeuverModifier"),
|
||||
self._nav_instruction_state.get("nextManeuverDistance"),
|
||||
),
|
||||
]
|
||||
for maneuver_type, maneuver_modifier, maneuver_distance in candidates:
|
||||
target_speed = self._nav_maneuver_target_speed(maneuver_type, maneuver_modifier)
|
||||
if target_speed is None:
|
||||
continue
|
||||
target_speed = max(target_speed, nav_long_min_target_speed)
|
||||
|
||||
target = max(target_speed, self._nav_target_for_distance(target_speed, maneuver_distance))
|
||||
if target + NAV_TURN_MIN_TARGET_DELTA < v_cruise:
|
||||
return target
|
||||
|
||||
return 0.0
|
||||
|
||||
# ===== Main update =====
|
||||
|
||||
@@ -176,6 +280,8 @@ class StarPilotVCruise:
|
||||
self.slc_offset = 0
|
||||
self.slc_target = 0
|
||||
|
||||
self.nav_turn_target = self._get_nav_turn_control_target(v_cruise, sm, starpilot_toggles)
|
||||
|
||||
# Single tuning knob (signed feet -> meters). Defense clamp on top of UI bounds.
|
||||
offset_ft_raw = int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0)
|
||||
offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw))
|
||||
@@ -215,7 +321,9 @@ class StarPilotVCruise:
|
||||
|
||||
self.tracked_model_length = self.starpilot_planner.model_length
|
||||
|
||||
targets = [self.csc_target, v_cruise]
|
||||
targets = [v_cruise]
|
||||
if self.csc_target >= CSC_MIN_SPEED:
|
||||
targets.append(self.csc_target)
|
||||
slc_control_target = get_active_slc_control_target(
|
||||
starpilot_toggles.speed_limit_controller,
|
||||
getattr(starpilot_toggles, "set_speed_limit", False),
|
||||
@@ -224,8 +332,10 @@ class StarPilotVCruise:
|
||||
self.slc.overridden_speed,
|
||||
v_ego_diff,
|
||||
)
|
||||
if slc_control_target > 0.0:
|
||||
if slc_control_target >= CSC_MIN_SPEED:
|
||||
targets.append(slc_control_target)
|
||||
v_cruise = min([target if target >= CSC_MIN_SPEED else v_cruise for target in targets])
|
||||
if self.nav_turn_target > 0.0:
|
||||
targets.append(self.nav_turn_target)
|
||||
v_cruise = min(targets)
|
||||
|
||||
return v_cruise
|
||||
|
||||
@@ -263,6 +263,7 @@ class Navigationd:
|
||||
"maneuverDistance": float(payload.get("maneuverDistance") or 0.0),
|
||||
"nextManeuverType": str(next_maneuver.get("type") or ""),
|
||||
"nextManeuverModifier": str(next_maneuver.get("modifier") or ""),
|
||||
"nextManeuverDistance": float(next_maneuver.get("distance") or 0.0),
|
||||
}
|
||||
if state != self._last_nav_state:
|
||||
self.params_memory.put_nonblocking("NavInstructionState", state)
|
||||
|
||||
@@ -1031,6 +1031,14 @@
|
||||
"ui_type": "toggle",
|
||||
"parent_key": "LongitudinalTune"
|
||||
},
|
||||
{
|
||||
"key": "NavLongitudinalAllowed",
|
||||
"label": "Use Route Speed Control",
|
||||
"description": "Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts.",
|
||||
"data_type": "bool",
|
||||
"ui_type": "toggle",
|
||||
"parent_key": "LongitudinalTune"
|
||||
},
|
||||
{
|
||||
"key": "QOLLongitudinal",
|
||||
"label": "Quality of Life",
|
||||
|
||||
@@ -151,6 +151,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
{"HumanLaneChanges", tr("Human-Like Lane Changes"), tr("<b>Lane-change behavior that mimics human drivers</b> by anticipating and tracking adjacent vehicles during lane changes."), ""},
|
||||
{"LeadDetectionThreshold", tr("Lead Detection Sensitivity"), tr("<b>How sensitive openpilot is to detecting vehicles.</b> Higher sensitivity allows quicker detection at longer distances but may react to non-vehicle objects; lower sensitivity is more conservative and reduces false detections."), ""},
|
||||
{"TacoTune", tr("\"Taco Bell Run\" Turn Speed Hack"), tr("<b>The turn-speed hack from comma's 2022 \"Taco Bell Run\".</b> Designed to slow down for left and right turns."), ""},
|
||||
{"NavLongitudinalAllowed", tr("Use Route Speed Control"), tr("<b>Allow an active navigation route to reduce cruise speed for upcoming turns, ramps, and roundabouts.</b>"), ""},
|
||||
|
||||
{"QOLLongitudinal", tr("Quality of Life"), tr("<b>Miscellaneous acceleration and braking control changes</b> to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_quality_of_life.png"},
|
||||
{"CustomCruise", tr("Cruise Interval"), tr("<b>How much the set speed increases or decreases</b> for each + or – cruise control button press."), ""},
|
||||
|
||||
@@ -33,7 +33,7 @@ private:
|
||||
QSet<QString> conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"};
|
||||
QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
|
||||
QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
|
||||
QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"};
|
||||
QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune", "NavLongitudinalAllowed"};
|
||||
QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
|
||||
QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
|
||||
QSet<QString> speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"};
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user