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-05-18 00:32:24 -05:00
parent 52b6f7abea
commit 41b4f73622
8 changed files with 194 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
common/params.cc
View File

@@ -232,6 +232,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CarMake", PERSISTENT},
{"CarModel", PERSISTENT},
{"CarSpeedLimit", PERSISTENT},
{"CarSpeedLimitLiteral", PERSISTENT},
{"CECurves", PERSISTENT},
{"CECurvesLead", PERSISTENT},
{"CENavigation", PERSISTENT},

10
openpilot/clearpilot_devqueue_amain.txt
View File

@@ -1,5 +1,15 @@
- fix lane lines
- fix always on lateral detection
- position override under 25 mph suspends assist
- output the three speeds
- test - if engaged, adjust the speed twoards experimental one unit every second
- disable onroad on parked, not car off
- test that lkas button crashes controlsd, it should
- test that lkas button creates an alert, it should

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

@@ -70,7 +70,16 @@ class CarState(CarStateBase):
speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
# print(speed_limit_bus.vl, sys.stderr)
# sys.stderr.write(str({k: v for k, v in vars(speed_limit_bus).items() if isinstance(v, (int, float, str, bool))}) + '\n')
return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
# Clearpilot fix
best_speed = 0
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]):
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
if (best_speed == 0 or best_speed < speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]
if (best_speed == 0):
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return best_speed
else:
if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
return 0
@@ -199,11 +208,15 @@ class CarState(CarStateBase):
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
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_conv)
return ret
def update_canfd(self, cp, cp_cam, frogpilot_variables):
# ---- Preexisting unsorted frogpilot code ----
ret = car.CarState.new_message()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
@@ -280,28 +293,28 @@ class CarState(CarStateBase):
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
# testme
# if ret.cruiseState.speed > 1 and self.main_enabled == False:
# self.main_enabled = True
# ------ End old frogpilot code ------
# test if this blocks trying to engage while pressin brakes
# Clearpilot Activation button fixes:
# This code tracks the engaged state of openpilot based on when the user clicks the
# cruise button. However, cruise only engages when the break is not pressed,
# and cruise can already be enabled when openpilot starts. This compensates for that behavior.
# Block attempt to engage if already engaged according to cruise state
if ret.cruiseState > 0 and self.main_enabled == False:
self.main_buttons[-1] = 0
# Block attempt to engage if breaks are pressed
if self.main_buttons[-1] != 0 and ret.brakePressed and self.main_enabled == False:
self.main_buttons[-1] = 0
# Suprisingly this works. Test with op long and submit.
# But enable always on lateral if self.main_enabled = false
# I think this may require an override variable?
# It looks like always on lat is only set when cruise control is enabled
# We need a temp version that enables or disables it if cruise is off via button
# and then the temp state removes when the button is pressed again
# Swicth to enabled based on button if above conditions pass
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
# if self.fix_main_enabled_check:
# self.fix_main_enabled_check = False
# if ret.cruiseState.speed > 1 and self.main_enabled == False:
# self.fix_main_enabled_cancel_main = True
# self.fix_main_enabled_executed = True
# --------- Resume preexisting frogpilot code ------------
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
@@ -346,6 +359,58 @@ class CarState(CarStateBase):
# ret: cruiseState.speed > 0 = and cruiseState.enabled = false = idle cruise.
# press cruise to cancel it if not op engaged
# engaged at 25
# ret from carstate:
# . cruiseState = (
# enabled = true,
# speed = 11.176, # above 0 if disengaged but set
# available = true,
# speedOffset = 0,
# standstill = false,
# nonAdaptive = false,
# speedCluster = 0 ),
# .standstill = false,
# steeringPressed = false,
# Interesting values:
# 416.ACC_ObjDist - distance to lead radar?
# 506.SPEED_LIMIT_2
# print(speed_limit_bus.vl, sys.stderr)
# {416: {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0, 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3':
# 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0, 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0,
# 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0, 'COUNTER': 173.0,
# 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0, 'NEW_SIGNAL_2': 0.0,
# 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0, 'DISTANCE_SETTING': 0.0,
# 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0, 'aReqValue': 0.0, 'ZEROS_7': 0.0,
# 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002, 'JerkLowerLimit': 0.0, 'StopReq': 0.0,
# 'NEW_SIGNAL_15': 204.60000000000002, 'VSetDis': 0.0, 'MainMode_ACC': 0.0,
# 'JerkUpperLimit': 0.0}, 'SCC_CONTROL': {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0,
# 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3': 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0,
# 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0, 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0,
# 'COUNTER': 173.0, 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0,
# 'NEW_SIGNAL_2': 0.0, 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0,
# 'DISTANCE_SETTING': 0.0, 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0,
# 'aReqValue': 0.0, 'ZEROS_7': 0.0, 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002,
# 'JerkLowerLimit': 0.0, 'StopReq': 0.0, 'NEW_SIGNAL_15': 204.60000000000002,
# 'VSetDis': 0.0, 'MainMode_ACC': 0.0, 'JerkUpperLimit': 0.0},
# 506: {'SPEED_LIMIT_3': 38.0,
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0},
# 'CLUSTER_SPEED_LIMIT': {'SPEED_LIMIT_3': 38.0,
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0}}
# <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
# ( enabled = false,
# speed = 9.83488,
# available = false,
# speedOffset = 0,
# standstill = false,
# nonAdaptive = false,
# speedCluster = 0 )
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)

