Compare commits

..

4 Commits

Author SHA1 Message Date
github-actions[bot] 36f1a62116 sunnypilot modeld: Sync Tinygrad (PR-1081) 2025-07-30 16:23:02 +00:00
github-actions[bot] becc36d2f0 ui: refresh model list (PR-1074) 2025-07-30 16:23:01 +00:00
github-actions[bot] 48b11c0a7d Long Control: Vibe Controller (PR-1068) 2025-07-30 16:23:00 +00:00
github-actions[bot] e3b50553c6 Visual: Move chevron to sp/qt (PR-1066) 2025-07-30 16:22:57 +00:00
57 changed files with 973 additions and 3829 deletions
-1
View File
@@ -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 }}
-7
View File
@@ -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>
-7
View File
@@ -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 &quot;source .venv/bin/activate &amp;&amp; scons -u -j$(nproc) tools/replay/&quot;" />
<option name="WORKING_DIRECTORY" value="$ProjectFileDir$" />
</exec>
</tool>
</toolSet>
+1 -1
View File
@@ -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>
-27
View File
@@ -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>
-7
View File
@@ -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>
-7
View File
@@ -1,7 +0,0 @@
<component name="ProjectRunConfigurationManager">
<configuration default="false" name="replay for controls" type="CLionNativeAppRunConfigurationType" focusToolWindowBeforeRun="true" PROGRAM_PARAMS="&quot;$Prompt$&quot; --block &quot;sendcan,carState,carParams,carOutput,liveTracks,carParamsSP,carStateSP&quot;" 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>
+7
View File
@@ -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 {
+4 -9
View File
@@ -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}
};
+2 -1
View File
@@ -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",
+1 -1
View File
@@ -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() {
+1 -4
View File
@@ -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),
+16 -3
View File
@@ -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]
+19 -1
View File
@@ -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 &param) {
@@ -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.");
+1
View File
@@ -88,6 +88,7 @@ protected:
Params params;
std::map<std::string, ParamControl*> toggles;
ButtonParamControl *long_personality_setting;
ButtonParamControl *accel_personality_setting;
virtual void updateToggles();
};
+1 -168
View File
@@ -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);
}
}
}
}
-10
View File
@@ -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;
};
-2
View File
@@ -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;
};
+157
View File
@@ -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);
}
+12 -1
View File
@@ -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();
};
+1
View File
@@ -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
+6 -16
View File
@@ -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
-4
View File
@@ -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:
+6 -11
View File
@@ -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
-28
View File
@@ -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)
+5 -6
View File
@@ -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
+1 -1
View File
@@ -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
-4
View File
@@ -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`.
+46 -115
View File
@@ -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)
+2 -8
View File
@@ -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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<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) &lt;= deadzone then
return 0.0
elseif val &lt; 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) &lt;= deadzone then
return 0.0
elseif val &lt; 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&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<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>