diff --git a/release/files_common b/release/files_common index 3a0acd5..9ed0675 100644 --- a/release/files_common +++ b/release/files_common @@ -570,5 +570,6 @@ 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/controls/lib/map_turn_speed_controller.py selfdrive/frogpilot/fleet_manager/fleet_manager.py selfdrive/frogpilot/navigation/mapd.py diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index e8cfc10..c644633 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, 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 +from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController GearShifter = car.CarState.GearShifter @@ -51,11 +52,13 @@ class FrogPilotPlanner: self.params_memory = Params("/dev/shm/params") self.cem = ConditionalExperimentalMode() + self.mtsc = MapTurnSpeedController() self.slower_lead = False self.acceleration_jerk = 0 self.frame = 0 + self.mtsc_target = 0 self.road_curvature = 0 self.speed_jerk = 0 @@ -64,6 +67,7 @@ class FrogPilotPlanner: v_cruise_kph = min(controlsState.vCruise, V_CRUISE_UNSET) v_cruise = v_cruise_kph * CV.KPH_TO_MS + v_cruise_changed = self.mtsc_target < v_cruise v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead @@ -83,6 +87,8 @@ class FrogPilotPlanner: if controlsState.experimentalMode: self.min_accel = ACCEL_MIN + elif v_cruise_changed: + self.min_accel = A_CRUISE_MIN elif frogpilot_toggles.deceleration_profile == 1: self.min_accel = get_min_accel_eco(v_ego) elif frogpilot_toggles.deceleration_profile == 2: @@ -153,13 +159,25 @@ class FrogPilotPlanner: self.slower_lead = max(braking_offset - far_lead_offset, 1) > 1 def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles): + gps_check = (liveLocationKalman.status == log.LiveLocationKalman.Status.valid) and liveLocationKalman.positionGeodetic.valid and liveLocationKalman.gpsOK + v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS v_cruise_diff = v_cruise_cluster - v_cruise v_ego_cluster = max(carState.vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego - targets = [] + # Pfeiferj's Map Turn Speed Controller + if frogpilot_toggles.map_turn_speed_controller and v_ego > CRUISING_SPEED and controlsState.enabled and gps_check: + mtsc_active = self.mtsc_target < v_cruise + self.mtsc_target = np.clip(self.mtsc.target_speed(v_ego, carState.aEgo), CRUISING_SPEED, v_cruise) + + if frogpilot_toggles.mtsc_curvature_check and self.road_curvature < 1.0 and not mtsc_active: + self.mtsc_target = v_cruise + else: + self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 + + targets = [self.mtsc_target] filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] return min(filtered_targets) @@ -175,6 +193,8 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk) frogpilotPlan.tFollow = float(self.t_follow) + frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.maxAcceleration = self.max_accel diff --git a/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py new file mode 100644 index 0000000..ea291d7 --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py @@ -0,0 +1,153 @@ +# PFEIFER - MTSC +import json +import math + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params + +params_memory = Params("/dev/shm/params") + +R = 6373000.0 # approximate radius of earth in meters +TO_RADIANS = math.pi / 180 +TO_DEGREES = 180 / math.pi +TARGET_JERK = -0.6 # m/s^3 should match up with the long planner +TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner +TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps + # reach the target velocity when innacuracies in the distance modeling logic would cause overshoot. + # The value is multiplied against the target velocity to determine the additional distance. This is + # done to keep the distance calculations consistent but results in the offset actually being less + # time than specified depending on how much of a speed diffrential there is between v_ego and the + # target velocity. + +def calculate_accel(t, target_jerk, a_ego): + return a_ego + target_jerk * t + +def calculate_velocity(t, target_jerk, a_ego, v_ego): + return v_ego + a_ego * t + target_jerk/2 * (t ** 2) + +def calculate_distance(t, target_jerk, a_ego, v_ego): + return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) + + +# points should be in radians +# output is meters +def distance_to_point(ax, ay, bx, by): + a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2) + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a)) + + return R * c # in meters + +class MapTurnSpeedController: + def __init__(self): + self.target_lat = 0.0 + self.target_lon = 0.0 + self.target_v = 0.0 + + def target_speed(self, v_ego, a_ego) -> float: + lat = 0.0 + lon = 0.0 + try: + position = json.loads(params_memory.get("LastGPSPosition")) + lat = position["latitude"] + lon = position["longitude"] + except: return 0.0 + + try: + target_velocities = json.loads(params_memory.get("MapTargetVelocities")) + except: return 0.0 + + min_dist = 1000 + min_idx = 0 + distances = [] + + # find our location in the path + for i in range(len(target_velocities)): + target_velocity = target_velocities[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS) + distances.append(d) + if d < min_dist: + min_dist = d + min_idx = i + + # only look at values from our current position forward + forward_points = target_velocities[min_idx:] + forward_distances = distances[min_idx:] + + # find velocities that we are within the distance we need to adjust for + valid_velocities = [] + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > v_ego: + continue + + d = forward_distances[i] + + a_diff = (a_ego - TARGET_ACCEL) + accel_t = abs(a_diff / TARGET_JERK) + min_accel_v = calculate_velocity(accel_t, TARGET_JERK, a_ego, v_ego) + + max_d = 0 + if tv > min_accel_v: + # calculate time needed based on target jerk + a = 0.5 * TARGET_JERK + b = a_ego + c = v_ego - tv + t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a + t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a + if not isinstance(t_a, complex) and t_a > 0: + t = t_a + else: + t = t_b + if isinstance(t, complex): + continue + + max_d = max_d + calculate_distance(t, TARGET_JERK, a_ego, v_ego) + + else: + t = accel_t + max_d = calculate_distance(t, TARGET_JERK, a_ego, v_ego) + + # calculate additional time needed based on target accel + t = abs((min_accel_v - tv) / TARGET_ACCEL) + max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v) + + if d < max_d + tv * TARGET_OFFSET: + valid_velocities.append((float(tv), tlat, tlon)) + + # Find the smallest velocity we need to adjust for + min_v = 100.0 + target_lat = 0.0 + target_lon = 0.0 + for tv, lat, lon in valid_velocities: + if tv < min_v: + min_v = tv + target_lat = lat + target_lon = lon + + if self.target_v < min_v and not (self.target_lat == 0 and self.target_lon == 0): + for i in range(len(forward_points)): + target_velocity = forward_points[i] + tlat = target_velocity["latitude"] + tlon = target_velocity["longitude"] + tv = target_velocity["velocity"] + if tv > v_ego: + continue + + if tlat == self.target_lat and tlon == self.target_lon and tv == self.target_v: + return float(self.target_v) + # not found so lets reset + self.target_v = 0.0 + self.target_lat = 0.0 + self.target_lon = 0.0 + + self.target_v = min_v + self.target_lat = target_lat + self.target_lon = target_lon + + return min_v diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 7242759..387e030 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -383,7 +383,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–"; QString speedStr = QString::number(std::nearbyint(speed)); - QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed)) : "–"; + QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–"; // Draw outer box + border to contain set speed and speed limit const int sign_margin = 12; @@ -402,7 +402,17 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { int bottom_radius = has_eu_speed_limit ? 100 : 32; QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); - if (trafficModeActive) { + if (is_cruise_set && cruiseAdjustment != 0) { + float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f); + QColor min = whiteColor(75); + QColor max = greenColor(); + + p.setPen(QPen(QColor::fromRgbF( + min.redF() + transition * (max.redF() - min.redF()), + min.greenF() + transition * (max.greenF() - min.greenF()), + min.blueF() + transition * (max.blueF() - min.blueF()) + ), 10)); + } else if (trafficModeActive) { p.setPen(QPen(redColor(), 10)); } else { p.setPen(QPen(whiteColor(75), 6)); @@ -793,6 +803,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { conditionalStatus = scene.conditional_status; showConditionalExperimentalStatusBar = scene.show_cem_status_bar; + bool disableSmoothing = scene.disable_smoothing_mtsc; + cruiseAdjustment = disableSmoothing || !is_cruise_set ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + experimentalMode = scene.experimental_mode; mapOpen = scene.map_open; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index a82df79..efa8423 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -155,6 +155,7 @@ private: bool trafficModeActive; float accelerationConversion; + float cruiseAdjustment; float distanceConversion; float speedConversion; @@ -165,6 +166,8 @@ private: QString leadDistanceUnit; QString leadSpeedUnit; + inline QColor greenColor(int alpha = 242) { return QColor(23, 134, 68, alpha); } + protected: void paintGL() override; void initializeGL() override; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 7b82817..c9ca0d9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -231,6 +231,7 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -270,6 +271,8 @@ void ui_update_frogpilot_params(UIState *s) { scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0; scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar"); + scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); + bool driving_personalities = scene.longitudinal_control && params.getBool("DrivingPersonalities"); scene.onroad_distance_button = driving_personalities && params.getBool("OnroadDistanceButton"); scene.use_kaofui_icons = scene.onroad_distance_button && params.getBool("KaofuiIcons"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 77f65e3..ee5b80a 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -188,6 +188,7 @@ typedef struct UIScene { // FrogPilot variables bool always_on_lateral_active; bool conditional_experimental; + bool disable_smoothing_mtsc; bool enabled; bool experimental_mode; bool experimental_mode_via_screen; @@ -203,6 +204,8 @@ typedef struct UIScene { bool traffic_mode_active; bool use_kaofui_icons; + float adjusted_cruise; + int alert_size; int conditional_speed; int conditional_speed_lead;