brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-06-09 16:08:19 -05:00
parent cb7eda64d9
commit a963ea0296
8 changed files with 63 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
selfdrive/car/hyundai/carcontroller.py
View File

@@ -123,39 +123,40 @@ class CarController(CarControllerBase):
if self.frame % 5 == 0 and (not hda2 or hda2_long):
# CLEARPILOT TEST self.CS.lkas_enabled
# can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CC.latActive))
params_memory = Params("/dev/shm/params")
lkas_icon = params_memory.get_bool("CPTLkasButtonAction") or CS.lkas_enabled
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, lkas_icon, lkas_icon))
# params_memory = Params("/dev/shm/params")
# lkas_icon = params_memory.get_bool("CPTLkasButtonAction") or CS.lkas_enabled
# lkas_icon = CS.experimentalMode
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CS.experimentalMode, False))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
# params_memory = Params("/dev/shm/params")
if params_memory.get_bool("CPTLkasButtonAction"):
if self.frame % 10 == 0:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.SET_DECEL))
# if params_memory.get_bool("CPTLkasButtonAction"):
# if self.frame % 10 == 0:
# for _ in range(20):
# can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.SET_DECEL))
# can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
# can_sends.append(hyundaicanfd.create_acc_set_speed(self.packer, self.CP, self.CAN, CS.cruise_info, 50))
# can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
# print("Debug cancel executed")
if self.CP.openpilotLongitudinalControl:
if self.CP.openpilotLongitudinalControl or CS.experimentalMode:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, hud_control))
self.accel_last = accel
else:
# else:
# Clearpilot
# If cruise control was enabled or idle on start, force cancel
# if CS.fix_main_enabled_cancel_main:
# CS.fix_main_enabled_cancel_main = False
# CC.cruiseControl.cancel = True
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
# can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
@@ -211,7 +212,7 @@ class CarController(CarControllerBase):
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
params_memory = Params("/dev/shm/params")
# params_memory = Params("/dev/shm/params")
# if params_memory.get_bool("CPTLkasButtonAction"):
# params_memory.put_bool("CPTLkasButtonAction", False)
# CC.cruiseControl.cancel = True
@@ -221,9 +222,9 @@ class CarController(CarControllerBase):
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# pass
# can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
print("Try Cancel Button Alt")
# for _ in range(20):
# can_sends.append(hyundaicanfd.create_buttons_alt(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
# print("Try Cancel Button Alt")
self.last_button_frame = self.frame
CS.lkas_trigger_result = 1
else:

4
selfdrive/car/hyundai/carstate.py
View File

@@ -209,7 +209,7 @@ class CarState(CarStateBase):
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
return ret
@@ -414,7 +414,7 @@ class CarState(CarStateBase):
# print("Set limit")
# print(self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
return ret

4
selfdrive/car/hyundai/hyundaicanfd.py
View File

@@ -164,10 +164,10 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# CLEARPILOT changed HDA icons
# This doesn't appear to do anything on my tucson
def create_lfahda_cluster(packer, CAN, enabled, lat_active):
def create_lfahda_cluster(packer, CAN, experimental, lat_active):
values = {
"HDA_ICON": 0, # Literally shows HDA. Maybe we use this to indicate experimental mode
"LFA_ICON": 2 if enabled else 1 if lat_active else 0
"LFA_ICON": 2 if experimental else 0
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)

12
selfdrive/controls/controlsd.py
View File

@@ -1237,14 +1237,14 @@ class Controls:
def clearpilot_state_control(self, CC, CS):
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
# self.params_memory.put_bool("CPTLkasButtonAction", True)
if self.params_memory.get_bool("CPTLkasButtonAction"):
self.params_memory.put_bool("CPTLkasButtonAction", False)
else:
self.params_memory.put_bool("CPTLkasButtonAction", True)
self.params_memory.put_bool("CPTLkasButtonAction", True)
# if self.params_memory.get_bool("CPTLkasButtonAction"):
# self.params_memory.put_bool("CPTLkasButtonAction", False)
# else:
# self.params_memory.put_bool("CPTLkasButtonAction", True)
# CS.lkas_enabled = self.params_memory.get_bool("CPTLkasButtonAction")
self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
# self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
# CC.actuators.speed
# print ("Alive")
return CC

2
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@@ -274,7 +274,7 @@ class FrogPilotPlanner:
frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target)
self.params_memory.put_int("SpeedLimitVTSC", frogpilotPlan.adjustedCruise)
# self.params_memory.put_int("SpeedLimitVTSC", frogpilotPlan.adjustedCruise)
pm.send('frogpilotPlan', frogpilot_plan_send)

46
selfdrive/ui/qt/onroad.cc
View File

@@ -175,52 +175,6 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
QPainter p(this);
p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
if (scene.fps_counter) {
qint64 currentMillis = QDateTime::currentMSecsSinceEpoch();
auto fpsQueue = std::queue<std::pair<qint64, double>>();
static double avgFPS = 0.0;
static double maxFPS = 0.0;
static double minFPS = 99.9;
minFPS = qMin(minFPS, fps);
maxFPS = qMax(maxFPS, fps);
fpsQueue.push({currentMillis, fps});
while (!fpsQueue.empty() && currentMillis - fpsQueue.front().first > 60000) {
fpsQueue.pop();
}
if (!fpsQueue.empty()) {
double totalFPS = 0;
for (auto tempQueue = fpsQueue; !tempQueue.empty(); tempQueue.pop()) {
totalFPS += tempQueue.front().second;
}
avgFPS = totalFPS / fpsQueue.size();
}
QString fpsDisplayString = QString("FPS: %1 (%2) | Min: %3 | Max: %4 | Avg: %5")
.arg(qRound(fps))
.arg(paramsMemory.getInt("CameraFPS"))
.arg(qRound(minFPS))
.arg(qRound(maxFPS))
.arg(qRound(avgFPS));
p.setFont(InterFont(28, QFont::DemiBold));
p.setRenderHint(QPainter::TextAntialiasing);
p.setPen(Qt::white);
QRect currentRect = rect();
int textWidth = p.fontMetrics().horizontalAdvance(fpsDisplayString);
int xPos = (currentRect.width() - textWidth) / 2;
int yPos = currentRect.bottom() - 5;
p.drawText(xPos, yPos, fpsDisplayString);
update();
}
QString logicsDisplayString = QString();
if (scene.show_jerk) {
logicsDisplayString += QString("Acceleration Jerk: %1 (%2%3) | Speed Jerk: %4 (%5%6) | ")