From 24055234920b0b8a465b6b416592fcdeed5b2126 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 26 May 2024 22:51:15 -0700 Subject: [PATCH] Controls - Conditional Experimental Mode Automatically switches to 'Experimental Mode' under predefined conditions. --- release/files_common | 1 + selfdrive/controls/controlsd.py | 6 ++- .../frogpilot/controls/frogpilot_planner.py | 8 ++++ .../lib/conditional_experimental_mode.py | 40 +++++++++++++++++++ selfdrive/ui/qt/offroad/settings.cc | 8 +++- 5 files changed, 61 insertions(+), 2 deletions(-) create mode 100644 selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py diff --git a/release/files_common b/release/files_common index ab23e17..3a0acd5 100644 --- a/release/files_common +++ b/release/files_common @@ -567,6 +567,7 @@ tinygrad_repo/tinygrad/*.py selfdrive/frogpilot/frogpilot_process.py selfdrive/frogpilot/controls/frogpilot_planner.py +selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py selfdrive/frogpilot/controls/lib/frogpilot_functions.py selfdrive/frogpilot/controls/lib/frogpilot_variables.py selfdrive/frogpilot/fleet_manager/fleet_manager.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 736f4bb..e10dba9 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -883,7 +883,8 @@ class Controls: def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode: + self.experimental_mode = self.params.get_bool("ExperimentalMode") self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") @@ -922,6 +923,9 @@ class Controls: self.FPCC.alwaysOnLateral &= self.driving_gear self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.frogpilot_toggles.always_on_lateral_pause_speed) or CS.standstill + if self.frogpilot_toggles.conditional_experimental_mode: + self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental + self.drive_distance += CS.vEgo * DT_CTRL self.drive_time += DT_CTRL diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 2843c57..bb1a1c8 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel +from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED @@ -23,6 +24,8 @@ class FrogPilotPlanner: def __init__(self): self.params_memory = Params("/dev/shm/params") + self.cem = ConditionalExperimentalMode() + self.acceleration_jerk = 0 self.frame = 0 self.road_curvature = 0 @@ -51,6 +54,9 @@ class FrogPilotPlanner: self.road_curvature = calculate_road_curvature(modelData, v_ego) self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles) + if frogpilot_toggles.conditional_experimental_mode: + self.cem.update(carState, controlsState.enabled, frogpilotNavigation, lead_distance, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles) + self.frame += 1 def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): @@ -78,6 +84,8 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk) frogpilotPlan.tFollow = float(self.t_follow) + frogpilotPlan.conditionalExperimental = self.cem.experimental_mode + frogpilotPlan.vCruise = float(self.v_cruise) pm.send('frogpilotPlan', frogpilot_plan_send) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py new file mode 100644 index 0000000..f8af525 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -0,0 +1,40 @@ +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY + +class ConditionalExperimentalMode: + def __init__(self): + self.params_memory = Params("/dev/shm/params") + + self.experimental_mode = False + + self.lead_detection_mac = MovingAverageCalculator() + + def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles): + self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles) + + condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles) and enabled + self.experimental_mode = condition_met + + self.params_memory.put_int("CEStatus", self.status_value if condition_met else 0) + + def check_conditions(self, carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles): + if carState.standstill: + self.status_value = 0 + return self.experimental_mode + + if (not self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit) or (self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit_lead): + self.status_value = 11 if self.lead_detected else 12 + return True + + return False + + def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles): + self.lead_detection(lead_status) + + def lead_detection(self, lead_status): + self.lead_detection_mac.add_data(lead_status) + self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4a29be0..2bf4c87 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -195,7 +195,13 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); if (hasLongitudinalControl(CP)) { // normal description and toggle - experimental_mode_toggle->setEnabled(true); + bool conditional_experimental = params.getBool("ConditionalExperimental"); + if (conditional_experimental) { + params.putBool("ExperimentalMode", true); + params.putBool("ExperimentalModeConfirmed", true); + experimental_mode_toggle->refresh(); + } + experimental_mode_toggle->setEnabled(!conditional_experimental); experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); } else {