42
selfdrive/controls/controlsd.py
View File

@@ -210,11 +210,19 @@ class Controls:
self.previous_speed_limit = 0
self.random_event_timer = 0
self.speed_limit_timer = 0
self.green_light_mac = MovingAverageCalculator()
self.update_frogpilot_params()
# Clearpilot variables go here
# These persist between frames.
# These variables reset every frame
def clearpilot_reset_frame_state(self):
# Clearpilot variables
self.force_set_speed = 0
self.force_cancel = False
self.force_resume = False
def set_initial_state(self):
if REPLAY:
controls_state = Params().get("ReplayControlsState")
@@ -232,6 +240,7 @@ class Controls:
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
self.clearpilot_reset_frame_state()
self.update_events(CS)
self.update_frogpilot_events(CS)
self.update_clearpilot_events(CS)
@@ -246,7 +255,7 @@ class Controls:
CC, lac_log = self.state_control(CS)
CC = self.clearpilot_state_control(CC, CS)
# Publish data
# Publish commands to tx on canbus (not sure why this is called logs)
self.publish_logs(CS, start_time, CC, lac_log)
self.CS_prev = CS
@@ -655,6 +664,8 @@ class Controls:
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
# Clearpilot note: this is the check for always on lateral
CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
@@ -797,6 +808,11 @@ class Controls:
CC.cruiseControl.override = self.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise) and self.CP.openpilotLongitudinalControl # IMPORTANT POSSIBLY PATCH needs and oplong
# CLearpilot
if self.force_cancel:
CC.cruiseControl.cancel = True
if self.joystick_mode and self.sm.recv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
CC.cruiseControl.cancel = True
@@ -1104,6 +1120,8 @@ class Controls:
self.FPCC.alwaysOnLateral &= self.always_on_lateral
self.FPCC.alwaysOnLateral &= self.driving_gear
self.FPCC.alwaysOnLateral &= self.speed_check
# Clearpilot: test: block always on lower than 30.
self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
# clearpilot allow experimental in stock long
@@ -1238,7 +1256,23 @@ class Controls:
def clearpilot_state_control(self, CC, CS):
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
# CC.cruiseControl.resume = True
CC.cruiseControl.cancel = True
self.force_cancel = True
# Master control for if lateral is active:
# CC.latActive
# Current speed in "MS":
# CS.vEgo
# Behavior:
# Test
# (make this a toggle)
# Pause lateral below 20 if override
# self.params.get_int("PauseLateralSpeed") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0
if CS.vEgo < (20 * CV.MPH_TO_MS) and self.events.contains(ET.OVERRIDE_LATERAL):
CC.latActive = False
return CC
def main():

13
selfdrive/controls/lib/desire_helper.py
View File

@@ -156,12 +156,13 @@ class DesireHelper:
self.prev_one_blinker = one_blinker
self.turn_completed &= one_blinker
if self.turn_direction != TurnDirection.none:
self.desire = TURN_DESIRES[self.turn_direction]
elif not self.turn_completed:
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
else:
self.desire = log.Desire.none
# Clearpilot test: disabling turn desires
# if self.turn_direction != TurnDirection.none:
# self.desire = TURN_DESIRES[self.turn_direction]
# elif not self.turn_completed:
# self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# else:
# self.desire = log.Desire.none
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):

4
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -20,6 +20,8 @@ from openpilot.system.version import get_short_branch
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
import sys
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -263,6 +265,8 @@ class LongitudinalPlanner:
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
print("LONG PLAN SPEEDS", sys.stderr)
print(longitudinalPlan.speeds, sys.stderr)
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()

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

@@ -247,6 +247,7 @@ class FrogPilotPlanner:
frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1)
frogpilotPlan.accelerationJerkStock = A_CHANGE_COST
# clearpilot: adjustedcruise is the max speed based on vision turn speed control (eval)
frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH))
frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor

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

