mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-24 22:52:05 +08:00
Merge remote-tracking branch 'sunnypilot/sunnypilot/hkg-angle-steering-2025' into hkg-angle-steering-2025
This commit is contained in:
Jason Wen
@@ -246,7 +246,22 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
|
||||
}
|
||||
}
|
||||
|
||||
struct HkgAngleDebugLimits {
|
||||
maxAngle @0 :Float32;
|
||||
maxAngleDelta @1 :Float32;
|
||||
}
|
||||
|
||||
struct HkgAngleDebug {
|
||||
applyAngle @0 :Float32;
|
||||
applyAngleSmooth @1 :Float32;
|
||||
applyAngleLimited @2 :Float32;
|
||||
applyAngleBaselineLimited @3 :Float32;
|
||||
currentLimits @4 :HkgAngleDebugLimits;
|
||||
baselineLimits @5 :HkgAngleDebugLimits;
|
||||
}
|
||||
|
||||
struct CarStateSP @0xb86e6369214c01c8 {
|
||||
hkgAngleDebug @0 :HkgAngleDebug;
|
||||
}
|
||||
|
||||
struct LiveMapDataSP @0xf416ec09499d9d19 {
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: 7bf72c82f0...2b0c06a4ee
@@ -110,10 +110,23 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) {
|
||||
list->addItem(lagd_toggle_control);
|
||||
|
||||
// Software delay control
|
||||
int liveDelayMaxInt = 30;
|
||||
std::string liveDelayBytes = params.get("LiveDelay");
|
||||
if (!liveDelayBytes.empty()) {
|
||||
capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(
|
||||
reinterpret_cast<const capnp::word*>(liveDelayBytes.data()),
|
||||
liveDelayBytes.size() / sizeof(capnp::word)));
|
||||
auto event = msg.getRoot<cereal::Event>();
|
||||
if (event.hasLiveDelay()) {
|
||||
auto liveDelay = event.getLiveDelay();
|
||||
float lateralDelay = liveDelay.getLateralDelay();
|
||||
liveDelayMaxInt = static_cast<int>(lateralDelay * 100.0f) + 20;
|
||||
}
|
||||
}
|
||||
delay_control = new OptionControlSP("LagdToggleDelay", tr("Adjust Software Delay"),
|
||||
tr("Adjust the software delay when Live Learning Steer Delay is toggled off."
|
||||
"\nThe default software delay value is 0.2"),
|
||||
"", {5, 30}, 1, false, nullptr, true, true);
|
||||
"", {5, liveDelayMaxInt}, 1, false, nullptr, true, true);
|
||||
|
||||
connect(delay_control, &OptionControlSP::updateLabels, [=]() {
|
||||
float value = QString::fromStdString(params.get("LagdToggleDelay")).toFloat();
|
||||
@@ -367,8 +380,39 @@ void ModelsPanel::updateLabels() {
|
||||
|
||||
// Update lagdToggle description with current value
|
||||
QString desc = tr("Enable this for the car to learn and adapt its steering response time. "
|
||||
"Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience. "
|
||||
"The Current value is updated automatically when the vehicle is Onroad.");
|
||||
"Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience.");
|
||||
bool lagdEnabled = params.getBool("LagdToggle");
|
||||
if (lagdEnabled) {
|
||||
std::string liveDelayBytes = params.get("LiveDelay");
|
||||
if (!liveDelayBytes.empty()) {
|
||||
capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(
|
||||
reinterpret_cast<const capnp::word*>(liveDelayBytes.data()),
|
||||
liveDelayBytes.size() / sizeof(capnp::word)));
|
||||
auto event = msg.getRoot<cereal::Event>();
|
||||
if (event.hasLiveDelay()) {
|
||||
auto liveDelay = event.getLiveDelay();
|
||||
float lateralDelay = liveDelay.getLateralDelay();
|
||||
desc += QString("<br><br><b><span style=\"color:#e0e0e0\">%1</span></b> <span style=\"color:#e0e0e0\">%2 s</span>")
|
||||
.arg(tr("Live Steer Delay:")).arg(QString::number(lateralDelay, 'f', 2));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
std::string carParamsBytes = params.get("CarParamsPersistent");
|
||||
if (!carParamsBytes.empty()) {
|
||||
capnp::FlatArrayMessageReader msg(kj::ArrayPtr<const capnp::word>(
|
||||
reinterpret_cast<const capnp::word*>(carParamsBytes.data()),
|
||||
carParamsBytes.size() / sizeof(capnp::word)));
|
||||
auto carParams = msg.getRoot<cereal::CarParams>();
|
||||
float steerDelay = carParams.getSteerActuatorDelay();
|
||||
float softwareDelay = QString::fromStdString(params.get("LagdToggleDelay")).toFloat();
|
||||
float totalLag = steerDelay + softwareDelay;
|
||||
desc += QString("<br><br><span style=\"color:#e0e0e0\">"
|
||||
"<b>%1</b> %2 s + <b>%3</b> %4 s = <b>%5</b> %6 s</span>")
|
||||
.arg(tr("Actuator Delay:"), QString::number(steerDelay, 'f', 2),
|
||||
tr("Software Delay:"), QString::number(softwareDelay, 'f', 2),
|
||||
tr("Total Delay:"), QString::number(totalLag, 'f', 2));
|
||||
}
|
||||
}
|
||||
lagd_toggle_control->setDescription(desc);
|
||||
|
||||
delay_control->setVisible(!params.getBool("LagdToggle"));
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,33 +1,33 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<Tab containers="1" tab_name="actuator data">
|
||||
<Tab tab_name="actuator data" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.25;0.25;0.25;0.25" count="4">
|
||||
<DockSplitter sizes="0.25;0.25;0.25;0.25" count="4" orientation="-">
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-0.100000" top="0.100000"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-129.414922" top="81.867986"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="0.977702" top="10.773600"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-0.028790" top="0.046017"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/actuators/curvature" color="#f14cc1"/>
|
||||
</plot>
|
||||
@@ -35,40 +35,41 @@
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab containers="1" tab_name="steering messages (CAN)">
|
||||
<Tab tab_name="steering messages (CAN)" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5">
|
||||
<DockSplitter sizes="0.200218;0.200218;0.199129;0.200218;0.200218" count="5" orientation="-">
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="0.077500" top="1.022500"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-146.187738" top="80.597360"/>
|
||||
<limitY/>
|
||||
<curve name="/can/128/LKAS_ALT/ADAS_StrAnglReqVal" color="#d62728"/>
|
||||
<curve name="/can/128/LKAS_ALT/ADAS_StrAnglReqVal" color="#29d627"/>
|
||||
<curve name="/can/192/LKAS_ALT/ADAS_StrAnglReqVal" color="#b41f32"/>
|
||||
</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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-0.025000" top="1.025000"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="149.100000" top="396.900000"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-0.025000" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="/can/1/CCNC_0x161/DAW_ICON" color="#f14cc1"/>
|
||||
</plot>
|
||||
@@ -76,18 +77,18 @@
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<Tab containers="1" tab_name="Understanding Torque">
|
||||
<Tab tab_name="Understanding Torque" containers="1">
|
||||
<Container>
|
||||
<DockSplitter orientation="-" sizes="0.5;0.5" count="2">
|
||||
<DockSplitter sizes="0.5;0.5" count="2" orientation="-">
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="590.696728" left="429.901019" bottom="-6.175000" top="253.175000"/>
|
||||
<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"/>
|
||||
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
|
||||
<range right="376.602125" left="368.465617" bottom="-19.564999" top="12.565000"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/steeringTorqueEps" color="#1ac938"/>
|
||||
</plot>
|
||||
@@ -102,7 +103,7 @@
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default time_axis="" delimiter="0"/>
|
||||
<default delimiter="0" time_axis=""/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
@@ -120,6 +121,12 @@
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<previouslyLoaded_Datafiles>
|
||||
<fileInfo filename="../tmpkgs3ntkk.rlog" prefix="">
|
||||
<selected_datasources value=""/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
</fileInfo>
|
||||
</previouslyLoaded_Datafiles>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations/>
|
||||
<snippets/>
|
||||
|
||||
@@ -1,35 +1,35 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<tabbed_widget name="Main Window" parent="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"/>
|
||||
<range right="396.045059" bottom="-0.100000" left="6.467547" top="0.100000"/>
|
||||
<limitY/>
|
||||
<curve color="#17becf" name="/carControl/actuators/torque"/>
|
||||
<curve name="/carControl/actuators/torque" color="#17becf"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-356.587494" left="6.467547" top="491.287506"/>
|
||||
<limitY/>
|
||||
<curve color="#1f77b4" name="/carControl/actuators/steeringAngleDeg"/>
|
||||
<curve name="/carControl/actuators/steeringAngleDeg" color="#1f77b4"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-0.856228" left="6.467547" top="35.834527"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="/carState/vEgoRaw"/>
|
||||
<curve name="/carState/vEgoRaw" color="#ff7f0e"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-0.186684" left="6.467547" top="0.134230"/>
|
||||
<limitY/>
|
||||
<curve color="#f14cc1" name="/carControl/actuators/curvature"/>
|
||||
<curve name="/carControl/actuators/curvature" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
@@ -37,43 +37,33 @@
|
||||
</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">
|
||||
<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="-189.000000" top="189.000000" right="723.115449" left="6.371879"/>
|
||||
<range right="396.045059" bottom="-0.022000" left="6.467547" top="0.902000"/>
|
||||
<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"/>
|
||||
<curve name="/sendcan/0/LKAS_ALT/ADAS_ACIAnglTqRedcGainVal" color="#1f77b4"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-5.168301" left="6.467547" top="4.590794"/>
|
||||
<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"/>
|
||||
<curve name="IMU_LatAccelVal_ms^2_roll_compensated" color="#ff7f0e"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-0.025000" left="6.467547" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve color="#ff7f0e" name="IMU_LatAccelVal_ms^2_roll_compensated"/>
|
||||
<curve name="/carState/steeringPressed" color="#d62728"/>
|
||||
</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"/>
|
||||
<range right="396.045059" bottom="-0.100000" left="6.467547" top="0.100000"/>
|
||||
<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"/>
|
||||
<curve name="/can/1/CCNC_0x161/DAW_ICON" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
@@ -84,29 +74,71 @@
|
||||
<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"/>
|
||||
<range right="722.420439" bottom="-6.250000" left="0.000000" top="256.250000"/>
|
||||
<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"/>
|
||||
<range right="396.045059" bottom="-34.457500" left="6.467547" top="26.757499"/>
|
||||
<limitY/>
|
||||
<curve color="#1ac938" name="/carState/steeringTorqueEps"/>
|
||||
<curve name="/carState/steeringTorqueEps" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="1"/>
|
||||
<Tab tab_name="Angle Error and Saturation TQ" 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 right="396.045059" bottom="-356.587494" left="6.467547" top="491.287506"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/actuators/steeringAngleDeg" color="#d62728"/>
|
||||
<curve name="/carState/steeringAngleDeg" color="#1ac938"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range right="396.045059" bottom="-134.196237" left="6.467547" top="34.112077"/>
|
||||
<limitY/>
|
||||
<curve name="Angle Error" color="#ff7f0e"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range right="396.045059" bottom="-0.022000" left="6.467547" top="0.902000"/>
|
||||
<limitY/>
|
||||
<curve name="/sendcan/0/LKAS_ALT/ADAS_ACIAnglTqRedcGainVal" color="#1f77b4"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range right="396.045059" bottom="-0.025000" left="6.467547" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="Angle Staturation" color="#f14cc1"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
|
||||
<range right="396.045059" bottom="-0.025000" left="6.467547" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="/carControl/latActive" color="#9467bd"/>
|
||||
<curve name="/carState/steeringPressed" color="#17becf"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
</DockSplitter>
|
||||
</Container>
|
||||
</Tab>
|
||||
<currentTabIndex index="3"/>
|
||||
</tabbed_widget>
|
||||
<use_relative_time_offset enabled="1"/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<Plugins>
|
||||
<plugin ID="DataLoad CSV">
|
||||
<default delimiter="0" time_axis=""/>
|
||||
<default time_axis="" delimiter="0"/>
|
||||
</plugin>
|
||||
<plugin ID="DataLoad MCAP"/>
|
||||
<plugin ID="DataLoad Rlog"/>
|
||||
@@ -124,15 +156,401 @@
|
||||
<plugin ID="CSV Exporter"/>
|
||||
</Plugins>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<previouslyLoaded_Datafiles>
|
||||
<fileInfo filename="../tmpgh9xhmjb.rlog" prefix="">
|
||||
<selected_datasources value=""/>
|
||||
</fileInfo>
|
||||
</previouslyLoaded_Datafiles>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
<customMathEquations>
|
||||
<snippet name="Desired lateral accel (roll compensated)">
|
||||
<snippet name="Angle Staturation">
|
||||
<global></global>
|
||||
<function>return (value * v1 ^ 2) - (v2 * 9.81)</function>
|
||||
<linked_source>/controlsState/desiredCurvature</linked_source>
|
||||
<function>if value > .3 then
|
||||
return 1
|
||||
end
|
||||
|
||||
return 0</function>
|
||||
<linked_source>Angle Error</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 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="max torque(calc)">
|
||||
<global>min=0
|
||||
max=250
|
||||
max_from_speed=96
|
||||
|
||||
rate_lim = 500
|
||||
|
||||
la_deadzone = 0.38
|
||||
|
||||
k1=200
|
||||
k2=20
|
||||
k3=1.0
|
||||
k4=1
|
||||
k5=10
|
||||
|
||||
old = 0
|
||||
|
||||
function sign(number)
|
||||
return number > 0 and 1 or (number == 0 and 0 or -1)
|
||||
end
|
||||
|
||||
function apply_rate_limit(old, new, limit)
|
||||
return math.min(math.max(new, old - limit), old + limit)
|
||||
end
|
||||
|
||||
function apply_deadzone(val, deadzone)
|
||||
if math.abs(val) <= deadzone then
|
||||
return 0.0
|
||||
elseif val < 0.0 then
|
||||
return val + deadzone
|
||||
else
|
||||
return val - deadzone
|
||||
end
|
||||
end</global>
|
||||
<function>la = apply_deadzone(v2, la_deadzone)
|
||||
lj = v3
|
||||
|
||||
if la == 0.0 then
|
||||
lj = 0.0
|
||||
end
|
||||
|
||||
fla = math.min(math.abs(k1 * la)^k3, max)
|
||||
flj = math.min(math.abs(k2 * lj)^k4, max)
|
||||
|
||||
out = fla
|
||||
|
||||
flv = math.min(max_from_speed, k5 * v4)
|
||||
|
||||
out = out + flv
|
||||
|
||||
out = math.max(math.min(out, max), min)
|
||||
|
||||
if sign(la) == sign(lj) then
|
||||
out = out - flj
|
||||
else
|
||||
out = out + flj
|
||||
end
|
||||
|
||||
|
||||
if v5 == 1.0 then
|
||||
out = 0.0
|
||||
end
|
||||
|
||||
out = math.max(math.min(out, max), min)
|
||||
out = apply_rate_limit(old, out, rate_lim)
|
||||
old = out
|
||||
|
||||
return out</function>
|
||||
<linked_source>/can/1/LFA_ALT/LKAS_ANGLE_CMD</linked_source>
|
||||
<additional_sources>
|
||||
<v1>ang_cmd rate</v1>
|
||||
<v2>desired lat accel</v2>
|
||||
<v3>desired lateral jark</v3>
|
||||
<v4>/carState/vEgo</v4>
|
||||
<v5>/can/1/LFA_ALT/LKAS_ANGLE_ACTIVE</v5>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="carState.vEgo mph">
|
||||
<global></global>
|
||||
<function>return value * 2.23694</function>
|
||||
<linked_source>/carState/vEgo</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="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="Zero">
|
||||
<global></global>
|
||||
<function>return (0)</function>
|
||||
<linked_source>/carState/canValid</linked_source>
|
||||
</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>
|
||||
<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="engaged_accel_actuator">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>accel = value
|
||||
brake = v1
|
||||
gas = v2
|
||||
enabled = v3
|
||||
|
||||
if (brake ~= 0 or gas ~= 0 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return value
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/carControl/actuators/accel</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/brakePressed</v1>
|
||||
<v2>/carState/gasPressed</v2>
|
||||
<v3>/carControl/enabled</v3>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Angle Error">
|
||||
<global>last_angle_requested = 0</global>
|
||||
<function>angle_error = last_angle_requested - v1
|
||||
|
||||
|
||||
last_angle_requested = value
|
||||
return angle_error</function>
|
||||
<linked_source>/carControl/actuators/steeringAngleDeg</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringAngleDeg</v1>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="carState.vEgo kmh">
|
||||
<global></global>
|
||||
<function>return value * 3.6</function>
|
||||
<linked_source>/carState/vEgo</linked_source>
|
||||
</snippet>
|
||||
<snippet name="engaged_accel_plan">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>accel = value
|
||||
brake = v1
|
||||
gas = v2
|
||||
enabled = v3
|
||||
|
||||
if (brake ~= 0 or gas ~= 0 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return value
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/longitudinalPlan/accels/0</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/brakePressed</v1>
|
||||
<v2>/carState/gasPressed</v2>
|
||||
<v3>/carControl/enabled</v3>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="engaged_accel_actual">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>accel = value
|
||||
brake = v1
|
||||
gas = v2
|
||||
enabled = v3
|
||||
|
||||
if (brake ~= 0 or gas ~= 0 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return value
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/carState/aEgo</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/brakePressed</v1>
|
||||
<v2>/carState/gasPressed</v2>
|
||||
<v3>/carControl/enabled</v3>
|
||||
</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="engaged curvature plan">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>curvature = value
|
||||
pressed = v1
|
||||
enabled = v2
|
||||
|
||||
if (pressed == 1 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return value
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/lateralPlan/curvatures/0</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/enabled</v2>
|
||||
</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>
|
||||
<snippet name="zero">
|
||||
<global>min=0
|
||||
max=250
|
||||
max_from_speed=96
|
||||
|
||||
rate_lim = 500
|
||||
|
||||
la_deadzone = 0.38
|
||||
|
||||
k1=200
|
||||
k2=20
|
||||
k3=1.0
|
||||
k4=1
|
||||
k5=10
|
||||
|
||||
old = 0
|
||||
|
||||
function sign(number)
|
||||
return number > 0 and 1 or (number == 0 and 0 or -1)
|
||||
end
|
||||
|
||||
function apply_rate_limit(old, new, limit)
|
||||
return math.min(math.max(new, old - limit), old + limit)
|
||||
end
|
||||
|
||||
function apply_deadzone(val, deadzone)
|
||||
if math.abs(val) <= deadzone then
|
||||
return 0.0
|
||||
elseif val < 0.0 then
|
||||
return val + deadzone
|
||||
else
|
||||
return val - deadzone
|
||||
end
|
||||
end</global>
|
||||
<function>return 0</function>
|
||||
<linked_source>/carState/aEgo</linked_source>
|
||||
</snippet>
|
||||
<snippet name="engaged curvature vehicle model">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>curvature = value
|
||||
pressed = v1
|
||||
enabled = v2
|
||||
|
||||
if (pressed == 1 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return value
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/enabled</v2>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="Actual lateral accel (roll compensated)">
|
||||
@@ -158,15 +576,6 @@ end
|
||||
<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
|
||||
@@ -179,21 +588,30 @@ return 0</function>
|
||||
<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)
|
||||
<snippet name="engaged curvature yaw">
|
||||
<global>engage_delay = 5
|
||||
last_bad_time = -engage_delay</global>
|
||||
<function>curvature = value / v3
|
||||
pressed = v1
|
||||
enabled = v2
|
||||
|
||||
if (pressed == 1 or enabled == 0) then
|
||||
last_bad_time = time
|
||||
end
|
||||
return 0</function>
|
||||
<linked_source>/controlsState/curvature</linked_source>
|
||||
|
||||
if (time > last_bad_time + engage_delay) then
|
||||
return curvature
|
||||
else
|
||||
return 0
|
||||
end</function>
|
||||
<linked_source>/liveLocationKalman/angularVelocityCalibrated/value/2</linked_source>
|
||||
<additional_sources>
|
||||
<v1>/carState/vEgo</v1>
|
||||
<v2>/liveParameters/roll</v2>
|
||||
<v3>/carState/steeringPressed</v3>
|
||||
<v4>/carControl/latActive</v4>
|
||||
<v1>/carState/steeringPressed</v1>
|
||||
<v2>/carControl/enabled</v2>
|
||||
<v3>/liveLocationKalman/velocityCalibrated/value/0</v3>
|
||||
</additional_sources>
|
||||
</snippet>
|
||||
<snippet name="IMU_LatAccelVal_ms^2">
|
||||
<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>
|
||||
@@ -202,6 +620,15 @@ return 0</function>
|
||||
<v2>/carControl/latActive</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/>
|
||||
<!-- - - - - - - - - - - - - - - -->
|
||||
|
||||
Reference in New Issue
Block a user