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

Conditional Experimental Mode

Browse Source 
Added toggles for "Conditional Experimental Mode".

Conditions based on road curvature, turn signals, speed, lead speed, navigation instructions, and stop signs/stop lights are all individually toggleable.

Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-02-28 19:46:38 -07:00
parent 89e6ebdf12
commit 50cc95341d
18 changed files with 423 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -3,10 +3,13 @@ import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
class FrogPilotPlanner:
def __init__(self, CP, params, params_memory):
self.CP = CP
@@ -14,6 +17,10 @@ class FrogPilotPlanner:
self.fpf = FrogPilotFunctions()
self.cem = ConditionalExperimentalMode(self.params_memory)
self.road_curvature = 0
self.stop_distance = 0
self.v_cruise = 0
self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)]
@@ -45,6 +52,10 @@ class FrogPilotPlanner:
self.accel_limits = [min_accel, max_accel]
# Update Conditional Experimental Mode
if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl:
self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego)
# Update the current lane widths
check_lane_width = self.blind_spot_path
if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN:
@@ -54,6 +65,12 @@ class FrogPilotPlanner:
self.lane_width_left = 0
self.lane_width_right = 0
# Update the current road curvature
self.road_curvature = self.fpf.road_curvature(modelData, v_ego)
# Update the desired stopping distance
self.stop_distance = STOP_DISTANCE
# Update the max allowed speed
self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego)
@@ -75,6 +92,8 @@ class FrogPilotPlanner:
frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
frogpilotPlan = frogpilot_plan_send.frogpilotPlan
frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
frogpilotPlan.laneWidthLeft = self.lane_width_left
frogpilotPlan.laneWidthRight = self.lane_width_right
@@ -83,6 +102,11 @@ class FrogPilotPlanner:
def update_frogpilot_params(self, params):
self.is_metric = params.get_bool("IsMetric")
self.conditional_experimental_mode = params.get_bool("ConditionalExperimental")
if self.conditional_experimental_mode:
self.cem.update_frogpilot_params(self.is_metric, params)
params.put_bool("ExperimentalMode", True)
custom_ui = params.get_bool("CustomUI")
self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath")