diff --git a/common/params.cc b/common/params.cc index f65de49..94b3b40 100755 --- a/common/params.cc +++ b/common/params.cc @@ -233,9 +233,13 @@ std::unordered_map keys = { {"CarModel", PERSISTENT}, {"CarSpeedLimit", PERSISTENT}, + + {"CarSpeedLimitWarning", PERSISTENT}, {"CarSpeedLimitLiteral", PERSISTENT}, - {"SpeedLimitLatDesired", PERSISTENT}, - {"SpeedLimitVTSC", PERSISTENT}, + + // {"SpeedLimitLatDesired", PERSISTENT}, + // {"SpeedLimitVTSC", PERSISTENT}, + {"CPTLkasButtonAction", PERSISTENT}, {"RadarDist", PERSISTENT}, diff --git a/openpilot/clearpilot_devqueue_amain.txt b/openpilot/clearpilot_devqueue_amain.txt index 944a576..f92e99d 100755 --- a/openpilot/clearpilot_devqueue_amain.txt +++ b/openpilot/clearpilot_devqueue_amain.txt @@ -1,174 +1,45 @@ ------ -Latest: -- fix hiding path if disabled -- fix suspend on override behavior - override doesnt happen on always on lateral. need alternative check. -- fix & test lane line narrow on lane change view -- the acc cancel does nothing. -- must simulate actual button presses. -- add radar dist, model dist, radar speed, model speed to debug hud -- add wheel touched, wheel override to hud +- lkas button toggle display off / minimal / full / info -- fix always on lateral saying its on when actually fully engaged +- revert a bunch of debugging in canfd -Notes on speed limit override: -- we need to capture the original speed and feed it to vtsc and model whule we - are under speed adjustment +- fix it so it reads cruise state and uses it to determine program state exclusively + - always on lat in particular ------- -- fix lane lines -- fix always on lateral detection -- position override under 25 mph suspends assist +- make override a state that can happen on always on lateral -- output the three speeds +- fix turning with override to disengage lower than 20 -- test - if engaged, adjust the speed twoards experimental one unit every second +- configure it so i can toggle experimental via lkas -- disable onroad on parked, not car off +- if experimental enabled, then transmit experimental speed settings + - but only if no lead car or not decelerating. got to think of a condition + where we reengage if the lead is going too slow and we need to let stock long decel -- test that lkas button crashes controlsd, it should -- test that lkas button creates an alert, it should +- implement speed limit controller as a simple Warning -- hyundai clsc https://github.com/garrettpall/openpilot/commit/5528198aa73d3d017d16ec4ca38306b11e5da0a8 +- implement a simple resume from standstill hack -- get ui to work on its own using no video feed, custom ui state inputs - - dress up ui +--- TRIP RELEASE --- -- add debug elements to ui with a boolean at top of onroad.cc to enable/disable: - - curviture - - current speed limit - - current cruise speed - - current experimental mode speed - - current distance to lead based on radar - - current distance to lead based on model draw distance - - current speed of lead +- configure orangepi wifi selector + - can i have it use phone bt tether? +- fix gps +- implement new onroad settings +- implement new offroad settings +- implement debug console (make it actually useful) +- implement dash cam +- implement dash cam viewer +- implement trip logger +- implement generic CLSC +- implement GM CLSC -- when changing lanes and lateral disabled, draw a narrow single 'line' down center -- of computed path, and don't show side lane boundaries +- CRAIG RELEASE - -- make side lanes 10% wider and the disengage mode 10% brighter -- make 25% of side lanes 50% darker than base +- implement logo selector +- update models, sync important changes from main +- update installer -- hello world alert triggered by lkas btn -- hello world bootstrap dashboard +- PUBLIC RELEASE - -- button stuff: -- read paddle left (for full nudgeless lane change + blinker, reset to drive) -- read paddle right ("") -- read current drive mode, reset paddle mode to drive -- write pause, res, accel, mode, drive - -wishlist: read / write info switcher, hvac, windows -wishlist: allow lane assist (not lane keep) on disengage - -test: -- disable all canbus -- set speed limit during stock long - -OP -> Oscar -- oscar - global clearpilot state var -- oscar.cs - clearpilot car state (populated by op variables for cp.planner) -- oscar.ce - clearpilot car event queue (array that can be pushed to from car) -- oscar.ceh - event queue history (for debug) -- oscar.clog - error and console.log style one off messages to transmit to logs or watching consoles -Oscar -> OP -- oscar.ps - clearpilot planner state (data from cp) -- oscar.pe - clearpilot planner event (one time events from planner to be processed by openpilot) -- oscar.peh - planner event history (for debug) -- oscar.plog - retransmits clog as well as additional data for UI based log consoles - -- oscar - clearpilot's planner - nodejs process that synchronizes state between all processes and makes decisions - -- settings menu -- - branch selector - release, stage, dev. Stage is my personal release branch. Should always be able to easilly switch to stage while im in a development state. - -- unfuck the directory structure. new structure: -- - -------- - -- make functions ---- get_curvature ---- get_wheel_angle ---- get_distance_to_left_lane ---- get_distance_to_right_lane ---- are hands on wheel ---- distance traveled for lane change ---- distance to lead -- put these on a debug. - -- Design alternate settings webview -- get speed limit display working -- get calculated experimental speed display working -- get button press emulation working -- get experimental mode working -- get speed limit set working -- dev enable lateral on blinker but no wheel pressure or no wheel presence -- bluetooth dummy device - -- Test if activation fix works for op long, submit to frog maintain -- (test) disable all turn signal output commands - they are causing issues -- test "create_acc_cancel" on canfd on cc engaged on boot -- Create clearpilot process. manages behaviors. -- experiment with reduced jerk values -- test toggle stop all canbus output -- check if acc_cancel events are being made on idle in stock long, if so, its a bug. - -- Warn if significantly slower traffic - -behaviors: - -- getting head mode - turn off babysitter with an alert on the screen that it is off, suspend for 10 minutes - - in this mode it should be extra grouchy if a curve is detected, slowdown is detected, or lane lines are weak - -- lane change wrong way reenable lateral -- blinker signal wheel angle minor enable lateral -- wheel angle sharp only engage lateral if over lane edge unless hands not on wheel -- no lateral on turn signal - only enforce if model curvature > 10 degrees, hands on wheel, or wheel override (maybe curve not necessary?) - - -- basic lane keep - nudge wheel slightly if over line and still going straight and hands on wheel - -- debug mode activated bu lkas - -- See where disk free is going with NCDU and add smarter log rotation - - Maybe this has logs where it could show what happened to frogpilot process? -- Test supress cruise icon on long paused -- increase center lane brightness 50% and make it blueish -- make drive mode color much brighter and 30% more white -- maintain lateral on icon on stop on dash -- prevent engagement if disengaged and brakes are applied, just enable lateral -- edit manager to log all stderr output -- find a way to disable all logging unless debug mode enabled (screen setting) -- set up dash cam recordings -- disable dash cam and record in real logger mode if debug mode is entered - -- Integrate here maps api for traffic data - - maybe even speed limit data? and location data? - -- write a debug function for python that cats data to a screen terminal and optionally a log file -- if cruise already engaged when boot, just enable lateral -- reengage lateral if changing lanes and changing the wrong way -- speed limit display / over speed display / trigger set -- hack the buttons so we can press them -- auto set speed limit -- conditional experimenal mode -- ui conditional experimental mode, orange lines, show large font of desired speed in lower left -- hold down button to turn off screen, remember setting -- bluetooth dummy device -- dash cam -- change disk used on sidebar to disk free / percent used -- -- warn if lead is going more than 30 under my speed or 20 if auto mode is off - -- mark os version different than release, forcing a os reinstall -- no prompt on os reinstall - - -Test features wizard: -- read paddles -- read speed -- adjust speed -- cancel / resume -- reset drive mode -- read radar distance -- activate blinkers +Hyundai CLSC +Hyundai Paddle shifter lane change \ No newline at end of file diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 9981296..6f8e560 100755 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 9b504c3..3463ebf 100755 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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 diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 65d0704..bb3eb39 100755 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -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) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 20f4e47..e44b30a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index c7c47b4..78bf726 100755 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -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) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 26bb974..2d62a9b 100755 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -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>(); - - 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) | ")