diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8857278..3687d1b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -23,6 +23,7 @@ struct FrogPilotNavigation @0xf35cc4560bbf6ec2 { } struct FrogPilotPlan @0xda96579883444c35 { + adjustedCruise @0 :Float64; conditionalExperimental @1 :Bool; desiredFollowDistance @2 :Int16; laneWidthLeft @3 :Float32; diff --git a/common/params.cc b/common/params.cc index c004f92..89ea211 100644 --- a/common/params.cc +++ b/common/params.cc @@ -251,6 +251,7 @@ std::unordered_map keys = { {"CydiaTune", PERSISTENT}, {"DecelerationProfile", PERSISTENT}, {"DeviceShutdown", PERSISTENT}, + {"DisableMTSCSmoothing", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DisableOpenpilotLongitudinal", PERSISTENT}, {"DisengageVolume", PERSISTENT}, @@ -288,7 +289,13 @@ std::unordered_map keys = { {"LoudBlindspotAlert", PERSISTENT}, {"LowerVolt", PERSISTENT}, {"ManualUpdateInitiated", CLEAR_ON_MANAGER_START}, + {"MapTargetLatA", PERSISTENT}, + {"MapTargetVelocities", PERSISTENT}, {"ModelUI", PERSISTENT}, + {"MTSCAggressiveness", PERSISTENT}, + {"MTSCCurvatureCheck", PERSISTENT}, + {"MTSCEnabled", PERSISTENT}, + {"MTSCLimit", PERSISTENT}, {"MuteOverheated", PERSISTENT}, {"NoLogging", PERSISTENT}, {"NoUploads", PERSISTENT}, diff --git a/release/files_common b/release/files_common index 7f8c295..4654a9f 100644 --- a/release/files_common +++ b/release/files_common @@ -561,3 +561,4 @@ tinygrad_repo/tinygrad/*.py selfdrive/frogpilot/functions/conditional_experimental_mode.py selfdrive/frogpilot/functions/frogpilot_functions.py selfdrive/frogpilot/functions/frogpilot_planner.py +selfdrive/frogpilot/functions/map_turn_speed_controller.py diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_speed_map.png b/selfdrive/frogpilot/assets/toggle_icons/icon_speed_map.png new file mode 100644 index 0000000..60b87eb Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_speed_map.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 32a29f7..c5d34f9 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -1,6 +1,8 @@ import cereal.messaging as messaging +import numpy as np from openpilot.common.conversions import Conversions as CV +from openpilot.common.numpy_fast import clip 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 @@ -9,6 +11,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode +from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController + class FrogPilotPlanner: def __init__(self, CP, params, params_memory): @@ -18,7 +22,9 @@ class FrogPilotPlanner: self.fpf = FrogPilotFunctions() self.cem = ConditionalExperimentalMode(self.params_memory) + self.mtsc = MapTurnSpeedController() + self.mtsc_target = 0 self.road_curvature = 0 self.stop_distance = 0 self.v_cruise = 0 @@ -30,8 +36,13 @@ class FrogPilotPlanner: def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego): enabled = controlsState.enabled + # Use the stock deceleration profile to handle MTSC more precisely + v_cruise_changed = self.mtsc_target < v_cruise + # Configure the deceleration profile - if self.deceleration_profile == 1: + if v_cruise_changed: + min_accel = A_CRUISE_MIN + elif self.deceleration_profile == 1: min_accel = self.fpf.get_min_accel_eco(v_ego) elif self.deceleration_profile == 2: min_accel = self.fpf.get_min_accel_sport(v_ego) @@ -82,7 +93,20 @@ class FrogPilotPlanner: v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS v_cruise_diff = v_cruise_cluster - v_cruise - targets = [] + # Pfeiferj's Map Turn Speed Controller + if self.map_turn_speed_controller and v_ego > CRUISING_SPEED and enabled: + mtsc_active = self.mtsc_target < v_cruise + self.mtsc_target = np.clip(self.mtsc.target_speed(v_ego, carState.aEgo), CRUISING_SPEED, v_cruise) + + # MTSC failsafes + if self.mtsc_curvature_check and self.road_curvature < 1.0 and not mtsc_active: + self.mtsc_target = v_cruise + if v_ego - self.mtsc_limit >= self.mtsc_target: + self.mtsc_target = v_cruise + else: + self.mtsc_target = v_cruise + + targets = [self.mtsc_target] filtered_targets = [target for target in targets if target > CRUISING_SPEED] return min(filtered_targets) if filtered_targets else v_cruise @@ -92,6 +116,7 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan + frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor @@ -136,3 +161,9 @@ class FrogPilotPlanner: self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration") self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 + + self.map_turn_speed_controller = params.get_bool("MTSCEnabled") + if self.map_turn_speed_controller: + self.mtsc_curvature_check = params.get_bool("MTSCCurvatureCheck") + self.mtsc_limit = params.get_float("MTSCLimit") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) + self.params_memory.put_float("MapTargetLatA", 2 * (params.get_int("MTSCAggressiveness") / 100)) diff --git a/selfdrive/frogpilot/functions/map_turn_speed_controller.py b/selfdrive/frogpilot/functions/map_turn_speed_controller.py new file mode 100644 index 0000000..2307e1b --- /dev/null +++ b/selfdrive/frogpilot/functions/map_turn_speed_controller.py @@ -0,0 +1,152 @@ +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 There's some jounce limits that are not consistent so we're fudging this some +TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit +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_distance(t, target_jerk, a_ego, v_ego): + return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3) + +def calculate_velocity(t, target_jerk, a_ego, v_ego): + return v_ego + a_ego * t + target_jerk/2 * (t ** 2) + + +# 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/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 6fe3700..2a3bf87 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -33,6 +33,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""}, {"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""}, + {"MTSCEnabled", "Map Turn Speed Control", "Slow down for anticipated curves detected by your downloaded maps.", "../frogpilot/assets/toggle_icons/icon_speed_map.png"}, + {"DisableMTSCSmoothing", "Disable MTSC UI Smoothing", "Disables the smoothing for the requested speed in the onroad UI.", ""}, + {"MTSCCurvatureCheck", "Model Curvature Detection Failsafe", "Only trigger MTSC when the model detects a curve in the road. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""}, + {"MTSCLimit", "Speed Change Hard Cap", "Set a hard cap for MTSC. If MTSC requests a speed decrease greater than this value, it ignores the requested speed from MTSC. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""}, + {"MTSCAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.\n\nA change of +- 1% results in the velocity being raised or lowered by about 1 mph.", ""}, + {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"DisableOnroadUploads", "Disable Onroad Uploads", "Prevent large data uploads when onroad.", ""}, {"HigherBitrate", "Higher Bitrate Recording", "Increases the quality of the footage uploaded to comma connect.", ""}, @@ -213,6 +219,20 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } else if (param == "StoppingDistance") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map(), this, false, " feet"); + } else if (param == "MTSCEnabled") { + FrogPilotParamManageControl *mtscToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(mtscToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(mtscKeys.find(key.c_str()) != mtscKeys.end()); + } + }); + toggle = mtscToggle; + } else if (param == "MTSCAggressiveness") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map(), this, false, "%"); + } else if (param == "MTSCLimit") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map(), this, false, " mph"); + } else if (param == "QOLControls") { FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -309,17 +329,24 @@ void FrogPilotControlsPanel::updateMetric() { double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); + params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } + FrogPilotParamValueControl *mtscLimitToggle = static_cast(toggles["MTSCLimit"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { + mtscLimitToggle->updateControl(0, 99, " kph"); + stoppingDistanceToggle->updateControl(0, 5, " meters"); } else { + mtscLimitToggle->updateControl(0, 99, " mph"); + stoppingDistanceToggle->updateControl(0, 10, " feet"); } + mtscLimitToggle->refresh(); stoppingDistanceToggle->refresh(); previousIsMetric = isMetric; diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index b25e07e..d89bfde 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -41,7 +41,7 @@ private: std::set laneChangeKeys = {}; std::set lateralTuneKeys = {"ForceAutoTune"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"}; - std::set mtscKeys = {}; + std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "ReverseCruise"}; std::set speedLimitControllerKeys = {}; std::set speedLimitControllerControlsKeys = {}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index f30fc3b..65f57fe 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -475,7 +475,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)) : "–"; if (!showDriverCamera) { // Draw outer box + border to contain set speed and speed limit @@ -495,7 +495,16 @@ 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 (scene.reverse_cruise) { + if (is_cruise_set && cruiseAdjustment) { + float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f); + QColor min = whiteColor(75), max = greenColor(75); + + 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()) + ), 6)); + } else if (scene.reverse_cruise) { p.setPen(QPen(QColor(0, 150, 255), 6)); } else { p.setPen(QPen(whiteColor(75), 6)); @@ -1106,6 +1115,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { conditionalSpeedLead = scene.conditional_speed_lead; conditionalStatus = scene.conditional_status; + bool disableSmoothing = scene.vtsc_controlling_curve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; + cruiseAdjustment = disableSmoothing ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + customColors = scene.custom_colors; experimentalMode = scene.experimental_mode; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index f070d1d..c35eb04 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -136,6 +136,7 @@ private: bool turnSignalLeft; bool turnSignalRight; + float cruiseAdjustment; float distanceConversion; float laneWidthLeft; float laneWidthRight; @@ -166,6 +167,8 @@ private: QTimer *animationTimer; + 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 41d3632..31705d4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -255,6 +255,7 @@ static void update_state(UIState *s) { scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); } + scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -307,6 +308,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.custom_signals = custom_theme ? params.getInt("CustomSignals") : 0; scene.holiday_themes = custom_theme && params.getBool("HolidayThemes"); + scene.disable_smoothing_mtsc = params.getBool("DisableMTSCSmoothing"); scene.driver_camera = params.getBool("DriverCamera"); scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 5b82005..2ce5f2d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -178,6 +178,7 @@ typedef struct UIScene { bool blind_spot_path; bool blind_spot_right; bool conditional_experimental; + bool disable_smoothing_mtsc; bool driver_camera; bool dynamic_path_width; bool enabled; @@ -199,6 +200,7 @@ typedef struct UIScene { bool unlimited_road_ui_length; bool use_si; + float adjusted_cruise; float lane_line_width; float lane_width_left; float lane_width_right;