@@ -435,14 +435,20 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
// QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
// QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
// QString speedStr = QString::number(std::nearbyint(speed));
// QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
QString speedStr = QString::number(std::nearbyint(speed));
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
p.restore();
if (!scene.hide_max_speed) {
// drawSpeedWidget(p, 60, 45, QString("MAX"), QString("33"), QColor(0xff, 0xff, 0xff));
}
// if (!scene.hide_max_speed) {
drawSpeedWidget(p, 60, 45, QString("MAX"), setSpeedStr, QColor(0xff, 0xff, 0xff));
// }
QString speedLimitStr = QString::fromStdString(paramsMemory.get());
drawSpeedWidget(p, 60, 45 + (225), QString("Limit"), CarSpeedLimitLiteral, QColor(0xff, 0xff, 0xff));
// Todo: lead speed
// Todo: Experimental speed
// Draw FrogPilot widgets
paintFrogPilotWidgets(p);
@@ -505,46 +511,46 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
// }
void AnnotatedCameraWidget::drawSpeedWidget(QPainter &p, int x, int y, const QString &title, const QString &speedLimitStr, QColor colorSpeed) {
// Draw outer box + border to contain set speed and speed limit
const int sign_margin = 12;
const int us_sign_height = 186;
const int eu_sign_size = 176;
// Draw outer box + border to contain set speed and speed limit
const int sign_margin = 12;
const int us_sign_height = 186;
const int eu_sign_size = 176;
const QSize default_size = {172, 204};
QSize set_speed_size = default_size;
if (is_metric || has_eu_speed_limit) set_speed_size.rwidth() = 200;
if (has_us_speed_limit && speedLimitStr.size() >= 3) set_speed_size.rwidth() = 223;
const QSize default_size = {172, 204};
QSize set_speed_size = default_size;
if (is_metric || has_eu_speed_limit) set_speed_size.rwidth() = 200;
if (has_us_speed_limit && speedLimitStr.size() >= 3) set_speed_size.rwidth() = 223;
if (has_us_speed_limit) set_speed_size.rheight() += us_sign_height + sign_margin;
else if (has_eu_speed_limit) set_speed_size.rheight() += eu_sign_size + sign_margin;
if (has_us_speed_limit) set_speed_size.rheight() += us_sign_height + sign_margin;
else if (has_eu_speed_limit) set_speed_size.rheight() += eu_sign_size + sign_margin;
int top_radius = 32;
int bottom_radius = has_eu_speed_limit ? 100 : 32;
int top_radius = 32;
int bottom_radius = has_eu_speed_limit ? 100 : 32;
QRect set_speed_rect(QPoint(x + (default_size.width() - set_speed_size.width()) / 2, y), set_speed_size);
p.setPen(QPen(colorSpeed));
p.setBrush(blackColor(166));
drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
QRect set_speed_rect(QPoint(x + (default_size.width() - set_speed_size.width()) / 2, y), set_speed_size);
p.setPen(QPen(colorSpeed));
p.setBrush(blackColor(166));
drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
// Draw MAX
QColor max_color = QColor(0xaa, 0xaa, 0xaa, 0xff);
p.setFont(InterFont(40, QFont::DemiBold));
p.setPen(max_color);
p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, title);
p.setFont(InterFont(90, QFont::Bold));
p.setPen(colorSpeed);
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
// Draw MAX
QColor max_color = QColor(0xaa, 0xaa, 0xaa, 0xff);
p.setFont(InterFont(40, QFont::DemiBold));
p.setPen(max_color);
p.drawText(set_speed_rect.adjusted(0, 27, 0, 0), Qt::AlignTop | Qt::AlignHCenter, title);
p.setFont(InterFont(90, QFont::Bold));
p.setPen(colorSpeed);
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
// // current speed
// if (!(scene.hide_speed)) {
// // CLEARPILOT changes to 120 from ~176
// // Maybe we want to hide this?
// p.setFont(InterFont(140, QFont::Bold));
// drawText(p, rect().center().x(), 210, speedStr);
// // CLEARPILOT changes to 40 from 66
// p.setFont(InterFont(50));
// drawText(p, rect().center().x(), 290, speedUnit, 200);
// }
if (!(scene.hide_speed)) {
// CLEARPILOT changes to 120 from ~176
// Maybe we want to hide this?
p.setFont(InterFont(140, QFont::Bold));
drawText(p, rect().center().x(), 210, speedStr);
// CLEARPILOT changes to 40 from 66
p.setFont(InterFont(50));
drawText(p, rect().center().x(), 290, speedUnit, 200);
}
}
void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
@@ -608,13 +614,16 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
int is_no_lat_lane_change = paramsMemory.getInt("no_lat_lane_change");
QColor center_lane_color = is_no_lat_lane_change ?
bg_colors[CENTER_LANE_COLOR] :
bg_colors[CHANGE_LANE_PATH_COLOR];
QColor center_lane_color;
if (is_no_lat_lane_change) {
center_lane_color = bg_colors[CENTER_LANE_COLOR];
} else {
center_lane_color = bg_colors[CHANGE_LANE_PATH_COLOR];
}
QLinearGradient path_gradient(0, height(), 0, 0);
if (edgeColor != bg_colors[STATUS_DISENGAGED] || is_no_lat_lane_change) {
if (scene.acceleration_path) {
// The first half of track_vertices are the points for the right side of the path