mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-15 10:53:42 +08:00
Compare commits
4 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 36f1a62116 | |||
| becc36d2f0 | |||
| 48b11c0a7d | |||
| e3b50553c6 |
@@ -2,5 +2,4 @@ Wen
|
||||
REGIST
|
||||
PullRequest
|
||||
cancelled
|
||||
indeces
|
||||
FOF
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
name: Build and push all tinygrad models
|
||||
name: Build and push all tinygrad models
|
||||
|
||||
on:
|
||||
workflow_dispatch:
|
||||
@@ -57,7 +57,7 @@ jobs:
|
||||
env:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai3.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
git checkout main
|
||||
git sparse-checkout set --no-cone models/
|
||||
@@ -174,7 +174,7 @@ jobs:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
echo "Cloning GitLab"
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai3.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
echo "checkout models/${RECOMPILED_DIR}"
|
||||
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}
|
||||
|
||||
@@ -109,7 +109,7 @@ jobs:
|
||||
GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
|
||||
run: |
|
||||
echo "Cloning GitLab"
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
|
||||
git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai3.git gitlab_docs
|
||||
cd gitlab_docs
|
||||
echo "checkout models/${RECOMPILED_DIR}"
|
||||
git sparse-checkout set --no-cone models/${RECOMPILED_DIR}
|
||||
|
||||
@@ -153,7 +153,6 @@ jobs:
|
||||
}
|
||||
}' -F label="is:pr is:open label:${PR_LABEL} draft:false sort:created-asc")
|
||||
|
||||
PR_LIST=${PR_LIST//\'/}
|
||||
echo "PR_LIST=${PR_LIST}" >> $GITHUB_OUTPUT
|
||||
env:
|
||||
GH_TOKEN: ${{ secrets.GITHUB_TOKEN }}
|
||||
|
||||
Generated
-7
@@ -21,12 +21,5 @@
|
||||
</clean>
|
||||
</configuration>
|
||||
</target>
|
||||
<target id="f2590b2b-9b93-49f9-8510-da3f3724a2ae" name="replay" defaultType="TOOL">
|
||||
<configuration id="d475264f-6f4c-4092-9b4e-6773309f38b7" name="replay" toolchainName="Default">
|
||||
<build type="TOOL">
|
||||
<tool actionId="Tool_External Tools_uv build tools replay" />
|
||||
</build>
|
||||
</configuration>
|
||||
</target>
|
||||
</component>
|
||||
</project>
|
||||
Generated
-7
@@ -20,11 +20,4 @@
|
||||
<option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
|
||||
</exec>
|
||||
</tool>
|
||||
<tool name="uv build tools replay" showInMainMenu="false" showInEditor="false" showInProject="false" showInSearchPopup="false" disabled="false" useConsole="true" showConsoleOnStdOut="false" showConsoleOnStdErr="false" synchronizeAfterRun="true">
|
||||
<exec>
|
||||
<option name="COMMAND" value="bash" />
|
||||
<option name="PARAMETERS" value="-c "source .venv/bin/activate && scons -u -j$(nproc) tools/replay/"" />
|
||||
<option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
|
||||
</exec>
|
||||
</tool>
|
||||
</toolSet>
|
||||
@@ -1,5 +1,5 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
|
||||
<configuration default="false" name="Build Debug" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="false" WORKING_DIR="file://$ProjectFileDir$/selfdrive/ui" PASS_PARENT_ENVS_2="true" PROJECT_NAME="sunnypilot" TARGET_NAME="uv Scons Build Debug" CONFIG_NAME="uv Scons Build Debug" RUN_PATH="ui">
|
||||
<envs>
|
||||
<env name="QT_DBL_CLICK_DIST" value="150" />
|
||||
</envs>
|
||||
|
||||
@@ -1,27 +0,0 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Debug Route Controls" type="PythonConfigurationType" factoryName="Python">
|
||||
<module name="openpilot-special" />
|
||||
<option name="ENV_FILES" value="" />
|
||||
<option name="INTERPRETER_OPTIONS" value="" />
|
||||
<option name="PARENT_ENVS" value="true" />
|
||||
<envs>
|
||||
<env name="PYTHONUNBUFFERED" value="1" />
|
||||
<env name="FINGERPRINT" value="KIA_EV9" />
|
||||
<env name="SKIP_FW_QUERY" value="1" />
|
||||
</envs>
|
||||
<option name="SDK_HOME" value="" />
|
||||
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/selfdrive/car" />
|
||||
<option name="IS_MODULE_SDK" value="true" />
|
||||
<option name="ADD_CONTENT_ROOTS" value="true" />
|
||||
<option name="ADD_SOURCE_ROOTS" value="true" />
|
||||
<EXTENSION ID="PythonCoverageRunConfigurationExtension" runner="coverage.py" />
|
||||
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/selfdrive/car/card.py" />
|
||||
<option name="PARAMETERS" value="" />
|
||||
<option name="SHOW_COMMAND_LINE" value="false" />
|
||||
<option name="EMULATE_TERMINAL" value="true" />
|
||||
<option name="MODULE_MODE" value="false" />
|
||||
<option name="REDIRECT_INPUT" value="false" />
|
||||
<option name="INPUT_FILE" value="" />
|
||||
<method v="2" />
|
||||
</configuration>
|
||||
</component>
|
||||
@@ -1,7 +0,0 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="Replay for controls + ui" type="Multirun" separateTabs="false" reuseTabsWithFailures="false" startOneByOne="true" markFailedProcess="true" hideSuccessProcess="false" delayTime="0.0">
|
||||
<runConfiguration name="replay for controls" type="Native Application" />
|
||||
<runConfiguration name="Build Debug" type="Custom Build Application" />
|
||||
<method v="2" />
|
||||
</configuration>
|
||||
</component>
|
||||
@@ -1,7 +0,0 @@
|
||||
<component name="ProjectRunConfigurationManager">
|
||||
<configuration default="false" name="replay for controls" type="CLionNativeAppRunConfigurationType" focusToolWindowBeforeRun="true" PROGRAM_PARAMS=""$Prompt$" --block "sendcan,carState,carParams,carOutput,liveTracks,carParamsSP,carStateSP"" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" EMULATE_TERMINAL="true" WORKING_DIR="file://$ProjectFileDir$/tools/replay" PASS_PARENT_ENVS_2="true" PROJECT_NAME="openpilot-special" TARGET_NAME="replay" CONFIG_NAME="replay" version="1" RUN_PATH="replay">
|
||||
<method v="2">
|
||||
<option name="CLION.COMPOUND.BUILD" enabled="true" />
|
||||
</method>
|
||||
</configuration>
|
||||
</component>
|
||||
@@ -101,6 +101,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
|
||||
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
dec @0 :DynamicExperimentalControl;
|
||||
accelPersonality @1 :AccelerationPersonality;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -112,6 +113,12 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
enum AccelerationPersonality {
|
||||
sport @0;
|
||||
normal @1;
|
||||
eco @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -123,6 +123,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"Version", PERSISTENT},
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"AccelPersonality", PERSISTENT},
|
||||
{"ApiCache_DriveStats", PERSISTENT},
|
||||
{"AutoLaneChangeBsmDelay", PERSISTENT | BACKUP},
|
||||
{"AutoLaneChangeTimer", PERSISTENT | BACKUP},
|
||||
@@ -147,6 +148,9 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"QuickBootToggle", PERSISTENT | BACKUP},
|
||||
{"QuietMode", PERSISTENT | BACKUP},
|
||||
{"ShowAdvancedControls", PERSISTENT | BACKUP},
|
||||
{"VibePersonalityEnabled", PERSISTENT},
|
||||
{"VibeAccelPersonalityEnabled", PERSISTENT},
|
||||
{"VibeFollowPersonalityEnabled", PERSISTENT},
|
||||
|
||||
// MADS params
|
||||
{"Mads", PERSISTENT | BACKUP},
|
||||
@@ -207,13 +211,4 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"OsmStateTitle", PERSISTENT},
|
||||
{"OsmWayTest", PERSISTENT},
|
||||
{"RoadName", CLEAR_ON_ONROAD_TRANSITION},
|
||||
|
||||
// Tuning keys
|
||||
{"EnableHkgTuningAngleSmoothingFactor", PERSISTENT | BACKUP},
|
||||
{"HkgTuningAngleMinTorqueReductionGain", PERSISTENT | BACKUP},
|
||||
{"HkgTuningAngleMaxTorqueReductionGain", PERSISTENT | BACKUP},
|
||||
{"HkgTuningAngleActiveTorqueReductionGain", PERSISTENT | BACKUP},
|
||||
{"HkgTuningOverridingCycles", PERSISTENT | BACKUP},
|
||||
{"HkgAngleLiveTuning", CLEAR_ON_MANAGER_START}
|
||||
|
||||
};
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: 038b75cef4...b68fab9ea4
+2
-1
@@ -59,7 +59,7 @@ dependencies = [
|
||||
"future-fstrings",
|
||||
|
||||
# joystickd
|
||||
"pygame",
|
||||
"inputs",
|
||||
|
||||
# these should be removed
|
||||
"psutil",
|
||||
@@ -108,6 +108,7 @@ dev = [
|
||||
"opencv-python-headless",
|
||||
"parameterized >=0.8, <0.9",
|
||||
"pyautogui",
|
||||
"pygame",
|
||||
"pyopencl; platform_machine != 'aarch64'", # broken on arm64
|
||||
"pytools < 2024.1.11; platform_machine != 'aarch64'", # pyopencl use a broken version
|
||||
"pywinctl",
|
||||
|
||||
@@ -13,7 +13,7 @@ cd $ROOT
|
||||
|
||||
FAILED=0
|
||||
|
||||
IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md|layouts\/.*\.xml"
|
||||
IGNORED_FILES="uv\.lock|docs\/CARS.md|LICENSE\.md"
|
||||
IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*"
|
||||
|
||||
function run() {
|
||||
|
||||
@@ -17,7 +17,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle_torque import LatControlAngleTorque
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
@@ -59,9 +58,7 @@ class Controls(ControlsExt):
|
||||
self.LoC = LongControl(self.CP)
|
||||
self.VM = VehicleModel(self.CP)
|
||||
self.LaC: LatControl
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle and self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlAngleTorque(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
|
||||
|
||||
@@ -1,13 +0,0 @@
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
|
||||
|
||||
class LatControlAngleTorque(LatControlTorque, LatControlAngle):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
LatControlTorque.__init__(self, CP, CP_SP, CI)
|
||||
LatControlAngle.__init__(self, CP, CP_SP, CI)
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
|
||||
torque, _, _ = LatControlTorque.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
|
||||
_, angle, angle_log = LatControlAngle.update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited)
|
||||
return torque, angle, angle_log
|
||||
@@ -10,6 +10,8 @@ from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.modeld.constants import index_function
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
|
||||
|
||||
if __name__ == '__main__': # generating code
|
||||
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
else:
|
||||
@@ -228,6 +230,7 @@ class LongitudinalMpc:
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.reset()
|
||||
self.source = SOURCES[2]
|
||||
self.vibe_controller = VibePersonalityController()
|
||||
|
||||
def reset(self):
|
||||
# self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
@@ -328,10 +331,31 @@ class LongitudinalMpc:
|
||||
return lead_xv
|
||||
|
||||
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
v_ego = self.x0[1]
|
||||
|
||||
# Get following distance
|
||||
if self.vibe_controller.is_follow_enabled():
|
||||
t_follow = self.vibe_controller.get_follow_distance_multiplier(v_ego)
|
||||
if t_follow is None:
|
||||
# Fallback to stock behavior when vibe controller can't provide a value
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
else:
|
||||
t_follow = get_T_FOLLOW(personality)
|
||||
|
||||
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
|
||||
|
||||
# Get acceleration limits
|
||||
if self.vibe_controller.is_accel_enabled():
|
||||
accel_limits = self.vibe_controller.get_accel_limits(v_ego)
|
||||
if accel_limits is not None:
|
||||
min_accel = accel_limits[0]
|
||||
else:
|
||||
min_accel = CRUISE_MIN_ACCEL
|
||||
else:
|
||||
min_accel = CRUISE_MIN_ACCEL
|
||||
|
||||
a_cruise_min = min_accel
|
||||
|
||||
lead_xv_0 = self.process_lead(radarstate.leadOne)
|
||||
lead_xv_1 = self.process_lead(radarstate.leadTwo)
|
||||
|
||||
@@ -350,7 +374,7 @@ class LongitudinalMpc:
|
||||
|
||||
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
|
||||
# when the leads are no factor.
|
||||
v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
|
||||
v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
|
||||
# TODO does this make sense when max_a is negative?
|
||||
v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
|
||||
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
|
||||
|
||||
@@ -124,9 +124,22 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
|
||||
|
||||
if self.mode == 'acc':
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
|
||||
if self.vibe_controller.is_accel_enabled():
|
||||
# Only get max acceleration from vibe controller, use default ACCEL_MIN for minimum
|
||||
accel_limits = self.vibe_controller.get_accel_limits(v_ego)
|
||||
if accel_limits is not None:
|
||||
max_accel = accel_limits[1]
|
||||
accel_clip = [ACCEL_MIN, max_accel]
|
||||
else:
|
||||
# Fallback to stock if vibe controller returns None
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
# Recalculate limit turn according to the new max limit
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
|
||||
else:
|
||||
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
|
||||
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
|
||||
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
|
||||
else:
|
||||
accel_clip = [ACCEL_MIN, ACCEL_MAX]
|
||||
|
||||
|
||||
@@ -92,7 +92,15 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
"your steering wheel distance button."),
|
||||
"../assets/icons/speed_limit.png",
|
||||
longi_button_texts);
|
||||
|
||||
// accel controller
|
||||
std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco")};
|
||||
accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"),
|
||||
tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. "
|
||||
"In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these "
|
||||
"acceleration personality within Onroad Settings on the driving screen."),
|
||||
"",
|
||||
accel_personality_texts);
|
||||
accel_personality_setting->showDescription();
|
||||
// set up uiState update for personality setting
|
||||
QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
|
||||
|
||||
@@ -120,6 +128,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
|
||||
// insert longitudinal personality after NDOG toggle
|
||||
if (param == "DisengageOnAccelerator") {
|
||||
addItem(long_personality_setting);
|
||||
addItem(accel_personality_setting);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -140,6 +149,13 @@ void TogglesPanel::updateState(const UIState &s) {
|
||||
}
|
||||
uiState()->scene.personality = personality;
|
||||
}
|
||||
if (sm.updated("longitudinalPlanSP")) {
|
||||
auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality();
|
||||
if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) {
|
||||
accel_personality_setting->setCheckedButton(static_cast<int>(accel_personality));
|
||||
}
|
||||
uiState()->scene.accel_personality = accel_personality;
|
||||
}
|
||||
}
|
||||
|
||||
void TogglesPanel::expandToggleDescription(const QString ¶m) {
|
||||
@@ -186,10 +202,12 @@ void TogglesPanel::updateToggles() {
|
||||
experimental_mode_toggle->setEnabled(true);
|
||||
experimental_mode_toggle->setDescription(e2e_description);
|
||||
long_personality_setting->setEnabled(true);
|
||||
accel_personality_setting->setEnabled(true);
|
||||
} else {
|
||||
// no long for now
|
||||
experimental_mode_toggle->setEnabled(false);
|
||||
long_personality_setting->setEnabled(false);
|
||||
accel_personality_setting->setEnabled(true);
|
||||
params.remove("ExperimentalMode");
|
||||
|
||||
const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.");
|
||||
|
||||
@@ -88,6 +88,7 @@ protected:
|
||||
Params params;
|
||||
std::map<std::string, ParamControl*> toggles;
|
||||
ButtonParamControl *long_personality_setting;
|
||||
ButtonParamControl *accel_personality_setting;
|
||||
|
||||
virtual void updateToggles();
|
||||
};
|
||||
|
||||
@@ -34,7 +34,6 @@ void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
|
||||
drawLead(painter, lead_two, lead_vertices[1], surface_rect);
|
||||
}
|
||||
}
|
||||
drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
|
||||
|
||||
painter.restore();
|
||||
}
|
||||
@@ -175,172 +174,6 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
|
||||
}
|
||||
|
||||
|
||||
void ModelRenderer::drawLeadStatus(QPainter &painter, int height, int width) {
|
||||
auto *s = uiState();
|
||||
auto &sm = *(s->sm);
|
||||
|
||||
if (!sm.alive("radarState")) return;
|
||||
|
||||
const auto &radar_state = sm["radarState"].getRadarState();
|
||||
const auto &lead_one = radar_state.getLeadOne();
|
||||
const auto &lead_two = radar_state.getLeadTwo();
|
||||
|
||||
// Check if we have any active leads
|
||||
bool has_lead_one = lead_one.getStatus();
|
||||
bool has_lead_two = lead_two.getStatus();
|
||||
|
||||
if (!has_lead_one && !has_lead_two) {
|
||||
// Fade out status display
|
||||
lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f);
|
||||
if (lead_status_alpha <= 0.0f) return;
|
||||
} else {
|
||||
// Fade in status display
|
||||
lead_status_alpha = std::min(1.0f, lead_status_alpha + 0.1f);
|
||||
}
|
||||
|
||||
// Draw status for each lead vehicle under its chevron
|
||||
if (true) {
|
||||
drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1");
|
||||
}
|
||||
|
||||
if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) {
|
||||
drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2");
|
||||
}
|
||||
}
|
||||
|
||||
void ModelRenderer::drawLeadStatusAtPosition(QPainter &painter,
|
||||
const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &chevron_pos,
|
||||
int height, int width,
|
||||
const QString &label) {
|
||||
|
||||
float d_rel = lead_data.getDRel();
|
||||
float v_rel = lead_data.getVRel();
|
||||
auto *s = uiState();
|
||||
auto &sm = *(s->sm);
|
||||
float v_ego = sm["carState"].getCarState().getVEgo();
|
||||
|
||||
int chevron_data = std::atoi(Params().get("ChevronInfo").c_str());
|
||||
|
||||
// Calculate chevron size (same logic as drawLead)
|
||||
float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
|
||||
|
||||
QFont content_font = painter.font();
|
||||
content_font.setPixelSize(35);
|
||||
content_font.setBold(true);
|
||||
painter.setFont(content_font);
|
||||
|
||||
QFontMetrics fm(content_font);
|
||||
bool is_metric = s->scene.is_metric;
|
||||
|
||||
QStringList text_lines;
|
||||
|
||||
const int chevron_types = 3;
|
||||
const int chevron_all = chevron_types + 1; // All metrics (value 4)
|
||||
QStringList chevron_text[chevron_types];
|
||||
int position;
|
||||
float val;
|
||||
|
||||
// Distance display (chevron_data == 1 or all)
|
||||
if (chevron_data == 1 || chevron_data == chevron_all) {
|
||||
position = 0;
|
||||
val = std::max(0.0f, d_rel);
|
||||
QString distance_unit = is_metric ? "m" : "ft";
|
||||
if (!is_metric) {
|
||||
val *= 3.28084f; // Convert meters to feet
|
||||
}
|
||||
chevron_text[position].append(QString::number(val, 'f', 0) + " " + distance_unit);
|
||||
}
|
||||
|
||||
// Absolute velocity display (chevron_data == 2 or all)
|
||||
if (chevron_data == 2 || chevron_data == chevron_all) {
|
||||
position = (chevron_data == 2) ? 0 : 1;
|
||||
val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast<float>(MS_TO_KPH) : static_cast<float>(MS_TO_MPH)));
|
||||
chevron_text[position].append(QString::number(val, 'f', 0) + " " + (is_metric ? "km/h" : "mph"));
|
||||
}
|
||||
|
||||
// Time-to-contact display (chevron_data == 3 or all)
|
||||
if (chevron_data == 3 || chevron_data == chevron_all) {
|
||||
position = (chevron_data == 3) ? 0 : 2;
|
||||
val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f;
|
||||
QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---";
|
||||
chevron_text[position].append(ttc_str);
|
||||
}
|
||||
|
||||
// Collect all non-empty text lines
|
||||
for (int i = 0; i < chevron_types; ++i) {
|
||||
if (!chevron_text[i].isEmpty()) {
|
||||
text_lines.append(chevron_text[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// If no text to display, return early
|
||||
if (text_lines.isEmpty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Text box dimensions
|
||||
float str_w = 150; // Width of text area
|
||||
float str_h = 45; // Height per line
|
||||
|
||||
// Position text below chevron, centered horizontally
|
||||
float text_x = chevron_pos.x() - str_w / 2;
|
||||
float text_y = chevron_pos.y() + sz + 15;
|
||||
|
||||
// Clamp to screen bounds
|
||||
text_x = std::clamp(text_x, 10.0f, (float)width - str_w - 10);
|
||||
|
||||
// Shadow offset
|
||||
QPoint shadow_offset(2, 2);
|
||||
|
||||
// Draw each line of text with shadow
|
||||
for (int i = 0; i < text_lines.size(); ++i) {
|
||||
if (!text_lines[i].isEmpty()) {
|
||||
QRect textRect(text_x, text_y + (i * str_h), str_w, str_h);
|
||||
|
||||
// Draw shadow
|
||||
painter.setPen(QColor(0x0, 0x0, 0x0, (int)(200 * lead_status_alpha)));
|
||||
painter.drawText(textRect.translated(shadow_offset.x(), shadow_offset.y()),
|
||||
Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
|
||||
|
||||
// Determine text color based on content and danger level
|
||||
QColor text_color;
|
||||
|
||||
// Check if this is a distance line (contains 'm' or 'ft')
|
||||
if (text_lines[i].contains("m") || text_lines[i].contains("ft")) {
|
||||
if (d_rel < 20.0f) {
|
||||
text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - danger
|
||||
} else if (d_rel < 40.0f) {
|
||||
text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution
|
||||
} else {
|
||||
text_color = QColor(80, 255, 120, (int)(255 * lead_status_alpha)); // Green - safe
|
||||
}
|
||||
}
|
||||
// Enhanced color coding for time-to-contact
|
||||
else if (text_lines[i].contains("s") && !text_lines[i].contains("---")) {
|
||||
float ttc_val = text_lines[i].left(text_lines[i].length() - 1).toFloat();
|
||||
if (ttc_val < 3.0f) {
|
||||
text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - urgent
|
||||
} else if (ttc_val < 6.0f) {
|
||||
text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution
|
||||
} else {
|
||||
text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White - safe
|
||||
}
|
||||
}
|
||||
else {
|
||||
text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White for other lines
|
||||
}
|
||||
|
||||
// Draw main text
|
||||
painter.setPen(text_color);
|
||||
painter.drawText(textRect, Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Reset pen
|
||||
painter.setPen(Qt::NoPen);
|
||||
}
|
||||
|
||||
void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &vd, const QRect &surface_rect) {
|
||||
const float speedBuff = 10.;
|
||||
@@ -402,4 +235,4 @@ void ModelRenderer::mapLineToPolygon(const cereal::XYZTData::Reader &line, float
|
||||
pvd->push_front(right);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -34,12 +34,6 @@ protected:
|
||||
bool mapToScreen(float in_x, float in_y, float in_z, QPointF *out);
|
||||
void mapLineToPolygon(const cereal::XYZTData::Reader &line, float y_off, float z_off,
|
||||
QPolygonF *pvd, int max_idx, bool allow_invert = true);
|
||||
void drawLeadStatus(QPainter &painter, int height, int width);
|
||||
void drawLeadStatusAtPosition(QPainter &painter,
|
||||
const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &chevron_pos,
|
||||
int height, int width,
|
||||
const QString &label);
|
||||
void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd, const QRect &surface_rect);
|
||||
void update_leads(const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
|
||||
virtual void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead);
|
||||
@@ -65,8 +59,4 @@ protected:
|
||||
Eigen::Matrix3f car_space_transform = Eigen::Matrix3f::Zero();
|
||||
QRectF clip_region;
|
||||
|
||||
float lead_status_alpha = 0.0f;
|
||||
QPointF lead_status_pos;
|
||||
QString lead_status_text;
|
||||
QColor lead_status_color;
|
||||
};
|
||||
|
||||
@@ -4,7 +4,6 @@ widgets_src = [
|
||||
"sunnypilot/qt/widgets/controls.cc",
|
||||
"sunnypilot/qt/widgets/drive_stats.cc",
|
||||
"sunnypilot/qt/widgets/expandable_row.cc",
|
||||
"sunnypilot/qt/widgets/external_storage.cc",
|
||||
"sunnypilot/qt/widgets/prime.cc",
|
||||
"sunnypilot/qt/widgets/scrollview.cc",
|
||||
"sunnypilot/qt/network/networking.cc",
|
||||
@@ -48,7 +47,6 @@ lateral_panel_qt_src = [
|
||||
"sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc",
|
||||
"sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc",
|
||||
"sunnypilot/qt/offroad/settings/lateral/mads_settings.cc",
|
||||
"sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.cc",
|
||||
"sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc",
|
||||
]
|
||||
|
||||
|
||||
@@ -5,14 +5,9 @@
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/developer_panel.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/external_storage.h"
|
||||
|
||||
DeveloperPanelSP::DeveloperPanelSP(SettingsWindow *parent) : DeveloperPanel(parent) {
|
||||
|
||||
#ifndef __APPLE__
|
||||
addItem(new ExternalStorageControl());
|
||||
#endif
|
||||
|
||||
// Advanced Controls Toggle
|
||||
showAdvancedControls = new ParamControlSP("ShowAdvancedControls", tr("Show Advanced Controls"), tr("Toggle visibility of advanced sunnypilot controls.\nThis only toggles the visibility of the controls; it does not toggle the actual control enabled/disabled state."), "");
|
||||
addItem(showAdvancedControls);
|
||||
|
||||
@@ -1,87 +0,0 @@
|
||||
/**
|
||||
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
*
|
||||
* This file is part of sunnypilot and is licensed under the MIT License.
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h"
|
||||
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
|
||||
|
||||
AngleTunningSettings::AngleTunningSettings(QWidget *parent) : QWidget(parent) {
|
||||
QVBoxLayout *main_layout = new QVBoxLayout(this);
|
||||
main_layout->setContentsMargins(50, 20, 50, 20);
|
||||
main_layout->setSpacing(20);
|
||||
|
||||
// Back button
|
||||
PanelBackButton *back = new PanelBackButton();
|
||||
connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
|
||||
main_layout->addWidget(back, 0, Qt::AlignLeft);
|
||||
|
||||
auto *list = new ListWidgetSP(this, false);
|
||||
|
||||
main_layout->addWidget(new QWidget());
|
||||
|
||||
enableHkgAngleSmoothingFactor = new ExpandableToggleRow("EnableHkgTuningAngleSmoothingFactor", tr("HKG Angle Smoothing Factor"), tr("Applies EMA (Exponential Moving Average) to the desired angle steering and avoid overcorrections."), "../assets/offroad/icon_blank.png");
|
||||
list->addItem(enableHkgAngleSmoothingFactor);
|
||||
|
||||
auto first_row = new QHBoxLayout();
|
||||
hkgTuningOverridingCycles = new OptionControlSP("HkgTuningOverridingCycles", tr("Override Ramp-Down Cycles"), tr("Number of cycles to ramp down the current amount of torque on the steering wheel.<br/>A smaller value means a faster override by the user (less effort)"), "../assets/offroad/icon_blank.png", {10, 30}, 1);
|
||||
connect(hkgTuningOverridingCycles, &OptionControlSP::updateLabels, hkgTuningOverridingCycles, [=]() {
|
||||
this->updateToggles(offroad);
|
||||
});
|
||||
first_row->addWidget(hkgTuningOverridingCycles);
|
||||
|
||||
hkgAngleMinTorque = new OptionControlSP("HkgTuningAngleMinTorqueReductionGain", tr("Override Steering Effort"), tr("Sets the steering effort percentage used when the driver is overriding lateral control.<br/>Higher values increase resistance and make the wheel feel stiffer."), "../assets/offroad/icon_blank.png", {5, 60}, 1);
|
||||
connect(hkgAngleMinTorque, &OptionControlSP::updateLabels, hkgAngleMinTorque, [=]() {
|
||||
this->updateToggles(offroad);
|
||||
});
|
||||
first_row->addWidget(hkgAngleMinTorque);
|
||||
list->addItem(first_row);
|
||||
|
||||
auto second_row = new QHBoxLayout();
|
||||
hkgAngleActiveTorque = new OptionControlSP("HkgTuningAngleActiveTorqueReductionGain", tr("Min Active Torque"), tr("Torque applied when lateral control is active but the vehicle is not turning.<br/>Used to maintain lane centering on straight paths when no user input is detected."), "../assets/offroad/icon_blank.png", {10, 100}, 1);
|
||||
connect(hkgAngleActiveTorque, &OptionControlSP::updateLabels, hkgAngleActiveTorque, [=]() {
|
||||
this->updateToggles(offroad);
|
||||
});
|
||||
second_row->addWidget(hkgAngleActiveTorque);
|
||||
|
||||
hkgAngleMaxTorque = new OptionControlSP("HkgTuningAngleMaxTorqueReductionGain", tr("Max Torque Allowance"), tr("Sets the maximum torque reduction percentage the controller can apply during normal lateral control.<br/>"), "../assets/offroad/icon_blank.png", {10, 100}, 1);
|
||||
connect(hkgAngleMaxTorque, &OptionControlSP::updateLabels, hkgAngleMaxTorque, [=]() {
|
||||
this->updateToggles(offroad);
|
||||
});
|
||||
second_row->addWidget(hkgAngleMaxTorque);
|
||||
list->addItem(second_row);
|
||||
|
||||
QObject::connect(uiState(), &UIState::offroadTransition, this, &AngleTunningSettings::updateToggles);
|
||||
|
||||
main_layout->addWidget(new ScrollViewSP(list, this));
|
||||
|
||||
auto *warning = new QLabel(tr("Reboot required for settings to apply; Tap on each setting to see more details."));
|
||||
warning->setStyleSheet("font-size: 30px; font-weight: 500; font-family: 'Noto Color Emoji'; color: orange;");
|
||||
main_layout->addWidget(warning, 0, Qt::AlignCenter);
|
||||
}
|
||||
|
||||
void AngleTunningSettings::showEvent(QShowEvent *event) {
|
||||
updateToggles(offroad);
|
||||
}
|
||||
|
||||
void AngleTunningSettings::updateToggles(bool _offroad) {
|
||||
auto HkgAngleSmoothingFactorValue = params.getBool("EnableHkgTuningAngleSmoothingFactor");
|
||||
enableHkgAngleSmoothingFactor->toggleFlipped(HkgAngleSmoothingFactorValue);
|
||||
|
||||
auto HkgAngleMinTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMinTorqueReductionGain")).toInt();
|
||||
hkgAngleMinTorque->setLabel(QString::number(HkgAngleMinTorqueValue)+"%");
|
||||
|
||||
auto HkgAngleActiveTorqueValue = QString::fromStdString(params.get("HkgTuningAngleActiveTorqueReductionGain")).toInt();
|
||||
hkgAngleActiveTorque->setLabel(QString::number(HkgAngleActiveTorqueValue)+"%");
|
||||
|
||||
auto HkgAngleMaxTorqueValue = QString::fromStdString(params.get("HkgTuningAngleMaxTorqueReductionGain")).toInt();
|
||||
hkgAngleMaxTorque->setLabel(QString::number(HkgAngleMaxTorqueValue)+"%");
|
||||
|
||||
auto HkgTuningOverridingCyclesValue = QString::fromStdString(params.get("HkgTuningOverridingCycles")).toInt();
|
||||
hkgTuningOverridingCycles->setLabel(QString::number(HkgTuningOverridingCyclesValue));
|
||||
|
||||
offroad = _offroad;
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
/**
|
||||
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
*
|
||||
* This file is part of sunnypilot and is licensed under the MIT License.
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
#include "selfdrive/ui/sunnypilot/ui.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h"
|
||||
|
||||
class AngleTunningSettings : public QWidget {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
explicit AngleTunningSettings(QWidget *parent = nullptr);
|
||||
|
||||
void showEvent(QShowEvent *event) override;
|
||||
|
||||
signals:
|
||||
void backPress();
|
||||
|
||||
public slots:
|
||||
void updateToggles(bool _offroad);
|
||||
|
||||
private:
|
||||
Params params;
|
||||
bool offroad;
|
||||
|
||||
ExpandableToggleRow* enableHkgAngleSmoothingFactor;
|
||||
OptionControlSP* hkgAngleMinTorque;
|
||||
OptionControlSP* hkgAngleActiveTorque;
|
||||
OptionControlSP* hkgAngleMaxTorque;
|
||||
OptionControlSP* hkgTuningOverridingCycles;
|
||||
ParamControlSP* hkgAngleLiveTuning;
|
||||
};
|
||||
@@ -89,36 +89,6 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
|
||||
nnlcToggle->updateToggle();
|
||||
});
|
||||
|
||||
#pragma region hkg angle tuning
|
||||
list->addItem(vertical_space());
|
||||
list->addItem(horizontal_line());
|
||||
list->addItem(vertical_space());
|
||||
|
||||
// HKG Angle Tuning
|
||||
// angleTuningToggle = new ParamControl(
|
||||
// "AngleTuning",
|
||||
// tr("Modular Assistive Driving System (MADS)"),
|
||||
// tr("Enable the beloved MADS feature. Disable toggle to revert back to stock sunnypilot engagement/disengagement."),
|
||||
// "");
|
||||
// angleTuningToggle->setConfirmation(true, false);
|
||||
// list->addItem(angleTuningToggle);
|
||||
|
||||
angleTuningSettingsButton = new PushButtonSP(tr("Customize ANGLE Tuning"));
|
||||
angleTuningSettingsButton->setObjectName("angle_btn");
|
||||
connect(angleTuningSettingsButton, &QPushButton::clicked, [=]() {
|
||||
sunnypilotScroller->setLastScrollPosition();
|
||||
main_layout->setCurrentWidget(angleTuningWidget);
|
||||
});
|
||||
// QObject::connect(angleTuningToggle, &ToggleControl::toggleFlipped, angleTuningSettingsButton, &PushButtonSP::setEnabled);
|
||||
|
||||
angleTuningWidget = new AngleTunningSettings(this);
|
||||
connect(angleTuningWidget, &AngleTunningSettings::backPress, [=]() {
|
||||
sunnypilotScroller->restoreScrollPosition();
|
||||
main_layout->setCurrentWidget(sunnypilotScreen);
|
||||
});
|
||||
list->addItem(angleTuningSettingsButton);
|
||||
#pragma endregion
|
||||
|
||||
toggleOffroadOnly = {
|
||||
madsToggle, nnlcToggle,
|
||||
};
|
||||
@@ -129,7 +99,6 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) {
|
||||
|
||||
main_layout->addWidget(sunnypilotScreen);
|
||||
main_layout->addWidget(madsWidget);
|
||||
main_layout->addWidget(angleTuningWidget);
|
||||
main_layout->addWidget(laneChangeWidget);
|
||||
|
||||
setStyleSheet(R"(
|
||||
@@ -180,7 +149,6 @@ void LateralPanel::updateToggles(bool _offroad) {
|
||||
}
|
||||
|
||||
madsSettingsButton->setEnabled(madsToggle->isToggled());
|
||||
// angleTuningSettingsButton->setEnabled(angleTuningToggle->isToggled());
|
||||
|
||||
blinkerPauseLateralSettings->refresh();
|
||||
|
||||
|
||||
@@ -13,7 +13,6 @@
|
||||
#include "selfdrive/ui/sunnypilot/ui.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/angle_tuning_settings.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h"
|
||||
#include "selfdrive/ui/qt/util.h"
|
||||
@@ -40,11 +39,8 @@ private:
|
||||
bool offroad;
|
||||
|
||||
ParamControl *madsToggle;
|
||||
// ParamControl *angleTuningToggle;
|
||||
PushButtonSP *madsSettingsButton;
|
||||
PushButtonSP *angleTuningSettingsButton;
|
||||
MadsSettings *madsWidget = nullptr;
|
||||
AngleTunningSettings *angleTuningWidget = nullptr;
|
||||
PushButtonSP *laneChangeSettingsButton;
|
||||
LaneChangeSettings *laneChangeWidget = nullptr;
|
||||
NeuralNetworkLateralControl *nnlcToggle = nullptr;
|
||||
|
||||
@@ -23,6 +23,35 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
|
||||
|
||||
QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh);
|
||||
|
||||
|
||||
// Vibe Personality Controller
|
||||
vibePersonalityControl = new ParamControlSP("VibePersonalityEnabled",
|
||||
tr("Vibe Personality Controller"),
|
||||
tr("Advanced driving personality system with separate controls for acceleration behavior (Eco/Normal/Sport) and following distance/braking (Relaxed/Standard/Aggressive). "
|
||||
"Customize your driving experience with independent acceleration and distance personalities."),
|
||||
"../assets/offroad/icon_shell.png");
|
||||
list->addItem(vibePersonalityControl);
|
||||
|
||||
connect(vibePersonalityControl, &ParamControlSP::toggleFlipped, [=]() {
|
||||
refresh(offroad);
|
||||
});
|
||||
|
||||
// Vibe Acceleration Personality
|
||||
vibeAccelPersonalityControl = new ParamControlSP("VibeAccelPersonalityEnabled",
|
||||
tr("Acceleration Personality"),
|
||||
tr("Controls acceleration behavior: Eco (efficient), Normal (balanced), Sport (responsive). "
|
||||
"Adjust how aggressively the vehicle accelerates while maintaining smooth operation."),
|
||||
"../assets/offroad/icon_shell.png");
|
||||
list->addItem(vibeAccelPersonalityControl);
|
||||
|
||||
// Vibe Following Distance Personality
|
||||
vibeFollowPersonalityControl = new ParamControlSP("VibeFollowPersonalityEnabled",
|
||||
tr("Following Distance Personality"),
|
||||
tr("Controls following distance and braking behavior: Relaxed (longer distance, gentler braking), Standard (balanced), Aggressive (shorter distance, firmer braking). "
|
||||
"Fine-tune your comfort level in traffic situations."),
|
||||
"../assets/offroad/icon_shell.png");
|
||||
list->addItem(vibeFollowPersonalityControl);
|
||||
|
||||
main_layout->addWidget(cruisePanelScreen);
|
||||
main_layout->setCurrentWidget(cruisePanelScreen);
|
||||
refresh(offroad);
|
||||
@@ -70,10 +99,26 @@ void LongitudinalPanel::refresh(bool _offroad) {
|
||||
customAccIncrement->showDescription();
|
||||
}
|
||||
}
|
||||
bool vibePersonalityEnabled = params.getBool("VibePersonalityEnabled");
|
||||
if (vibePersonalityEnabled) {
|
||||
vibeAccelPersonalityControl->setVisible(true);
|
||||
vibeFollowPersonalityControl->setVisible(true);
|
||||
} else {
|
||||
vibeAccelPersonalityControl->setVisible(false);
|
||||
vibeFollowPersonalityControl->setVisible(false);
|
||||
}
|
||||
|
||||
// enable toggle when long is available and is not PCM cruise
|
||||
customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad);
|
||||
customAccIncrement->refresh();
|
||||
|
||||
// Vibe Personality controls - always enabled for toggling
|
||||
vibePersonalityControl->setEnabled(true);
|
||||
vibeAccelPersonalityControl->setEnabled(true);
|
||||
vibeFollowPersonalityControl->setEnabled(true);
|
||||
vibePersonalityControl->refresh();
|
||||
vibeAccelPersonalityControl->refresh();
|
||||
vibeFollowPersonalityControl->refresh();
|
||||
|
||||
offroad = _offroad;
|
||||
}
|
||||
|
||||
@@ -29,4 +29,8 @@ private:
|
||||
ScrollViewSP *cruisePanelScroller = nullptr;
|
||||
QWidget *cruisePanelScreen = nullptr;
|
||||
CustomAccIncrement *customAccIncrement = nullptr;
|
||||
|
||||
ParamControlSP *vibePersonalityControl;
|
||||
ParamControlSP *vibeAccelPersonalityControl;
|
||||
ParamControlSP *vibeFollowPersonalityControl;
|
||||
};
|
||||
|
||||
@@ -68,6 +68,14 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
|
||||
connect(uiStateSP(), &UIStateSP::uiUpdate, this, &ModelsPanel::updateLabels);
|
||||
list->addItem(currentModelLblBtn);
|
||||
|
||||
refreshAvailableModelsBtn = new ButtonControlSP(tr("Refresh Model List"), tr("REFRESH"), "", this);
|
||||
connect(refreshAvailableModelsBtn, &ButtonControlSP::clicked, this, [=]() {
|
||||
params.put("ModelManager_LastSyncTime", "0");
|
||||
ConfirmationDialog::alert(tr("Fetching Latest Models"), this);
|
||||
});
|
||||
|
||||
list->addItem(refreshAvailableModelsBtn);
|
||||
|
||||
clearModelCacheBtn = new ButtonControlSP(tr("Clear Model Cache"), tr("CLEAR"), "", this);
|
||||
connect(clearModelCacheBtn, &ButtonControlSP::clicked, this, &ModelsPanel::clearModelCache);
|
||||
|
||||
|
||||
@@ -79,5 +79,6 @@ private:
|
||||
QFrame *policyFrame;
|
||||
Params params;
|
||||
ButtonControlSP *clearModelCacheBtn;
|
||||
ButtonControlSP *refreshAvailableModelsBtn;
|
||||
|
||||
};
|
||||
|
||||
@@ -48,5 +48,162 @@ void ModelRendererSP::drawPath(QPainter &painter, const cereal::ModelDataV2::Rea
|
||||
painter.drawPolygon(right_blindspot_vertices);
|
||||
}
|
||||
}
|
||||
|
||||
ModelRenderer::drawPath(painter, model, surface_rect.height());
|
||||
|
||||
drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
|
||||
}
|
||||
|
||||
void ModelRendererSP::drawLeadStatus(QPainter &painter, int height, int width) {
|
||||
auto *s = uiState();
|
||||
auto &sm = *(s->sm);
|
||||
|
||||
if (!sm.alive("radarState")) return;
|
||||
|
||||
const auto &radar_state = sm["radarState"].getRadarState();
|
||||
const auto &lead_one = radar_state.getLeadOne();
|
||||
const auto &lead_two = radar_state.getLeadTwo();
|
||||
|
||||
// Check if we have any active leads
|
||||
bool has_lead_one = lead_one.getStatus();
|
||||
bool has_lead_two = lead_two.getStatus();
|
||||
|
||||
if (!has_lead_one && !has_lead_two) {
|
||||
// Fade out status display
|
||||
lead_status_alpha = std::max(0.0f, lead_status_alpha - 0.05f);
|
||||
if (lead_status_alpha <= 0.0f) return;
|
||||
} else {
|
||||
// Fade in status display
|
||||
lead_status_alpha = std::min(1.0f, lead_status_alpha + 0.1f);
|
||||
}
|
||||
|
||||
if (has_lead_one) {
|
||||
drawLeadStatusAtPosition(painter, lead_one, lead_vertices[0], height, width, "L1");
|
||||
}
|
||||
|
||||
if (has_lead_two && std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0) {
|
||||
drawLeadStatusAtPosition(painter, lead_two, lead_vertices[1], height, width, "L2");
|
||||
}
|
||||
}
|
||||
|
||||
void ModelRendererSP::drawLeadStatusAtPosition(QPainter &painter,
|
||||
const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &chevron_pos,
|
||||
int height, int width,
|
||||
const QString &label) {
|
||||
|
||||
float d_rel = lead_data.getDRel();
|
||||
float v_rel = lead_data.getVRel();
|
||||
auto *s = uiState();
|
||||
auto &sm = *(s->sm);
|
||||
float v_ego = sm["carState"].getCarState().getVEgo();
|
||||
|
||||
int chevron_data = std::atoi(Params().get("ChevronInfo").c_str());
|
||||
|
||||
// Calculate chevron size (same logic as drawLead)
|
||||
float sz = std::clamp((25 * 30) / (d_rel / 3 + 30), 15.0f, 30.0f) * 2.35;
|
||||
|
||||
QFont content_font = painter.font();
|
||||
content_font.setPixelSize(42);
|
||||
content_font.setBold(true);
|
||||
painter.setFont(content_font);
|
||||
|
||||
QFontMetrics fm(content_font);
|
||||
bool is_metric = s->scene.is_metric;
|
||||
|
||||
QStringList text_lines;
|
||||
|
||||
const int chevron_types = 3;
|
||||
const int chevron_all = chevron_types + 1; // All metrics (value 4)
|
||||
QStringList chevron_text[chevron_types];
|
||||
int position;
|
||||
float val;
|
||||
|
||||
// Distance display (chevron_data == 1 or all)
|
||||
if (chevron_data == 1 || chevron_data == chevron_all) {
|
||||
position = 0;
|
||||
val = std::max(0.0f, d_rel);
|
||||
QString distance_unit = is_metric ? "m" : "ft";
|
||||
if (!is_metric) {
|
||||
val *= 3.28084f; // Convert meters to feet
|
||||
}
|
||||
chevron_text[position].append(QString::number(val, 'f', 0) + " " + distance_unit);
|
||||
}
|
||||
|
||||
// Absolute velocity display (chevron_data == 2 or all)
|
||||
if (chevron_data == 2 || chevron_data == chevron_all) {
|
||||
position = (chevron_data == 2) ? 0 : 1;
|
||||
val = std::max(0.0f, (v_rel + v_ego) * (is_metric ? static_cast<float>(MS_TO_KPH) : static_cast<float>(MS_TO_MPH)));
|
||||
chevron_text[position].append(QString::number(val, 'f', 0) + " " + (is_metric ? "km/h" : "mph"));
|
||||
}
|
||||
|
||||
// Time-to-contact display (chevron_data == 3 or all)
|
||||
if (chevron_data == 3 || chevron_data == chevron_all) {
|
||||
position = (chevron_data == 3) ? 0 : 2;
|
||||
val = (d_rel > 0 && v_ego > 0) ? std::max(0.0f, d_rel / v_ego) : 0.0f;
|
||||
QString ttc_str = (val > 0 && val < 200) ? QString::number(val, 'f', 1) + "s" : "---";
|
||||
chevron_text[position].append(ttc_str);
|
||||
}
|
||||
|
||||
// Collect all non-empty text lines
|
||||
for (int i = 0; i < chevron_types; ++i) {
|
||||
if (!chevron_text[i].isEmpty()) {
|
||||
text_lines.append(chevron_text[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// If no text to display, return early
|
||||
if (text_lines.isEmpty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Text box dimensions
|
||||
float str_w = 150; // Width of text area
|
||||
float str_h = 45; // Height per line
|
||||
|
||||
// Position text below chevron, centered horizontally
|
||||
float text_x = chevron_pos.x() - str_w / 2;
|
||||
float text_y = chevron_pos.y() + sz + 15;
|
||||
|
||||
// Clamp to screen bounds
|
||||
text_x = std::clamp(text_x, 10.0f, (float)width - str_w - 10);
|
||||
|
||||
// Shadow offset
|
||||
QPoint shadow_offset(2, 2);
|
||||
|
||||
// Draw each line of text with shadow
|
||||
for (int i = 0; i < text_lines.size(); ++i) {
|
||||
if (!text_lines[i].isEmpty()) {
|
||||
QRect textRect(text_x, text_y + (i * str_h), str_w, str_h);
|
||||
|
||||
// Draw shadow
|
||||
painter.setPen(QColor(0x0, 0x0, 0x0, (int)(200 * lead_status_alpha)));
|
||||
painter.drawText(textRect.translated(shadow_offset.x(), shadow_offset.y()),
|
||||
Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
|
||||
|
||||
// Determine text color based on content and danger level
|
||||
QColor text_color;
|
||||
|
||||
// Check if this is a distance line (contains 'm' or 'ft')
|
||||
if (text_lines[i].contains("m") || text_lines[i].contains("ft")) {
|
||||
if (d_rel < 20.0f) {
|
||||
text_color = QColor(255, 80, 80, (int)(255 * lead_status_alpha)); // Red - danger
|
||||
} else if (d_rel < 40.0f) {
|
||||
text_color = QColor(255, 200, 80, (int)(255 * lead_status_alpha)); // Yellow - caution
|
||||
} else {
|
||||
text_color = QColor(80, 255, 120, (int)(255 * lead_status_alpha)); // Green - safe
|
||||
}
|
||||
}
|
||||
else {
|
||||
text_color = QColor(0xff, 0xff, 0xff, (int)(255 * lead_status_alpha)); // White for other lines
|
||||
}
|
||||
|
||||
// Draw main text
|
||||
painter.setPen(text_color);
|
||||
painter.drawText(textRect, Qt::AlignBottom | Qt::AlignHCenter, text_lines[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Reset pen
|
||||
painter.setPen(Qt::NoPen);
|
||||
}
|
||||
|
||||
@@ -17,6 +17,17 @@ private:
|
||||
void update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) override;
|
||||
void drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, const QRect &rect) override;
|
||||
|
||||
// Lead status display methods
|
||||
void drawLeadStatus(QPainter &painter, int height, int width);
|
||||
void drawLeadStatusAtPosition(QPainter &painter,
|
||||
const cereal::RadarState::LeadData::Reader &lead_data,
|
||||
const QPointF &chevron_pos,
|
||||
int height, int width,
|
||||
const QString &label);
|
||||
|
||||
QPolygonF left_blindspot_vertices;
|
||||
QPolygonF right_blindspot_vertices;
|
||||
};
|
||||
|
||||
// Lead status animation
|
||||
float lead_status_alpha = 0.0f;
|
||||
};
|
||||
@@ -1,167 +0,0 @@
|
||||
/**
|
||||
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
*
|
||||
* This file is part of sunnypilot and is licensed under the MIT License.
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/external_storage.h"
|
||||
|
||||
#include <QProcess>
|
||||
#include <QCoreApplication>
|
||||
#include <QShowEvent>
|
||||
#include <QTimer>
|
||||
#include <QtConcurrent>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "selfdrive/ui/qt/api.h"
|
||||
#include "selfdrive/ui/qt/widgets/input.h"
|
||||
#include "selfdrive/ui/sunnypilot/ui.h"
|
||||
|
||||
ExternalStorageControl::ExternalStorageControl() :
|
||||
ButtonControl(tr("External Storage"), "", tr("Extend your comma device's storage by inserting a USB drive into the aux port.")) {
|
||||
|
||||
QObject::connect(this, &ButtonControl::clicked, [=]() {
|
||||
if (text() == tr("CHECK") || text() == tr("MOUNT")) {
|
||||
mountStorage();
|
||||
} else if (text() == tr("UNMOUNT")) {
|
||||
unmountStorage();
|
||||
} else if (text() == tr("FORMAT")) {
|
||||
if (ConfirmationDialog::confirm(tr("Are you sure you want to format this drive? This will erase all data."), tr("Format"), this)) {
|
||||
formatStorage();
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
QObject::connect(uiState(), &UIState::offroadTransition, this, &ExternalStorageControl::updateState);
|
||||
updateState(!uiState()->scene.started);
|
||||
|
||||
refresh();
|
||||
}
|
||||
|
||||
void ExternalStorageControl::updateState(bool offroad) {
|
||||
setEnabled(offroad);
|
||||
}
|
||||
|
||||
void ExternalStorageControl::debouncedRefresh() {
|
||||
if (refreshPending) return;
|
||||
refreshPending = true;
|
||||
|
||||
QTimer::singleShot(250, this, [=]() {
|
||||
refreshPending = false;
|
||||
refresh();
|
||||
});
|
||||
}
|
||||
|
||||
void ExternalStorageControl::refresh() {
|
||||
QtConcurrent::run([=]() {
|
||||
auto run = [](const QString &cmd) {
|
||||
QProcess p;
|
||||
p.start("sh", QStringList() << "-c" << cmd);
|
||||
p.waitForFinished();
|
||||
return p.exitCode() == 0;
|
||||
};
|
||||
|
||||
bool isMounted = run("findmnt -n /mnt/external_realdata");
|
||||
bool hasDrive = run("lsblk -f /dev/sdg1");
|
||||
bool hasFs = run("lsblk -f /dev/sdg1 | grep -q ext4");
|
||||
bool hasLabel = run("sudo blkid /dev/sdg1 | grep -q 'LABEL=\"openpilot\"'");
|
||||
|
||||
QString info;
|
||||
if (isMounted && hasLabel) {
|
||||
QProcess df;
|
||||
df.start("sh", QStringList() << "-c" << "df -h /mnt/external_realdata | awk 'NR==2 {print $3 \"/\" $2}'");
|
||||
df.waitForFinished();
|
||||
info = df.readAllStandardOutput().trimmed();
|
||||
}
|
||||
|
||||
QMetaObject::invokeMethod(this, [=]() {
|
||||
if (formatting) {
|
||||
setValue(tr("formatting"));
|
||||
setText(tr("FORMAT"));
|
||||
setEnabled(false);
|
||||
} else {
|
||||
if (!hasDrive) {
|
||||
setValue(tr("insert drive"));
|
||||
setText(tr("CHECK"));
|
||||
} else if (!hasFs || !hasLabel) {
|
||||
setValue(tr("needs format"));
|
||||
setText(tr("FORMAT"));
|
||||
} else if (isMounted) {
|
||||
setValue(info);
|
||||
setText(tr("UNMOUNT"));
|
||||
} else {
|
||||
setValue("drive detected");
|
||||
setText(tr("MOUNT"));
|
||||
}
|
||||
updateState(!uiState()->scene.started);
|
||||
}
|
||||
}, Qt::QueuedConnection);
|
||||
});
|
||||
}
|
||||
|
||||
void ExternalStorageControl::mountStorage() {
|
||||
setValue(tr("mounting"));
|
||||
setEnabled(false);
|
||||
|
||||
QtConcurrent::run([=]() {
|
||||
QProcess process;
|
||||
process.start("sh", QStringList() << "-c" <<
|
||||
"sudo mount -o remount,rw / && "
|
||||
"sudo mkdir -p /mnt/external_realdata && "
|
||||
"grep -q '/dev/sdg1 /mnt/external_realdata' /etc/fstab || "
|
||||
"echo '/dev/sdg1 /mnt/external_realdata ext4 defaults,nofail 0 2' | sudo tee -a /etc/fstab && "
|
||||
"sudo systemctl daemon-reexec && "
|
||||
"sudo mount /mnt/external_realdata && "
|
||||
"sudo chown -R comma:comma /mnt/external_realdata && "
|
||||
"sudo chmod -R 775 /mnt/external_realdata && "
|
||||
"sudo mount -o remount,ro /");
|
||||
process.waitForFinished();
|
||||
|
||||
QMetaObject::invokeMethod(this, [=]() {
|
||||
debouncedRefresh();
|
||||
}, Qt::QueuedConnection);
|
||||
});
|
||||
}
|
||||
|
||||
void ExternalStorageControl::unmountStorage() {
|
||||
setValue(tr("unmounting"));
|
||||
setEnabled(false);
|
||||
|
||||
QtConcurrent::run([=]() {
|
||||
QProcess process;
|
||||
process.start("sh", QStringList() << "-c" << "sudo umount /mnt/external_realdata");
|
||||
process.waitForFinished();
|
||||
|
||||
QMetaObject::invokeMethod(this, [=]() {
|
||||
debouncedRefresh();
|
||||
}, Qt::QueuedConnection);
|
||||
});
|
||||
}
|
||||
|
||||
void ExternalStorageControl::formatStorage() {
|
||||
unmountStorage();
|
||||
formatting = true;
|
||||
setValue(tr("formatting"));
|
||||
setEnabled(false);
|
||||
|
||||
QProcess *process = new QProcess(this);
|
||||
connect(process, static_cast<void(QProcess::*)(int, QProcess::ExitStatus)>(&QProcess::finished),
|
||||
this, [=](int exitCode, QProcess::ExitStatus status) {
|
||||
process->deleteLater();
|
||||
formatting = false;
|
||||
if (exitCode == 0 && status == QProcess::NormalExit) {
|
||||
QProcess::execute("sh", QStringList() << "-c" << "sudo e2label /dev/sdg1 openpilot");
|
||||
mountStorage();
|
||||
} else {
|
||||
setValue(tr("needs format"));
|
||||
updateState(!uiState()->scene.started);
|
||||
}
|
||||
});
|
||||
process->start("sh", QStringList() << "-c" << "sudo mkfs.ext4 -F /dev/sdg1");
|
||||
}
|
||||
|
||||
void ExternalStorageControl::showEvent(QShowEvent *event) {
|
||||
ButtonControl::showEvent(event);
|
||||
QTimer::singleShot(100, this, &ExternalStorageControl::debouncedRefresh);
|
||||
}
|
||||
@@ -1,34 +0,0 @@
|
||||
/**
|
||||
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
*
|
||||
* This file is part of sunnypilot and is licensed under the MIT License.
|
||||
* See the LICENSE.md file in the root directory for more details.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "system/hardware/hw.h"
|
||||
#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
|
||||
#define ButtonControl ButtonControlSP
|
||||
|
||||
class ExternalStorageControl : public ButtonControl {
|
||||
Q_OBJECT
|
||||
|
||||
public:
|
||||
ExternalStorageControl();
|
||||
|
||||
protected:
|
||||
void showEvent(QShowEvent *event) override;
|
||||
|
||||
private:
|
||||
Params params;
|
||||
|
||||
bool refreshPending = false;
|
||||
bool formatting = false;
|
||||
void updateState(bool offroad);
|
||||
void refresh();
|
||||
void debouncedRefresh();
|
||||
void mountStorage();
|
||||
void unmountStorage();
|
||||
void formatStorage();
|
||||
};
|
||||
@@ -60,6 +60,7 @@ typedef struct UIScene {
|
||||
cereal::PandaState::PandaType pandaType;
|
||||
|
||||
cereal::LongitudinalPersonality personality;
|
||||
cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality;
|
||||
|
||||
float light_sensor = -1;
|
||||
bool started, ignition, is_metric, recording_audio;
|
||||
|
||||
@@ -10,12 +10,14 @@ from opendbc.car import structs
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, mpc):
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.vibe_controller = VibePersonalityController()
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
|
||||
@property
|
||||
@@ -31,6 +33,7 @@ class LongitudinalPlannerSP:
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.dec.update(sm)
|
||||
self.vibe_controller.update()
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
|
||||
@@ -0,0 +1,286 @@
|
||||
"""
|
||||
Test suite for VibePersonalityController
|
||||
|
||||
Tests the core functionality of the vibe personality system including
|
||||
acceleration profiles, following distance, and personality management.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
import numpy as np
|
||||
from pytest_mock import MockerFixture
|
||||
from cereal import log, custom
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
|
||||
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
LongPersonality = log.LongitudinalPersonality
|
||||
|
||||
|
||||
class TestVibePersonalityController:
|
||||
"""Test suite for VibePersonalityController"""
|
||||
|
||||
@pytest.fixture
|
||||
def mock_params(self, mocker: MockerFixture):
|
||||
"""Mock Params with realistic behavior"""
|
||||
mock = mocker.Mock()
|
||||
mock.get.return_value = None
|
||||
mock.get_bool.return_value = True
|
||||
mock.put.return_value = None
|
||||
mock.put_bool.return_value = None
|
||||
return mock
|
||||
|
||||
@pytest.fixture
|
||||
def controller(self, mock_params, mocker: MockerFixture):
|
||||
"""Create controller with mocked dependencies"""
|
||||
mocker.patch('openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality.Params', return_value=mock_params)
|
||||
controller = VibePersonalityController()
|
||||
controller.params = mock_params
|
||||
return controller
|
||||
|
||||
def test_initialization_sets_correct_defaults(self, controller):
|
||||
"""Controller should initialize with expected default values"""
|
||||
assert controller.accel_personality == AccelPersonality.normal
|
||||
assert controller.long_personality == LongPersonality.standard
|
||||
assert controller.frame == 0
|
||||
assert hasattr(controller, 'max_accel_slopes')
|
||||
assert hasattr(controller, 'min_accel_slopes')
|
||||
assert hasattr(controller, 'follow_distance_slopes')
|
||||
|
||||
def test_accel_personality_management(self, controller):
|
||||
"""Test acceleration personality setting and cycling"""
|
||||
# Valid personality setting
|
||||
result = controller.set_accel_personality(AccelPersonality.sport)
|
||||
assert result is True
|
||||
assert controller.accel_personality == AccelPersonality.sport
|
||||
controller.params.put.assert_called_with('AccelPersonality', str(AccelPersonality.sport))
|
||||
|
||||
# Invalid personality setting
|
||||
result = controller.set_accel_personality(999)
|
||||
assert result is False
|
||||
assert controller.accel_personality == AccelPersonality.sport # unchanged
|
||||
|
||||
# Cycling behavior
|
||||
controller.set_accel_personality(AccelPersonality.eco)
|
||||
assert controller.cycle_accel_personality() == AccelPersonality.normal
|
||||
assert controller.cycle_accel_personality() == AccelPersonality.sport
|
||||
assert controller.cycle_accel_personality() == AccelPersonality.eco
|
||||
|
||||
def test_long_personality_management(self, controller):
|
||||
"""Test longitudinal personality setting and cycling"""
|
||||
# Valid setting
|
||||
result = controller.set_long_personality(LongPersonality.aggressive)
|
||||
assert result is True
|
||||
assert controller.long_personality == LongPersonality.aggressive
|
||||
|
||||
# Invalid setting
|
||||
result = controller.set_long_personality(-1)
|
||||
assert result is False
|
||||
|
||||
# Cycling
|
||||
controller.set_long_personality(LongPersonality.relaxed)
|
||||
assert controller.cycle_long_personality() == LongPersonality.standard
|
||||
assert controller.cycle_long_personality() == LongPersonality.aggressive
|
||||
assert controller.cycle_long_personality() == LongPersonality.relaxed
|
||||
|
||||
def test_enable_disable_logic(self, controller):
|
||||
"""Test feature enable/disable states"""
|
||||
# Mock enabled state
|
||||
controller.params.get_bool.return_value = True
|
||||
assert controller.is_enabled() is True
|
||||
assert controller.is_accel_enabled() is True
|
||||
assert controller.is_follow_enabled() is True
|
||||
|
||||
# Mock disabled state
|
||||
controller.params.get_bool.return_value = False
|
||||
assert controller.is_enabled() is False
|
||||
assert controller.is_accel_enabled() is False
|
||||
assert controller.is_follow_enabled() is False
|
||||
|
||||
# Test partial disable (only main toggle off)
|
||||
def mock_get_bool(key):
|
||||
return key != 'VibePersonalityEnabled'
|
||||
controller.params.get_bool.side_effect = mock_get_bool
|
||||
assert controller.is_accel_enabled() is False
|
||||
assert controller.is_follow_enabled() is False
|
||||
|
||||
def test_get_accel_limits_returns_valid_range(self, controller):
|
||||
"""Acceleration limits should return valid min/max values"""
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
# Test at multiple speeds
|
||||
for speed in [0.0, 15.0, 30.0]:
|
||||
limits = controller.get_accel_limits(speed)
|
||||
assert limits is not None, f"Failed at speed {speed}"
|
||||
|
||||
min_a, max_a = limits
|
||||
assert isinstance(min_a, float)
|
||||
assert isinstance(max_a, float)
|
||||
assert min_a < 0, "Min acceleration should be negative (braking)"
|
||||
assert max_a > 0, "Max acceleration should be positive"
|
||||
assert min_a < max_a, "Min should be less than max"
|
||||
|
||||
def test_get_accel_limits_returns_none_when_disabled(self, controller):
|
||||
"""Should return None when acceleration control is disabled"""
|
||||
controller.params.get_bool.return_value = False
|
||||
assert controller.get_accel_limits(20.0) is None
|
||||
|
||||
def test_get_follow_distance_multiplier_returns_positive_value(self, controller):
|
||||
"""Follow distance multiplier should be positive"""
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
multiplier = controller.get_follow_distance_multiplier(20.0)
|
||||
assert multiplier is not None
|
||||
assert isinstance(multiplier, float)
|
||||
assert multiplier > 0
|
||||
|
||||
def test_get_follow_distance_multiplier_returns_none_when_disabled(self, controller):
|
||||
"""Should return None when follow distance control is disabled"""
|
||||
controller.params.get_bool.return_value = False
|
||||
assert controller.get_follow_distance_multiplier(20.0) is None
|
||||
|
||||
def test_personality_differences_produce_different_results(self, controller):
|
||||
"""Different personalities should produce measurably different outputs"""
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
# Test acceleration personality differences
|
||||
controller.set_accel_personality(AccelPersonality.eco)
|
||||
eco_limits = controller.get_accel_limits(15.0)
|
||||
|
||||
controller.set_accel_personality(AccelPersonality.sport)
|
||||
sport_limits = controller.get_accel_limits(15.0)
|
||||
|
||||
assert sport_limits[1] > eco_limits[1], "Sport should allow higher acceleration than eco"
|
||||
|
||||
# Test following distance personality differences
|
||||
controller.set_long_personality(LongPersonality.relaxed)
|
||||
relaxed_dist = controller.get_follow_distance_multiplier(20.0)
|
||||
|
||||
controller.set_long_personality(LongPersonality.aggressive)
|
||||
aggressive_dist = controller.get_follow_distance_multiplier(20.0)
|
||||
|
||||
assert relaxed_dist > aggressive_dist, "Relaxed should have longer following distance"
|
||||
|
||||
def test_interpolation_functions_work_correctly(self, controller):
|
||||
"""Test mathematical interpolation functions"""
|
||||
# Test slope computation
|
||||
x = np.array([0., 10., 20.])
|
||||
y = np.array([1.0, 2.0, 1.5])
|
||||
slopes = controller._compute_slopes(x, y)
|
||||
|
||||
assert len(slopes) == len(x)
|
||||
assert all(np.isfinite(slope) for slope in slopes)
|
||||
|
||||
# Test interpolation accuracy at known points
|
||||
result_0 = controller._interpolate(0.0, x, y, slopes)
|
||||
result_10 = controller._interpolate(10.0, x, y, slopes)
|
||||
|
||||
assert abs(result_0 - 1.0) < 1e-6, "Interpolation should be exact at breakpoints"
|
||||
assert abs(result_10 - 2.0) < 1e-6, "Interpolation should be exact at breakpoints"
|
||||
|
||||
# Test interpolation bounds
|
||||
result_below = controller._interpolate(-5.0, x, y, slopes)
|
||||
result_above = controller._interpolate(25.0, x, y, slopes)
|
||||
assert abs(result_below - y[0]) < 1e-6, "Should clamp to first value"
|
||||
assert abs(result_above - y[-1]) < 1e-6, "Should clamp to last value"
|
||||
|
||||
def test_get_personality_info_returns_complete_info(self, controller):
|
||||
"""Personality info should contain all required fields"""
|
||||
info = controller.get_personality_info()
|
||||
|
||||
required_fields = [
|
||||
'accel_personality', 'accel_personality_int',
|
||||
'long_personality', 'long_personality_int',
|
||||
'enabled', 'accel_enabled', 'follow_enabled'
|
||||
]
|
||||
|
||||
for field in required_fields:
|
||||
assert field in info, f"Missing required field: {field}"
|
||||
|
||||
assert info['accel_personality'] in ['Eco', 'Normal', 'Sport']
|
||||
assert info['long_personality'] in ['Relaxed', 'Standard', 'Aggressive']
|
||||
assert isinstance(info['enabled'], bool)
|
||||
|
||||
def test_toggle_functions_change_state(self, controller):
|
||||
"""Toggle functions should change parameter states"""
|
||||
# Mock current enabled state
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
# Test main toggle
|
||||
result = controller.toggle_personality()
|
||||
assert result is False # should return new state
|
||||
controller.params.put_bool.assert_called_with('VibePersonalityEnabled', False)
|
||||
|
||||
# Test specific toggles
|
||||
controller.params.get_bool.return_value = True
|
||||
controller.toggle_accel_personality()
|
||||
controller.params.put_bool.assert_called_with('VibeAccelPersonalityEnabled', False)
|
||||
|
||||
controller.toggle_follow_distance_personality()
|
||||
controller.params.put_bool.assert_called_with('VibeFollowPersonalityEnabled', False)
|
||||
|
||||
def test_reset_restores_defaults(self, controller):
|
||||
"""Reset should restore controller to default state"""
|
||||
# Change from defaults
|
||||
controller.set_accel_personality(AccelPersonality.sport)
|
||||
controller.set_long_personality(LongPersonality.aggressive)
|
||||
controller.frame = 1000
|
||||
|
||||
# Reset and verify defaults
|
||||
controller.reset()
|
||||
assert controller.accel_personality == AccelPersonality.normal
|
||||
assert controller.long_personality == LongPersonality.standard
|
||||
assert controller.frame == 0
|
||||
|
||||
def test_update_increments_frame_counter(self, controller):
|
||||
"""Update should increment frame counter with wraparound"""
|
||||
initial_frame = controller.frame
|
||||
controller.update()
|
||||
assert controller.frame == initial_frame + 1
|
||||
|
||||
# Test wraparound
|
||||
controller.frame = 999999
|
||||
controller.update()
|
||||
assert controller.frame == 0
|
||||
|
||||
def test_individual_accel_methods(self, controller):
|
||||
"""Test individual min/max accel convenience methods"""
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
min_accel = controller.get_min_accel(15.0)
|
||||
max_accel = controller.get_max_accel(15.0)
|
||||
|
||||
assert min_accel is not None
|
||||
assert max_accel is not None
|
||||
assert min_accel < 0
|
||||
assert max_accel > 0
|
||||
|
||||
# Test disabled state
|
||||
controller.params.get_bool.return_value = False
|
||||
assert controller.get_min_accel(15.0) is None
|
||||
assert controller.get_max_accel(15.0) is None
|
||||
|
||||
@pytest.mark.parametrize("speed", [0.0, 5.0, 15.0, 25.0, 40.0, 55.0])
|
||||
def test_accel_limits_at_various_speeds(self, controller, speed):
|
||||
"""Test acceleration limits across speed range"""
|
||||
controller.params.get_bool.return_value = True
|
||||
|
||||
limits = controller.get_accel_limits(speed)
|
||||
assert limits is not None
|
||||
|
||||
min_a, max_a = limits
|
||||
assert min_a < max_a
|
||||
assert -2.0 < min_a < 0 # Reasonable braking range
|
||||
assert 0 < max_a < 3.0 # Reasonable acceleration range
|
||||
|
||||
def test_error_handling_in_interpolation(self, controller):
|
||||
"""Test interpolation handles edge cases gracefully"""
|
||||
# Test with minimal points
|
||||
x = [0., 1.]
|
||||
y = [1.0, 2.0]
|
||||
slopes = controller._compute_slopes(x, y)
|
||||
result = controller._interpolate(0.5, x, y, slopes)
|
||||
assert 1.0 <= result <= 2.0
|
||||
|
||||
# Test with insufficient points
|
||||
with pytest.raises(ValueError):
|
||||
controller._compute_slopes([0.], [1.0])
|
||||
@@ -0,0 +1,306 @@
|
||||
"""
|
||||
Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from cereal import log, custom
|
||||
import numpy as np
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.params import Params
|
||||
|
||||
LongPersonality = log.LongitudinalPersonality
|
||||
AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
|
||||
|
||||
# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport)
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelPersonality.eco: [2.00, 2.00, 1.32, 0.85, .58, .46, .365, .317, .089], # eco
|
||||
AccelPersonality.normal: [2.00, 2.00, 1.42, 1.10, .65, .56, .43, .36, .12], # normal
|
||||
AccelPersonality.sport: [2.00, 2.00, 1.52, 1.40, .80, .70, .53, .46, .20], # sport
|
||||
}
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 6., 9., 11., 16., 20., 25., 30., 55.]
|
||||
|
||||
# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive)
|
||||
MIN_ACCEL_PROFILES = {
|
||||
LongPersonality.relaxed: [-1.20, -1.20], # gentler braking
|
||||
LongPersonality.standard: [-1.30, -1.30], # normal braking
|
||||
LongPersonality.aggressive: [-1.40, -1.40], # more aggressive braking
|
||||
}
|
||||
MIN_ACCEL_BREAKPOINTS = [0., 50.]
|
||||
|
||||
# Following Distance Profiles mapped to LongPersonality (relaxed/standard/aggressive)
|
||||
FOLLOW_DISTANCE_PROFILES = {
|
||||
LongPersonality.relaxed: {
|
||||
'x_vel': [0., 19.7, 22.2, 40.],
|
||||
'y_dist': [1.40, 1.40, 1.65, 1.65] # longer following distance
|
||||
},
|
||||
LongPersonality.standard: {
|
||||
'x_vel': [0., 19.7, 22.2, 40.],
|
||||
'y_dist': [1.35, 1.35, 1.40, 1.40] # normal following distance
|
||||
},
|
||||
LongPersonality.aggressive: {
|
||||
'x_vel': [0., 19.7, 22.2, 40.],
|
||||
'y_dist': [1.20, 1.20, 1.30, 1.30] # shorter following distance
|
||||
}
|
||||
}
|
||||
|
||||
class VibePersonalityController:
|
||||
"""
|
||||
Controller for managing separated acceleration and distance controls:
|
||||
- AccelPersonality controls acceleration behavior (eco, normal, sport)
|
||||
- LongPersonality controls braking and following distance (relaxed, standard, aggressive)
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = 0
|
||||
|
||||
# Separate personalities for acceleration and distance control
|
||||
self.accel_personality = AccelPersonality.normal
|
||||
self.long_personality = LongPersonality.standard
|
||||
|
||||
# Parameter keys
|
||||
self.param_keys = {
|
||||
'accel_personality': 'AccelPersonality', # eco=0, normal=1, sport=2
|
||||
'long_personality': 'LongitudinalPersonality', # relaxed=0, standard=1, aggressive=2
|
||||
'enabled': 'VibePersonalityEnabled',
|
||||
'accel_enabled': 'VibeAccelPersonalityEnabled',
|
||||
'follow_enabled': 'VibeFollowPersonalityEnabled'
|
||||
}
|
||||
|
||||
# Precompute slopes for all personalities
|
||||
self._precompute_slopes()
|
||||
|
||||
def _precompute_slopes(self):
|
||||
"""Precompute all interpolation slopes for efficiency"""
|
||||
self.max_accel_slopes = {}
|
||||
self.min_accel_slopes = {}
|
||||
self.follow_distance_slopes = {}
|
||||
|
||||
# Precompute for AccelPersonality (acceleration)
|
||||
for personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
|
||||
if personality in MAX_ACCEL_PROFILES:
|
||||
self.max_accel_slopes[personality] = self._compute_slopes(MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[personality])
|
||||
|
||||
# Precompute for LongPersonality (braking and following)
|
||||
for personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
|
||||
if personality in MIN_ACCEL_PROFILES:
|
||||
self.min_accel_slopes[personality] = self._compute_slopes(MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[personality])
|
||||
|
||||
if personality in FOLLOW_DISTANCE_PROFILES:
|
||||
profile = FOLLOW_DISTANCE_PROFILES[personality]
|
||||
self.follow_distance_slopes[personality] = self._compute_slopes(profile['x_vel'], profile['y_dist'])
|
||||
|
||||
def _update_from_params(self):
|
||||
"""Update personalities from params (rate limited)"""
|
||||
if self.frame % int(1. / DT_MDL) != 0:
|
||||
return
|
||||
|
||||
# Update AccelPersonality
|
||||
try:
|
||||
accel_personality_str = self.params.get(self.param_keys['accel_personality'], encoding='utf-8')
|
||||
if accel_personality_str:
|
||||
accel_personality_int = int(accel_personality_str)
|
||||
if accel_personality_int in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
|
||||
self.accel_personality = accel_personality_int
|
||||
except (ValueError, TypeError):
|
||||
pass
|
||||
|
||||
# Update LongPersonality
|
||||
try:
|
||||
long_personality_str = self.params.get(self.param_keys['long_personality'], encoding='utf-8')
|
||||
if long_personality_str:
|
||||
long_personality_int = int(long_personality_str)
|
||||
if long_personality_int in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
|
||||
self.long_personality = long_personality_int
|
||||
except (ValueError, TypeError):
|
||||
pass
|
||||
|
||||
def _get_toggle_state(self, key: str, default: bool = True) -> bool:
|
||||
"""Get toggle state with default fallback"""
|
||||
return self.params.get_bool(self.param_keys.get(key, key)) if key in self.param_keys else default
|
||||
|
||||
def _set_toggle_state(self, key: str, value: bool):
|
||||
"""Set toggle state in params"""
|
||||
if key in self.param_keys:
|
||||
self.params.put_bool(self.param_keys[key], value)
|
||||
|
||||
# AccelPersonality Management (for acceleration)
|
||||
def set_accel_personality(self, personality: int) -> bool:
|
||||
"""Set AccelPersonality (eco=0, normal=1, sport=2)"""
|
||||
if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
|
||||
self.accel_personality = personality
|
||||
self.params.put(self.param_keys['accel_personality'], str(personality))
|
||||
return True
|
||||
return False
|
||||
|
||||
def cycle_accel_personality(self) -> int:
|
||||
"""Cycle through AccelPersonality: eco -> normal -> sport -> eco"""
|
||||
personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
|
||||
current_idx = personalities.index(self.accel_personality)
|
||||
next_personality = personalities[(current_idx + 1) % len(personalities)]
|
||||
self.set_accel_personality(next_personality)
|
||||
return int(next_personality)
|
||||
|
||||
def get_accel_personality(self) -> int:
|
||||
"""Get current AccelPersonality"""
|
||||
self._update_from_params()
|
||||
return int(self.accel_personality)
|
||||
|
||||
# LongPersonality Management (for braking and following distance)
|
||||
def set_long_personality(self, personality: int) -> bool:
|
||||
"""Set LongPersonality (relaxed=0, standard=1, aggressive=2)"""
|
||||
if personality in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
|
||||
self.long_personality = personality
|
||||
self.params.put(self.param_keys['long_personality'], str(personality))
|
||||
return True
|
||||
return False
|
||||
|
||||
def cycle_long_personality(self) -> int:
|
||||
"""Cycle through LongPersonality: relaxed -> standard -> aggressive -> relaxed"""
|
||||
personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
|
||||
current_idx = personalities.index(self.long_personality)
|
||||
next_personality = personalities[(current_idx + 1) % len(personalities)]
|
||||
self.set_long_personality(next_personality)
|
||||
return int(next_personality)
|
||||
|
||||
def get_long_personality(self) -> int:
|
||||
"""Get current LongPersonality"""
|
||||
self._update_from_params()
|
||||
return int(self.long_personality)
|
||||
|
||||
# Toggle Functions
|
||||
def toggle_personality(self): return self._toggle_flag('enabled')
|
||||
def toggle_accel_personality(self): return self._toggle_flag('accel_enabled')
|
||||
def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled')
|
||||
|
||||
def _toggle_flag(self, key):
|
||||
current = self._get_toggle_state(key)
|
||||
self._set_toggle_state(key, not current)
|
||||
return not current
|
||||
|
||||
def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled)
|
||||
|
||||
# Feature-specific enable checks
|
||||
def is_accel_enabled(self) -> bool:
|
||||
self._update_from_params()
|
||||
return self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled')
|
||||
|
||||
def is_follow_enabled(self) -> bool:
|
||||
self._update_from_params()
|
||||
return self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled')
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
self._update_from_params()
|
||||
return (self._get_toggle_state('enabled') and
|
||||
(self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled')))
|
||||
|
||||
def get_accel_limits(self, v_ego: float) -> tuple[float, float] | None:
|
||||
"""
|
||||
Get acceleration limits based on current personalities.
|
||||
- Max acceleration from AccelPersonality (eco/normal/sport)
|
||||
- Min acceleration (braking) from LongPersonality (relaxed/standard/aggressive)
|
||||
Returns None if controller is disabled.
|
||||
"""
|
||||
self._update_from_params()
|
||||
if not self.is_accel_enabled():
|
||||
return None
|
||||
|
||||
try:
|
||||
# Max acceleration from AccelPersonality
|
||||
max_a = self._interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality],
|
||||
self.max_accel_slopes[self.accel_personality])
|
||||
|
||||
# Min acceleration (braking) from LongPersonality
|
||||
min_a = self._interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality],
|
||||
self.min_accel_slopes[self.long_personality])
|
||||
|
||||
return float(min_a), float(max_a)
|
||||
except (KeyError, IndexError):
|
||||
return None
|
||||
|
||||
def get_follow_distance_multiplier(self, v_ego: float) -> float | None:
|
||||
"""Get following distance multiplier based on LongPersonality only"""
|
||||
self._update_from_params()
|
||||
if not self.is_follow_enabled():
|
||||
return None
|
||||
|
||||
try:
|
||||
profile = FOLLOW_DISTANCE_PROFILES[self.long_personality]
|
||||
multiplier = float(self._interpolate(v_ego, profile['x_vel'], profile['y_dist'],
|
||||
self.follow_distance_slopes[self.long_personality]))
|
||||
return multiplier
|
||||
except (KeyError, IndexError):
|
||||
return None
|
||||
|
||||
def get_personality_info(self) -> dict:
|
||||
"""Get comprehensive info about current personalities and settings"""
|
||||
self._update_from_params()
|
||||
|
||||
accel_names = {AccelPersonality.eco: "Eco", AccelPersonality.normal: "Normal", AccelPersonality.sport: "Sport"}
|
||||
long_names = {LongPersonality.relaxed: "Relaxed", LongPersonality.standard: "Standard", LongPersonality.aggressive: "Aggressive"}
|
||||
|
||||
info = {
|
||||
"accel_personality": accel_names.get(self.accel_personality, "Unknown"),
|
||||
"accel_personality_int": self.accel_personality,
|
||||
"long_personality": long_names.get(self.long_personality, "Unknown"),
|
||||
"long_personality_int": self.long_personality,
|
||||
"enabled": self._get_toggle_state('enabled'),
|
||||
"accel_enabled": self._get_toggle_state('accel_enabled'),
|
||||
"follow_enabled": self._get_toggle_state('follow_enabled'),
|
||||
"accel_description": f"Acceleration: {accel_names.get(self.accel_personality, 'Unknown')}",
|
||||
"long_description": f"Following/Braking: {long_names.get(self.long_personality, 'Unknown')}",
|
||||
}
|
||||
|
||||
return info
|
||||
|
||||
def get_min_accel(self, v_ego: float) -> float | None:
|
||||
"""Get minimum acceleration (braking) from distance mode"""
|
||||
limits = self.get_accel_limits(v_ego)
|
||||
return limits[0] if limits else None
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float | None:
|
||||
"""Get maximum acceleration from drive mode"""
|
||||
limits = self.get_accel_limits(v_ego)
|
||||
return limits[1] if limits else None
|
||||
|
||||
def reset(self):
|
||||
"""Reset to default modes"""
|
||||
self.accel_personality = AccelPersonality.normal
|
||||
self.long_personality = LongPersonality.standard
|
||||
self.frame = 0
|
||||
|
||||
def update(self):
|
||||
"""Update frame counter"""
|
||||
self.frame = (self.frame + 1) % 1000000
|
||||
|
||||
def _compute_slopes(self, x, y):
|
||||
"""Compute slopes for Hermite interpolation using symmetric difference method."""
|
||||
n = len(x)
|
||||
if n < 2:
|
||||
raise ValueError("At least two points required")
|
||||
|
||||
m = np.zeros(n)
|
||||
for i in range(n):
|
||||
if i == 0:
|
||||
m[i] = (y[1] - y[0]) / (x[1] - x[0])
|
||||
elif i == n-1:
|
||||
m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
|
||||
else:
|
||||
m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
|
||||
return m
|
||||
|
||||
def _interpolate(self, x, xp, yp, slopes):
|
||||
"""Perform cubic Hermite interpolation."""
|
||||
x = np.clip(x, xp[0], xp[-1])
|
||||
idx = np.clip(np.searchsorted(xp, x) - 1, 0, len(slopes) - 2)
|
||||
|
||||
x0, x1 = xp[idx], xp[idx+1]
|
||||
y0, y1 = yp[idx], yp[idx+1]
|
||||
m0, m1 = slopes[idx], slopes[idx+1]
|
||||
|
||||
t = (x - x0) / (x1 - x0)
|
||||
h = [2*t**3 - 3*t**2 + 1, t**3 - 2*t**2 + t, -2*t**3 + 3*t**2, t**3 - t**2]
|
||||
|
||||
return h[0]*y0 + h[1]*(x1 - x0)*m0 + h[2]*y1 + h[3]*(x1 - x0)*m1
|
||||
@@ -381,20 +381,20 @@ def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: str = N
|
||||
return {"success": 1}
|
||||
|
||||
|
||||
def scan_dir(path: str, prefix: str, base: str) -> list[str]:
|
||||
def scan_dir(path: str, prefix: str) -> list[str]:
|
||||
files = []
|
||||
# only walk directories that match the prefix
|
||||
# (glob and friends traverse entire dir tree)
|
||||
with os.scandir(path) as i:
|
||||
for e in i:
|
||||
rel_path = os.path.relpath(e.path, base)
|
||||
rel_path = os.path.relpath(e.path, Paths.log_root())
|
||||
if e.is_dir(follow_symlinks=False):
|
||||
# add trailing slash
|
||||
rel_path = os.path.join(rel_path, '')
|
||||
# if prefix is a partial dir name, current dir will start with prefix
|
||||
# if prefix is a partial file name, prefix with start with dir name
|
||||
if rel_path.startswith(prefix) or prefix.startswith(rel_path):
|
||||
files.extend(scan_dir(e.path, prefix, base))
|
||||
files.extend(scan_dir(e.path, prefix))
|
||||
else:
|
||||
if rel_path.startswith(prefix):
|
||||
files.append(rel_path)
|
||||
@@ -402,12 +402,7 @@ def scan_dir(path: str, prefix: str, base: str) -> list[str]:
|
||||
|
||||
@dispatcher.add_method
|
||||
def listDataDirectory(prefix='') -> list[str]:
|
||||
internal_files = scan_dir(Paths.log_root(), prefix, Paths.log_root())
|
||||
try:
|
||||
external_files = scan_dir(Paths.log_root_external(), prefix, Paths.log_root_external())
|
||||
except FileNotFoundError:
|
||||
external_files = []
|
||||
return sorted(set(internal_files + external_files))
|
||||
return scan_dir(Paths.log_root(), prefix)
|
||||
|
||||
|
||||
@dispatcher.add_method
|
||||
@@ -432,13 +427,8 @@ def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlRespo
|
||||
failed.append(file.fn)
|
||||
continue
|
||||
|
||||
path_internal = os.path.join(Paths.log_root(), file.fn)
|
||||
path_external = os.path.join(Paths.log_root_external(), file.fn)
|
||||
if os.path.exists(path_internal) or os.path.exists(strip_zst_extension(path_internal)):
|
||||
path = path_internal
|
||||
elif os.path.exists(path_external) or os.path.exists(strip_zst_extension(path_external)):
|
||||
path = path_external
|
||||
else:
|
||||
path = os.path.join(Paths.log_root(), file.fn)
|
||||
if not os.path.exists(path) and not os.path.exists(strip_zst_extension(path)):
|
||||
failed.append(file.fn)
|
||||
continue
|
||||
|
||||
|
||||
@@ -20,10 +20,6 @@ class Paths:
|
||||
else:
|
||||
return '/data/media/0/realdata/'
|
||||
|
||||
@staticmethod
|
||||
def log_root_external() -> str:
|
||||
return '/mnt/external_realdata/'
|
||||
|
||||
@staticmethod
|
||||
def swaglog_root() -> str:
|
||||
if PC:
|
||||
|
||||
@@ -9,26 +9,21 @@ STATS_DIR_FILE_LIMIT = 10000
|
||||
STATS_SOCKET = "ipc:///tmp/stats"
|
||||
STATS_FLUSH_TIME_S = 60
|
||||
|
||||
PATH_DICT = {
|
||||
"internal": Paths.log_root(),
|
||||
"external": Paths.log_root_external()
|
||||
}
|
||||
|
||||
def get_available_percent(default: float, path_type="internal") -> float:
|
||||
def get_available_percent(default: float) -> float:
|
||||
try:
|
||||
statvfs = os.statvfs(PATH_DICT[path_type])
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
||||
except (OSError, KeyError):
|
||||
except OSError:
|
||||
available_percent = default
|
||||
|
||||
return available_percent
|
||||
|
||||
|
||||
def get_available_bytes(default: int, path_type="internal") -> int:
|
||||
def get_available_bytes(default: int) -> int:
|
||||
try:
|
||||
statvfs = os.statvfs(PATH_DICT[path_type])
|
||||
statvfs = os.statvfs(Paths.log_root())
|
||||
available_bytes = statvfs.f_bavail * statvfs.f_frsize
|
||||
except (OSError, KeyError):
|
||||
except OSError:
|
||||
available_bytes = default
|
||||
|
||||
return available_bytes
|
||||
|
||||
@@ -2,7 +2,6 @@
|
||||
import os
|
||||
import shutil
|
||||
import threading
|
||||
from pathlib import Path
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.loggerd.config import get_available_bytes, get_available_percent
|
||||
@@ -62,33 +61,6 @@ def deleter_thread(exit_event: threading.Event):
|
||||
if any(name.endswith(".lock") for name in os.listdir(delete_path)):
|
||||
continue
|
||||
|
||||
if Path(Paths.log_root_external()).is_mount():
|
||||
out_of_bytes_external = get_available_bytes(default=MIN_BYTES + 1, path_type="external") < MIN_BYTES
|
||||
out_of_percent_external = get_available_percent(default=MIN_PERCENT + 1, path_type="external") < MIN_PERCENT
|
||||
|
||||
if out_of_percent_external or out_of_bytes_external:
|
||||
dirs_external = listdir_by_creation(Paths.log_root_external())
|
||||
|
||||
# remove the earliest external directory we can
|
||||
for delete_dir_external in sorted(dirs_external):
|
||||
delete_path_external = os.path.join(Paths.log_root_external(), delete_dir_external)
|
||||
try:
|
||||
cloudlog.info(f"deleting {delete_path_external}")
|
||||
shutil.rmtree(delete_path_external)
|
||||
break
|
||||
except OSError:
|
||||
cloudlog.exception(f"issue deleting {delete_path_external}")
|
||||
|
||||
# move directory from internal to external
|
||||
path_external = os.path.join(Paths.log_root_external(), delete_dir)
|
||||
try:
|
||||
cloudlog.info(f"moving {delete_path} to {path_external}")
|
||||
shutil.move(delete_path, path_external)
|
||||
break
|
||||
except Exception as e:
|
||||
cloudlog.exception(f"issue moving {delete_path} to {path_external}: {str(e)}")
|
||||
continue
|
||||
|
||||
try:
|
||||
cloudlog.info(f"deleting {delete_path}")
|
||||
shutil.rmtree(delete_path)
|
||||
|
||||
@@ -45,6 +45,7 @@ def manager_init() -> None:
|
||||
]
|
||||
|
||||
sunnypilot_default_params: list[tuple[str, str | bytes]] = [
|
||||
("AccelPersonality", "1"),
|
||||
("AutoLaneChangeTimer", "0"),
|
||||
("AutoLaneChangeBsmDelay", "0"),
|
||||
("BlindSpot", "0"),
|
||||
@@ -74,12 +75,10 @@ def manager_init() -> None:
|
||||
("QuickBootToggle", "0"),
|
||||
("QuietMode", "0"),
|
||||
("ShowAdvancedControls", "0" if build_metadata.tested_channel else "1"),
|
||||
("EnableHkgTuningAngleSmoothingFactor", "1"),
|
||||
("HkgTuningAngleMinTorqueReductionGain", "10"),
|
||||
("HkgTuningAngleActiveTorqueReductionGain", "60"),
|
||||
("HkgTuningAngleMaxTorqueReductionGain", "100"),
|
||||
("HkgTuningOverridingCycles", "17"),
|
||||
("HkgAngleLiveTuning", "0"),
|
||||
("VibePersonalityEnabled", "0"),
|
||||
("VibeAccelPersonalityEnabled", "0"),
|
||||
("VibeFollowPersonalityEnabled", "0"),
|
||||
|
||||
]
|
||||
|
||||
# device boot mode
|
||||
|
||||
@@ -117,7 +117,7 @@ procs = [
|
||||
|
||||
PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
|
||||
NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
|
||||
PythonProcess("soundd", "selfdrive.ui.soundd", and_(only_onroad, not_joystick)),
|
||||
PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
|
||||
PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
|
||||
NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
|
||||
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -5,10 +5,6 @@
|
||||
With joystick_control, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard.
|
||||
joystick_control uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks.
|
||||
|
||||
## Note
|
||||
You might need to install [xow](https://github.com/medusalix/xow) to use the Xbox One controller on comma device.
|
||||
Even though **xone** is recommended on the xow page, **xow** is the one that works with the comma device.
|
||||
|
||||
## Usage
|
||||
|
||||
The car must be off, and openpilot must be offroad before starting `joystick_control`.
|
||||
|
||||
@@ -3,10 +3,7 @@ import os
|
||||
import argparse
|
||||
import threading
|
||||
import numpy as np
|
||||
try:
|
||||
import pygame as pg
|
||||
except ImportError:
|
||||
print("pygame is not installed. Please install it with 'uv pip install pygame'")
|
||||
from inputs import UnpluggedError, get_gamepad
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
@@ -14,10 +11,7 @@ from openpilot.common.realtime import Ratekeeper
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
from openpilot.tools.lib.kbhit import KBHit
|
||||
|
||||
# Set SDL environment variable to avoid using a video driver for headless operation
|
||||
os.environ["SDL_VIDEODRIVER"] = "dummy"
|
||||
|
||||
EXPO = 0.4 # Exponential factor for joystick response curve
|
||||
EXPO = 0.4
|
||||
|
||||
|
||||
class Keyboard:
|
||||
@@ -46,122 +40,60 @@ class Keyboard:
|
||||
return True
|
||||
|
||||
|
||||
class PyGameJoystick:
|
||||
class Joystick:
|
||||
def __init__(self):
|
||||
# Initialize pygame and joystick subsystem
|
||||
pg.init()
|
||||
if not pg.joystick.get_init():
|
||||
pg.joystick.init()
|
||||
|
||||
# Find connected joysticks
|
||||
joystick_count = pg.joystick.get_count()
|
||||
if joystick_count == 0:
|
||||
print("No joysticks found. Please connect a controller.")
|
||||
exit(1)
|
||||
|
||||
# Initialize the first joystick
|
||||
self.joystick = pg.joystick.Joystick(0)
|
||||
self.joystick.init()
|
||||
|
||||
print(f"Using joystick: {self.joystick.get_name()}")
|
||||
print(f"Number of axes: {self.joystick.get_numaxes()}")
|
||||
print(f"Number of buttons: {self.joystick.get_numbuttons()}")
|
||||
print(f"Number of hats: {self.joystick.get_numhats()}")
|
||||
|
||||
# This supports PlayStation and Xbox controllers on different platforms
|
||||
# This class supports a PlayStation 5 DualSense controller on the comma 3X
|
||||
# TODO: find a way to get this from API or detect gamepad/PC, perhaps "inputs" doesn't support it
|
||||
self.cancel_button = 'BTN_NORTH' # BTN_NORTH=X/triangle
|
||||
if HARDWARE.get_device_type() == 'pc':
|
||||
# Xbox mapping on PC
|
||||
self.accel_axis = 5 # Right trigger
|
||||
self.brake_axis = 4 # Left trigger
|
||||
self.steer_axis = 0 # Left stick horizontal
|
||||
self.cancel_button = 3 # Y/Triangle button
|
||||
accel_axis = 'ABS_Z'
|
||||
steer_axis = 'ABS_RX'
|
||||
# TODO: once the longcontrol API is finalized, we can replace this with outputting gas/brake and steering
|
||||
self.flip_map = {'ABS_RZ': accel_axis}
|
||||
else:
|
||||
# PlayStation mapping on comma device
|
||||
self.accel_axis = 5 # R2
|
||||
self.brake_axis = 4 # L2
|
||||
self.steer_axis = 0 # Left stick horizontal
|
||||
self.cancel_button = 3 # Triangle button
|
||||
accel_axis = 'ABS_RX'
|
||||
steer_axis = 'ABS_Z'
|
||||
self.flip_map = {'ABS_RY': accel_axis}
|
||||
|
||||
# Configure for adaptive mappings based on detected controller
|
||||
controller_name = self.joystick.get_name().lower()
|
||||
if "xbox" in controller_name:
|
||||
print("Xbox controller detected, using Xbox mappings")
|
||||
self.accel_axis = 5 # Right trigger (RT)
|
||||
self.brake_axis = 4 # Left trigger (LT)
|
||||
self.cancel_button = 3 # Y button
|
||||
elif "playstation" in controller_name or "dual" in controller_name:
|
||||
print("PlayStation controller detected, using PlayStation mappings")
|
||||
self.accel_axis = 5 # R2
|
||||
self.brake_axis = 4 # L2
|
||||
self.cancel_button = 3 # Triangle
|
||||
|
||||
# Initialize values
|
||||
self.axes_values = {'gb': 0., 'steer': 0.} # Maintain same keys as Keyboard class
|
||||
self.axes_order = ['gb', 'steer'] # Match expected format
|
||||
self.min_axis_value = {accel_axis: 0., steer_axis: 0.}
|
||||
self.max_axis_value = {accel_axis: 255., steer_axis: 255.}
|
||||
self.axes_values = {accel_axis: 0., steer_axis: 0.}
|
||||
self.axes_order = [accel_axis, steer_axis]
|
||||
self.cancel = False
|
||||
self.deadzone = 0.03 # 3% deadzone for noisy joysticks
|
||||
|
||||
# Process events once to clear the event queue
|
||||
pg.event.pump()
|
||||
|
||||
def update(self):
|
||||
# Process pygame events
|
||||
try:
|
||||
for event in pg.event.get():
|
||||
if event.type == pg.JOYDEVICEREMOVED:
|
||||
if event.instance_id == self.joystick.get_instance_id():
|
||||
print("Joystick disconnected!")
|
||||
self.axes_values = {'gb': 0., 'steer': 0.}
|
||||
return False
|
||||
|
||||
elif event.type == pg.JOYBUTTONDOWN:
|
||||
if event.button == self.cancel_button:
|
||||
self.cancel = True
|
||||
|
||||
elif event.type == pg.JOYBUTTONUP:
|
||||
if event.button == self.cancel_button:
|
||||
self.cancel = False
|
||||
except Exception as e:
|
||||
print(f"Error processing events: {e}")
|
||||
joystick_event = get_gamepad()[0]
|
||||
except (OSError, UnpluggedError):
|
||||
self.axes_values = dict.fromkeys(self.axes_values, 0.)
|
||||
return False
|
||||
|
||||
# Read current joystick state directly
|
||||
try:
|
||||
if not self.joystick.get_init():
|
||||
print("Joystick not initialized")
|
||||
return False
|
||||
event = (joystick_event.code, joystick_event.state)
|
||||
|
||||
# Read steering (left stick horizontal)
|
||||
steer_raw = self.joystick.get_axis(self.steer_axis) * -1
|
||||
steer = steer_raw if abs(steer_raw) > self.deadzone else 0.0
|
||||
# flip left trigger to negative accel
|
||||
if event[0] in self.flip_map:
|
||||
event = (self.flip_map[event[0]], -event[1])
|
||||
|
||||
# Read gas (right trigger)
|
||||
accel_raw = self.joystick.get_axis(self.accel_axis)
|
||||
# Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
|
||||
accel = (accel_raw + 1) / 2 if self.accel_axis in [4, 5] else accel_raw
|
||||
accel = accel if accel > self.deadzone else 0.0
|
||||
if event[0] == self.cancel_button:
|
||||
if event[1] == 1:
|
||||
self.cancel = True
|
||||
elif event[1] == 0: # state 0 is falling edge
|
||||
self.cancel = False
|
||||
elif event[0] in self.axes_values:
|
||||
self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
|
||||
self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
|
||||
|
||||
# Read brake (left trigger)
|
||||
brake_raw = self.joystick.get_axis(self.brake_axis)
|
||||
# Convert from [-1, 1] to [0, 1] range (triggers often start at -1 when not pressed)
|
||||
brake = (brake_raw + 1) / 2 if self.brake_axis in [4, 5] else brake_raw
|
||||
brake = brake if brake > self.deadzone else 0.0
|
||||
|
||||
# Apply expo for steering
|
||||
self.axes_values['steer'] = EXPO * steer**3 + (1 - EXPO) * steer # Apply expo for fine control
|
||||
|
||||
# Calculate combined gas/brake value for output [-1, 1] where negative is brake
|
||||
self.axes_values['gb'] = accel - brake
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error reading joystick: {e}")
|
||||
self.axes_values = {'gb': 0., 'steer': 0.}
|
||||
norm = -float(np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]))
|
||||
norm = norm if abs(norm) > 0.03 else 0. # center can be noisy, deadzone of 3%
|
||||
self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm # less action near center for fine control
|
||||
else:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def send_thread(joystick):
|
||||
pm = messaging.PubMaster(['testJoystick'])
|
||||
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
|
||||
while True:
|
||||
@@ -173,6 +105,7 @@ def send_thread(joystick):
|
||||
joystick_msg.testJoystick.axes = [joystick.axes_values[ax] for ax in joystick.axes_order]
|
||||
|
||||
pm.send('testJoystick', joystick_msg)
|
||||
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
@@ -184,12 +117,13 @@ def joystick_control_thread(joystick):
|
||||
|
||||
|
||||
def main():
|
||||
joystick_control_thread(PyGameJoystick())
|
||||
joystick_control_thread(Joystick())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' +
|
||||
'openpilot must be offroad before starting joystick_control. This tool supports ' +
|
||||
'PlayStation and Xbox controllers on various platforms.',
|
||||
'a PlayStation 5 DualSense controller on the comma 3X.',
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick')
|
||||
args = parser.parse_args()
|
||||
@@ -205,12 +139,9 @@ if __name__ == '__main__':
|
||||
print('Buttons')
|
||||
print('- `R`: Resets axes')
|
||||
print('- `C`: Cancel cruise control')
|
||||
joystick_control_thread(Keyboard())
|
||||
else:
|
||||
print('Using pygame joystick')
|
||||
print('Standard controller mapping:')
|
||||
print('- Left stick: Steering')
|
||||
print('- Right trigger (R2): Gas')
|
||||
print('- Left trigger (L2): Brake')
|
||||
print('- Triangle/Y button: Cancel')
|
||||
joystick_control_thread(PyGameJoystick())
|
||||
print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!')
|
||||
print('If not running on a comma device, the mapping may need to be adjusted.')
|
||||
|
||||
joystick = Keyboard() if args.keyboard else Joystick()
|
||||
joystick_control_thread(joystick)
|
||||
|
||||
@@ -19,24 +19,18 @@ def joystickd_thread():
|
||||
CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
|
||||
VM = VehicleModel(CP)
|
||||
|
||||
sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick', 'selfdriveStateSP'], frequency=1. / DT_CTRL)
|
||||
sm = messaging.SubMaster(['carState', 'onroadEvents', 'liveParameters', 'selfdriveState', 'testJoystick'], frequency=1. / DT_CTRL)
|
||||
pm = messaging.PubMaster(['carControl', 'controlsState'])
|
||||
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
while 1:
|
||||
sm.update(0)
|
||||
ss_sp = sm['selfdriveStateSP']
|
||||
_lat_active = False
|
||||
if ss_sp.mads.available:
|
||||
_lat_active = ss_sp.mads.active
|
||||
else:
|
||||
_lat_active = sm['selfdriveState'].active
|
||||
|
||||
cc_msg = messaging.new_message('carControl')
|
||||
cc_msg.valid = True
|
||||
CC = cc_msg.carControl
|
||||
CC.enabled = sm['selfdriveState'].enabled
|
||||
CC.latActive = _lat_active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
|
||||
CC.latActive = sm['selfdriveState'].active and not sm['carState'].steerFaultTemporary and not sm['carState'].steerFaultPermanent
|
||||
CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in sm['onroadEvents']) and CP.openpilotLongitudinalControl
|
||||
CC.cruiseControl.cancel = sm['carState'].cruiseState.enabled and (not CC.enabled or not CP.pcmCruise)
|
||||
CC.hudControl.leadDistanceBars = 2
|
||||
|
||||
@@ -1,128 +0,0 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab containers="1" tab_name="actuator data">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.25;0.25;0.25;0.25" count="4">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-1.050000" top="1.050000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/actuators/torque" color="#17becf"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-553.384988" top="395.185006" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/actuators/steeringAngleDeg" color="#1f77b4"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-0.444933" top="18.242243" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/vEgoRaw" color="#ff7f0e"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-0.119883" top="0.177208" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/actuators/curvature" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab containers="1" tab_name="steering messages (CAN)">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-0.025000" top="1.025000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/can/128/LKAS_ALT/ADAS_ACIAnglTqRedcGainVal" color="#1f77b4"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-189.000000" top="189.000000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/can/128/LKAS_ALT/ADAS_StrAnglReqVal" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-0.025000" top="1.025000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/steeringPressed" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-4.150000" top="170.150000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/pandaStates/0/safetyTxBlocked" color="#1f77b4"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-0.025000" top="1.025000" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/can/1/CCNC_0x161/DAW_ICON" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab containers="1" tab_name="Understanding Torque">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.5;0.5" count="2">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="590.696728" bottom="-6.175000" top="253.175000" left="429.901019"/>
|
||||
<limitY/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
|
||||
<range right="558.699945" bottom="-107.707498" top="64.807501" left="6.491930"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/steeringTorqueEps" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="1"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default time_axis="" delimiter="0"/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="WebSocket Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="Reactive Script Editor">
|
||||
<library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/>
|
||||
<scripts/>
|
||||
</plugin>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
@@ -1,209 +0,0 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab tab_name="actuator data" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.25;0.25;0.25;0.25" count="4">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-1.050000" top="1.050000" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#17becf" name="/carControl/actuators/torque"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-453.043646" top="538.555487" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-0.647461" top="26.545898" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/carState/vEgoRaw"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-0.209157" top="0.175448" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="/carControl/actuators/curvature"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="steering messages (CAN)" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-189.000000" top="189.000000" right="723.115449" left="6.371879"/>
|
||||
<limitY/>
|
||||
<curve color="#b6ff00" name="/can/128/LKAS_ALT/LKAS_ANGLE_CMD"/>
|
||||
<curve color="#00fab2" name="/can/0/LKAS_ALT/LKAS_ANGLE_CMD"/>
|
||||
<curve color="#d62728" name="/can/192/LKAS_ALT/LKAS_ANGLE_CMD"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-6.250000" top="256.250000" right="722.420439" left="6.497277"/>
|
||||
<limitY/>
|
||||
<curve color="#00ffe8" name="/can/128/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
|
||||
<curve color="#e5fd00" name="/can/0/LKAS_ALT/LKAS_ANGLE_MAX_TORQUE"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-4.399729" top="4.056632" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="IMU_LatAccelVal_ms^2_roll_compensated"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#d62728" name="/carState/steeringPressed"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-0.025000" top="1.025000" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="/can/1/CCNC_0x161/DAW_ICON"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="Understanding Torque" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.5;0.5" count="2">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-6.250000" top="256.250000" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range bottom="-42.062501" top="78.162503" right="722.420439" left="0.000000"/>
|
||||
<limitY/>
|
||||
<curve color="#1ac938" name="/carState/steeringTorqueEps"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="1"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default delimiter="0" time_axis=""/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="WebSocket Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="Reactive Script Editor">
|
||||
<library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/>
|
||||
<scripts/>
|
||||
</plugin>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations>
|
||||
<snippet name="Desired lateral accel (roll compensated)">
|
||||
<global></global>
|
||||
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Actual lateral accel (roll compensated)">
|
||||
<global></global>
|
||||
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
|
||||
<global></global>
|
||||
<function>if (v1 == 0 and v2 == 1) then
|
||||
return (value * -9.8) - (v3 * 9.81)
|
||||
end
|
||||
--return 0
|
||||
return (value * -9.8) - (v3 * 9.81)</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
<v3>/liveParameters/roll</v3>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_Ms^3">
|
||||
<global></global>
|
||||
<function>return value * -9.8</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_Ms^2">
|
||||
<global></global>
|
||||
<function>if (v1 == 0 and v2 == 1) then
|
||||
return value * -9.8
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="roll compensated lateral acceleration">
|
||||
<global></global>
|
||||
<function>if (v3 == 0 and v4 == 1) then
|
||||
return (value * v1 ^ 2) - (v2 * 9.81)
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v3>/carState/steeringPressed</v3>
|
||||
<v4>/carControl/latActive</v4>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_ms^2">
|
||||
<global></global>
|
||||
<function>return value * -9.8</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
</customMathEquations>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
@@ -1,281 +0,0 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab containers="1" tab_name="tab1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.500397;0.499603" count="2" orientation="-">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
|
||||
<range top="256.250000" left="0.000000" right="421.102293" bottom="-6.250000"/>
|
||||
<limitY/>
|
||||
<curve color="#1ac938" name="/can/1/LFA_ALT/LKAS_ANGLE_MAX_TORQUE"/>
|
||||
<curve color="#17becf" name="max torque(calc)">
|
||||
<transform name="Moving Average" alias="max torque(calc)[Moving Average]">
|
||||
<options compensate_offset="true" value="10"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" flip_y="false" mode="TimeSeries" style="Lines">
|
||||
<range top="2.776497" left="0.000000" right="421.102293" bottom="-2.918548"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="desired lat accel"/>
|
||||
<curve color="#888888" name="zero"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="0"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default time_axis="" delimiter="0"/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="WebSocket Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="Reactive Script Editor">
|
||||
<library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/>
|
||||
<scripts/>
|
||||
</plugin>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<previouslyLoaded_Datafiles>
|
||||
<fileInfo prefix="" filename="../tmpa_e8kwpj.rlog">
|
||||
<selected_datasources value=""/>
|
||||
</fileInfo>
|
||||
</previouslyLoaded_Datafiles>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations>
|
||||
<snippet name="zero">
|
||||
<global>min=0
|
||||
max=250
|
||||
max_from_speed=96
|
||||
|
||||
rate_lim = 500
|
||||
|
||||
la_deadzone = 0.38
|
||||
|
||||
k1=200
|
||||
k2=20
|
||||
k3=1.0
|
||||
k4=1
|
||||
k5=10
|
||||
|
||||
old = 0
|
||||
|
||||
function sign(number)
|
||||
return number > 0 and 1 or (number == 0 and 0 or -1)
|
||||
end
|
||||
|
||||
function apply_rate_limit(old, new, limit)
|
||||
return math.min(math.max(new, old - limit), old + limit)
|
||||
end
|
||||
|
||||
function apply_deadzone(val, deadzone)
|
||||
if math.abs(val) <= deadzone then
|
||||
return 0.0
|
||||
elseif val < 0.0 then
|
||||
return val + deadzone
|
||||
else
|
||||
return val - deadzone
|
||||
end
|
||||
end</global>
|
||||
<function>return 0</function>
|
||||
<linked_source>/carState/aEgo</linked_source>
|
||||
</snippet>
|
||||
<snippet name="max torque lj adj">
|
||||
<global>min=0
|
||||
max=250
|
||||
max_from_speed=96
|
||||
|
||||
k1=200
|
||||
k2=30
|
||||
k3=1
|
||||
k4=1
|
||||
k5=10
|
||||
|
||||
function sign(number)
|
||||
return number > 0 and 1 or (number == 0 and 0 or -1)
|
||||
end</global>
|
||||
<function>return 250 - value * 20</function>
|
||||
<linked_source>desired lateral jark</linked_source>
|
||||
</snippet>
|
||||
<snippet name="ang_cmd rate">
|
||||
<global>firstX = 0
|
||||
firstY = 0
|
||||
is_first = true
|
||||
secondX = 0
|
||||
secondY = 0
|
||||
is_second = false</global>
|
||||
<function>-- Wait for initial values
|
||||
if (is_first) then
|
||||
is_first = false
|
||||
is_second = true
|
||||
firstX = time
|
||||
firstY = value
|
||||
end
|
||||
|
||||
if (is_second) then
|
||||
is_second = false
|
||||
secondX = time
|
||||
secondY = value
|
||||
end
|
||||
|
||||
-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
|
||||
dx = time - firstX
|
||||
dy = value - firstY
|
||||
-- Increment
|
||||
firstX = secondX
|
||||
firstY = secondY
|
||||
secondX = time
|
||||
secondY = value
|
||||
|
||||
return dy/dx</function>
|
||||
<linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
|
||||
</snippet>
|
||||
<snippet name="max torque(calc)">
|
||||
<global>min=0
|
||||
max=250
|
||||
max_from_speed=96
|
||||
|
||||
rate_lim = 500
|
||||
|
||||
la_deadzone = 0.38
|
||||
|
||||
k1=200
|
||||
k2=20
|
||||
k3=1.0
|
||||
k4=1
|
||||
k5=10
|
||||
|
||||
old = 0
|
||||
|
||||
function sign(number)
|
||||
return number > 0 and 1 or (number == 0 and 0 or -1)
|
||||
end
|
||||
|
||||
function apply_rate_limit(old, new, limit)
|
||||
return math.min(math.max(new, old - limit), old + limit)
|
||||
end
|
||||
|
||||
function apply_deadzone(val, deadzone)
|
||||
if math.abs(val) <= deadzone then
|
||||
return 0.0
|
||||
elseif val < 0.0 then
|
||||
return val + deadzone
|
||||
else
|
||||
return val - deadzone
|
||||
end
|
||||
end</global>
|
||||
<function>la = apply_deadzone(v2, la_deadzone)
|
||||
lj = v3
|
||||
|
||||
if la == 0.0 then
|
||||
lj = 0.0
|
||||
end
|
||||
|
||||
fla = math.min(math.abs(k1 * la)^k3, max)
|
||||
flj = math.min(math.abs(k2 * lj)^k4, max)
|
||||
|
||||
out = fla
|
||||
|
||||
flv = math.min(max_from_speed, k5 * v4)
|
||||
|
||||
out = out + flv
|
||||
|
||||
out = math.max(math.min(out, max), min)
|
||||
|
||||
if sign(la) == sign(lj) then
|
||||
out = out - flj
|
||||
else
|
||||
out = out + flj
|
||||
end
|
||||
|
||||
|
||||
if v5 == 1.0 then
|
||||
out = 0.0
|
||||
end
|
||||
|
||||
out = math.max(math.min(out, max), min)
|
||||
out = apply_rate_limit(old, out, rate_lim)
|
||||
old = out
|
||||
|
||||
return out</function>
|
||||
<linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
|
||||
<additional_sources>
|
||||
<v1>ang_cmd rate</v1>
|
||||
<v2>desired lat accel</v2>
|
||||
<v3>desired lateral jark</v3>
|
||||
<v4>/carState/vEgo</v4>
|
||||
<v5>/can/1/LFA_ALT/LKAS_ANGLE_ACTIVE</v5>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="desired lateral jark">
|
||||
<global>firstX = 0
|
||||
firstY = 0
|
||||
is_first = true
|
||||
secondX = 0
|
||||
secondY = 0
|
||||
is_second = false</global>
|
||||
<function>-- Wait for initial values
|
||||
if (is_first) then
|
||||
is_first = false
|
||||
is_second = true
|
||||
firstX = time
|
||||
firstY = value
|
||||
end
|
||||
|
||||
if (is_second) then
|
||||
is_second = false
|
||||
secondX = time
|
||||
secondY = value
|
||||
end
|
||||
|
||||
-- Central derivative: dy/dx ~= f(x+delta_x)-f(x-delta_x)/(2*delta_x)
|
||||
dx = time - firstX
|
||||
dy = value - firstY
|
||||
-- Increment
|
||||
firstX = secondX
|
||||
firstY = secondY
|
||||
secondX = time
|
||||
secondY = value
|
||||
|
||||
return dy/dx</function>
|
||||
<linked_source>desired lat accel</linked_source>
|
||||
</snippet>
|
||||
<snippet name="abs(des la accel)">
|
||||
<global></global>
|
||||
<function>return math.abs(value)</function>
|
||||
<linked_source>desired lat accel</linked_source>
|
||||
</snippet>
|
||||
<snippet name="desired lat accel">
|
||||
<global></global>
|
||||
<function>return value * v1^2</function>
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="abs(ang_cmd)">
|
||||
<global></global>
|
||||
<function>return math.abs(value)</function>
|
||||
<linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
|
||||
</snippet>
|
||||
</customMathEquations>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
@@ -1,175 +0,0 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab tab_name="tab1" containers="1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="-3.582051" right="1431.113121" top="5.314632" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="Actual lateral accel (roll compensated)" color="#1ac938"/>
|
||||
<curve name="Desired lateral accel (roll compensated)" color="#ff7f0e"/>
|
||||
<curve name="IMU_LatAccelVal_ms^2" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="-3.741271" right="1431.113121" top="3.756006" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="roll compensated lateral acceleration" color="#ff7f0e"/>
|
||||
<curve name="IMU_LatAccelVal_Ms^2" color="#1ac938"/>
|
||||
<curve name="IMU_LatAccelVal_ms^2_roll_compensated" color="#9467bd"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="-0.025000" right="1431.113121" top="1.025000" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/steeringPressed" color="#0097ff"/>
|
||||
<curve name="/carOutput/actuatorsOutput/torque" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="-1.660728" right="1431.113121" top="67.942958" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/vEgo" color="#f14cc1">
|
||||
<transform name="Scale/Offset" alias="/carState/vEgo[Scale/Offset]">
|
||||
<options value_scale="2.23694" time_offset="0" value_offset="0"/>
|
||||
</transform>
|
||||
</curve>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab tab_name="tab2" containers="1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.5;0.5" count="2" orientation="-">
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="30595.900000" right="1431.113121" top="34884.100000" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="/can/1/IMU_01_10ms/IMU_RollRtVal" color="#17becf"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
|
||||
<range bottom="-0.058795" right="1431.113121" top="0.072448" left="5.322399"/>
|
||||
<limitY/>
|
||||
<curve name="/liveParameters/roll" color="#bcbd22"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="1"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default time_axis="" delimiter="0"/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
<plugin ID="DataLoad ULog"/>
|
||||
<plugin ID="Cereal Subscriber"/>
|
||||
<plugin ID="UDP Server"/>
|
||||
<plugin ID="WebSocket Server"/>
|
||||
<plugin ID="ZMQ Subscriber"/>
|
||||
<plugin ID="Fast Fourier Transform"/>
|
||||
<plugin ID="Quaternion to RPY"/>
|
||||
<plugin ID="Reactive Script Editor">
|
||||
<library code="--[[ Helper function to create a series from arrays

 new_series: a series previously created with ScatterXY.new(name)
 prefix: prefix of the timeseries, before the index of the array
 suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.
 suffix_Y: suffix to complete the name of the series containing the Y value
 timestamp: usually the tracker_time variable
 
 Example:
 
 Assuming we have multiple series in the form:
 
 /trajectory/node.{X}/position/x
 /trajectory/node.{X}/position/y
 
 where {N} is the index of the array (integer). We can create a reactive series from the array with:
 
 new_series = ScatterXY.new("my_trajectory") 
 CreateSeriesFromArray( new_series, "/trajectory/node", "position/x", "position/y", tracker_time );
--]]

function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )
 
 --- clear previous values
 new_series:clear()
 
 --- Append points to new_series
 index = 0
 while(true) do

 x = index;
 -- if not nil, get the X coordinate from a series
 if suffix_X ~= nil then 
 series_x = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_X) )
 if series_x == nil then break end
 x = series_x:atTime(timestamp)	 
 end
 
 series_y = TimeseriesView.find( string.format( "%s.%d/%s", prefix, index, suffix_Y) )
 if series_y == nil then break end 
 y = series_y:atTime(timestamp)
 
 new_series:push_back(x,y)
 index = index+1
 end
end

--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]

function GetSeriesNamesByPrefix(prefix)
 -- GetSeriesNames(9 is a built-in function
 all_names = GetSeriesNames()
 filtered_names = {}
 for i, name in ipairs(all_names) do
 -- check the prefix
 if name:find(prefix, 1, #prefix) then
 table.insert(filtered_names, name);
 end
 end
 return filtered_names
end

--[[ Modify an existing series, applying offsets to all their X and Y values

 series: an existing timeseries, obtained with TimeseriesView.find(name)
 delta_x: offset to apply to each x value
 delta_y: offset to apply to each y value 
 
--]]

function ApplyOffsetInPlace(series, delta_x, delta_y)
 -- use C++ indeces, not Lua indeces
 for index=0, series:size()-1 do
 x,y = series:at(index)
 series:set(index, x + delta_x, y + delta_y)
 end
end
"/>
|
||||
<scripts/>
|
||||
</plugin>
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations>
|
||||
<snippet name="IMU_LatAccelVal_ms^2">
|
||||
<global></global>
|
||||
<function>return value * -9.8</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="roll compensated lateral acceleration">
|
||||
<global></global>
|
||||
<function>if (v3 == 0 and v4 == 1) then
|
||||
return (value * v1 ^ 2) - (v2 * 9.81)
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v3>/carState/steeringPressed</v3>
|
||||
<v4>/carControl/latActive</v4>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_Ms^2">
|
||||
<global></global>
|
||||
<function>if (v1 == 0 and v2 == 1) then
|
||||
return value * -9.8
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_Ms^3">
|
||||
<global></global>
|
||||
<function>return value * -9.8</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_ms^2_roll_compensated">
|
||||
<global></global>
|
||||
<function>
|
||||
|
||||
if (v1 == 0 and v2 == 1) then
|
||||
return (value * -9.8) - (v3 * 9.81)
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/can/1/IMU_01_10ms/IMU_LatAccelVal</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/latActive</v2>
|
||||
<v3>/liveParameters/roll</v3>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Actual lateral accel (roll compensated)">
|
||||
<global></global>
|
||||
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Desired lateral accel (roll compensated)">
|
||||
<global></global>
|
||||
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
</customMathEquations>
|
||||
<snippets/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
</root>
|
||||
|
||||
Reference in New Issue
Block a user