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

Speed limit controller

Browse Source 
Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself.

Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
Co-Authored-By: Efini <19368997+efini@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 6fa4b545a8
commit 22bfc8d9b7
16 changed files with 440 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
cereal/custom.capnp
View File

@@ -37,6 +37,10 @@ struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 {
redLight @4 :Bool;
safeObstacleDistance @5 :Int16;
safeObstacleDistanceStock @6 :Int16;
slcOverridden @7 :Bool;
slcOverriddenSpeed @8 :Float32;
slcSpeedLimit @9 :Float32;
slcSpeedLimitOffset @10 :Float32;
stoppedEquivalenceFactor @11 :Int16;
}

16
common/params.cc
View File

@@ -229,6 +229,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CameraView", PERSISTENT},
{"CarMake", PERSISTENT},
{"CarModel", PERSISTENT},
{"CarSpeedLimit", PERSISTENT},
{"CECurves", PERSISTENT},
{"CECurvesLead", PERSISTENT},
{"CENavigation", PERSISTENT},
@@ -273,6 +274,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"MapboxPublicKey", PERSISTENT},
{"MapboxSecretKey", PERSISTENT},
{"MapsSelected", PERSISTENT},
{"MapSpeedLimit", PERSISTENT},
{"MapSpeedLimitControl", PERSISTENT},
{"MapTargetVelocities", PERSISTENT},
{"Model", PERSISTENT},
{"ModelList", PERSISTENT},
@@ -284,6 +287,8 @@ std::unordered_map<std::string, uint32_t> keys = {
{"MuteOverheated", PERSISTENT},
{"MuteSeatbelt", PERSISTENT},
{"NavEnable", PERSISTENT},
{"NavSpeedLimit", PERSISTENT},
{"NavSpeedLimitControl", PERSISTENT},
{"NNFF", PERSISTENT},
{"NNFFModelFuzzyMatch", PERSISTENT},
{"NNFFModelName", PERSISTENT},
@@ -291,6 +296,10 @@ std::unordered_map<std::string, uint32_t> keys = {
{"NudgelessLaneChange", PERSISTENT},
{"NumericalTemp", PERSISTENT},
{"OfflineMode", PERSISTENT},
{"Offset1", PERSISTENT},
{"Offset2", PERSISTENT},
{"Offset3", PERSISTENT},
{"Offset4", PERSISTENT},
{"OneLaneChange", PERSISTENT},
{"OSMDownloadLocations", PERSISTENT},
{"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
@@ -298,6 +307,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"PathWidth", PERSISTENT},
{"PauseLateralOnSignal", PERSISTENT},
{"PreferredSchedule", PERSISTENT},
{"PreviousSpeedLimit", PERSISTENT},
{"RelaxedFollow", PERSISTENT},
{"RelaxedJerk", PERSISTENT},
{"ReverseCruise", PERSISTENT},
@@ -312,12 +322,18 @@ std::unordered_map<std::string, uint32_t> keys = {
{"ShowFPS", PERSISTENT},
{"ShowGPU", PERSISTENT},
{"ShowMemoryUsage", PERSISTENT},
{"ShowSLCOffset", PERSISTENT},
{"ShowStorageLeft", PERSISTENT},
{"ShowStorageUsed", PERSISTENT},
{"Sidebar", PERSISTENT},
{"SilentMode", PERSISTENT},
{"SLCExperimentalMode", PERSISTENT},
{"SLCFallback", PERSISTENT},
{"SLCOverride", PERSISTENT},
{"SLCPriority", PERSISTENT},
{"SmoothBraking", PERSISTENT},
{"SNGHack", PERSISTENT},
{"SpeedLimitController", PERSISTENT},
{"StandardFollow", PERSISTENT},
{"StandardJerk", PERSISTENT},
{"StoppingDistance", PERSISTENT},

1
release/files_common
View File

@@ -561,3 +561,4 @@ selfdrive/frogpilot/functions/conditional_experimental_mode.py
selfdrive/frogpilot/functions/frogpilot_planner.py
selfdrive/frogpilot/functions/map_turn_speed_controller.py
selfdrive/frogpilot/functions/model_switcher.py
selfdrive/frogpilot/functions/speed_limit_controller.py

32
selfdrive/car/toyota/carstate.py
View File

@@ -10,6 +10,7 @@ from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
SteerControlType = car.CarParams.SteerControlType
@@ -46,6 +47,8 @@ class CarState(CarStateBase):
# FrogPilot variables
self.traffic_signals = {}
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
@@ -178,8 +181,32 @@ class CarState(CarStateBase):
self.param.put_bool("ExperimentalMode", not experimental_mode)
self.lkas_previously_pressed = lkas_pressed
# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team!
self.update_traffic_signals(cp_cam)
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit()
SpeedLimitController.write_car_state()
return ret
def update_traffic_signals(self, cp_cam):
signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"]
new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals}
if new_values != self.traffic_signals:
self.traffic_signals.update(new_values)
def calculate_speed_limit(self):
tsgn1 = self.traffic_signals.get("TSGN1", None)
spdval1 = self.traffic_signals.get("SPDVAL1", None)
if tsgn1 == 1:
return spdval1 * CV.KPH_TO_MS
elif tsgn1 == 36:
return spdval1 * CV.MPH_TO_MS
else:
return 0
@staticmethod
def get_can_parser(CP):
messages = [
@@ -231,6 +258,11 @@ class CarState(CarStateBase):
def get_cam_can_parser(CP):
messages = []
messages += [
("RSA1", 0),
("RSA2", 0),
]
if CP.carFingerprint != CAR.PRIUS_V:
messages += [
("LKAS_HUD", 1),

2
selfdrive/controls/controlsd.py
View File

@@ -950,7 +950,7 @@ class Controls:
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
if self.CP.openpilotLongitudinalControl and not self.conditional_experimental_mode:
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.params_memory.get_bool("SLCExperimentalMode")
if self.CP.notCar:
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
time.sleep(0.1)

5
selfdrive/frogpilot/functions/conditional_experimental_mode.py
View File

@@ -99,6 +99,11 @@ class ConditionalExperimentalMode:
self.status_value = 5
return True
# Speed Limit Controller check
if self.params_memory.get_bool("SLCExperimentalMode"):
self.status_value = 6
return True
# Speed check
if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead):
self.status_value = 7 if self.lead_detected else 8

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

@@ -10,6 +10,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode
from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
# Acceleration profiles - Credit goes to the DragonPilot team!
# MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123]
@@ -54,7 +55,11 @@ class FrogPilotPlanner:
self.cem = ConditionalExperimentalMode()
self.mtsc = MapTurnSpeedController()
self.override_slc = False
self.overridden_speed = 0
self.mtsc_target = 0
self.slc_target = 0
self.v_cruise = 0
self.x_desired_trajectory = np.zeros(CONTROL_N)
@@ -99,8 +104,39 @@ class FrogPilotPlanner:
else:
self.mtsc_target = v_cruise
# Pfeiferj's Speed Limit Controller
if self.speed_limit_controller:
SpeedLimitController.update_current_max_velocity(v_cruise)
self.slc_target = SpeedLimitController.desired_speed_limit
# Override SLC upon gas pedal press and reset upon brake/cancel button
if self.speed_limit_controller_override:
self.override_slc |= carState.gasPressed
self.override_slc &= enabled
self.override_slc &= v_ego > self.slc_target
else:
self.override_slc = False
self.overridden_speed *= enabled
# Use the override speed if SLC is being overridden
if self.override_slc:
if self.speed_limit_controller_override == 1:
# Set the max speed to the manual set speed
if carState.gasPressed:
self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise)
self.slc_target = self.overridden_speed
elif self.speed_limit_controller_override == 2:
self.overridden_speed = v_cruise
self.slc_target = v_cruise
if self.slc_target == 0:
self.slc_target = v_cruise
else:
self.overriden_speed = 0
self.slc_target = v_cruise
v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
return min(v_cruise, self.mtsc_target) - v_ego_diff
return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff
def publish_lateral(self, sm, pm, DH):
frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan')
@@ -127,6 +163,11 @@ class FrogPilotPlanner:
frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor
frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock
frogpilotLongitudinalPlan.slcOverridden = self.override_slc
frogpilotLongitudinalPlan.slcOverriddenSpeed = float(self.overridden_speed)
frogpilotLongitudinalPlan.slcSpeedLimit = float(self.slc_target)
frogpilotLongitudinalPlan.slcSpeedLimitOffset = float(SpeedLimitController.offset)
pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
def update_frogpilot_params(self, params):
@@ -163,3 +204,8 @@ class FrogPilotPlanner:
self.lane_change_delay = params.get_int("LaneChangeTime") if self.nudgeless else 0
self.lane_detection = params.get_bool("LaneDetection") if self.nudgeless else False
self.one_lane_change = params.get_bool("OneLaneChange") if self.nudgeless else False
self.speed_limit_controller = params.get_bool("SpeedLimitController")
if self.speed_limit_controller:
self.speed_limit_controller_override = params.get_int("SLCOverride")
SpeedLimitController.update_frogpilot_params()

119
selfdrive/frogpilot/functions/speed_limit_controller.py Normal file
View File

@@ -0,0 +1,119 @@
from openpilot.common.conversions import Conversions as CV
from openpilot.common.params import Params
import json
params = Params()
params_memory = Params("/dev/shm/params")
class SpeedLimitController:
car_speed_limit: float = 0 # m/s
lst_speed_limit: float = 0 # m/s
map_speed_limit: float = 0 # m/s
nav_speed_limit: float = 0 # m/s
def __init__(self) -> None:
self.update_frogpilot_params()
self.write_car_state()
self.write_map_state()
self.write_nav_state()
def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None:
if load_state:
self.load_state()
@property
def offset(self) -> float:
if self.speed_limit < 14:
return self.offset1
elif self.speed_limit < 24:
return self.offset2
elif self.speed_limit < 29:
return self.offset3
else:
return self.offset4
@property
def speed_limit(self) -> float:
params_memory.put_bool("SLCExperimentalMode", False)
limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit]
filtered_limits = [limit for limit in limits if limit > 0]
if self.highest and filtered_limits:
return max(filtered_limits)
if self.lowest and filtered_limits:
return min(filtered_limits)
priority_orders = [
["nav", "car", "map"],
["nav", "map", "car"],
["nav", "map"],
["nav", "car"],
["nav"],
["map", "car", "nav"],
["map", "nav", "car"],
["map", "nav"],
["map", "car"],
["map"],
["car", "nav", "map"],
["car", "map", "nav"],
["car", "map"],
["car", "nav"],
["car"]
]
if self.speed_limit_priority < len(priority_orders):
for source in priority_orders[self.speed_limit_priority]:
limit = getattr(self, f"{source}_speed_limit", 0)
if limit > 0:
self.prv_speed_limit = limit
return limit
if self.use_experimental_mode:
params_memory.put_bool("SLCExperimentalMode", True)
return 0
elif self.use_previous_limit:
if self.lst_speed_limit != self.prv_speed_limit:
params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100)
self.lst_speed_limit = self.prv_speed_limit
return self.prv_speed_limit
return 0
@property
def desired_speed_limit(self):
return self.speed_limit + self.offset if self.speed_limit else 0
def load_state(self):
self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit"))
self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit"))
self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit"))
def write_car_state(self):
params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit))
def write_map_state(self):
params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit))
def write_nav_state(self):
params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit))
def update_frogpilot_params(self):
conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS
self.offset1 = params.get_int("Offset1") * conversion
self.offset2 = params.get_int("Offset2") * conversion
self.offset3 = params.get_int("Offset3") * conversion
self.offset4 = params.get_int("Offset4") * conversion
self.speed_limit_priority = params.get_int("SLCPriority")
self.highest = self.speed_limit_priority == 15
self.lowest = self.speed_limit_priority == 16
slc_fallback = params.get_int("SLCFallback")
self.use_experimental_mode = slc_fallback == 1
self.use_previous_limit = slc_fallback == 2
self.prv_speed_limit = params.get_int("PreviousSpeedLimit") / 100
SpeedLimitController = SpeedLimitController()

138
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -41,6 +41,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
{"LaneDetection", "Lane Detection", "Block nudgeless lane changes when a lane isn't detected.", ""},
{"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""},
{"PauseLateralOnSignal", "Pause Lateral On Turn Signal", "Temporarily disable lateral control during turn signal use.", ""},
{"SpeedLimitController", "Speed Limit Controller", "Automatically adjust vehicle speed to match speed limits using 'Open Street Map's, 'Navigate On openpilot', or your car's dashboard (TSS2 Toyotas only).", "../assets/offroad/icon_speed_limit.png"},
{"Offset1", "Speed Limit Offset (0-34 mph)", "Speed limit offset for speed limits between 0-34 mph.", ""},
{"Offset2", "Speed Limit Offset (35-54 mph)", "Speed limit offset for speed limits between 35-54 mph.", ""},
{"Offset3", "Speed Limit Offset (55-64 mph)", "Speed limit offset for speed limits between 55-64 mph.", ""},
{"Offset4", "Speed Limit Offset (65-99 mph)", "Speed limit offset for speed limits between 65-99 mph.", ""},
{"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""},
{"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""},
{"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""},
};
for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -223,6 +232,91 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
}
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false);
} else if (param == "SpeedLimitController") {
FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
parentToggleClicked();
for (auto &[key, toggle] : toggles) {
toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end());
}
slscPriorityButton->setVisible(true);
});
toggle = speedLimitControllerToggle;
} else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") {
toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map<int, QString>(), this, false, " mph");
} else if (param == "SLCFallback") {
std::vector<QString> fallbackOptions{tr("None"), tr("Experimental Mode"), tr("Previous Limit")};
FrogPilotButtonParamControl *fallbackSelection = new FrogPilotButtonParamControl(param, title, desc, icon, fallbackOptions);
toggle = fallbackSelection;
} else if (param == "SLCOverride") {
std::vector<QString> overrideOptions{tr("None"), tr("Manual Set Speed"), tr("Max Set Speed")};
FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions);
toggle = overrideSelection;
} else if (param == "SLCPriority") {
const QStringList priorities {
"Navigation, Dashboard, Offline Maps",
"Navigation, Offline Maps, Dashboard",
"Navigation, Offline Maps",
"Navigation, Dashboard",
"Navigation",
"Offline Maps, Dashboard, Navigation",
"Offline Maps, Navigation, Dashboard",
"Offline Maps, Navigation",
"Offline Maps, Dashboard",
"Offline Maps",
"Dashboard, Navigation, Offline Maps",
"Dashboard, Offline Maps, Navigation",
"Dashboard, Offline Maps",
"Dashboard, Navigation",
"Dashboard",
"Highest",
"Lowest",
"",
};
slscPriorityButton = new ButtonControl(title, tr("SELECT"), desc);
QObject::connect(slscPriorityButton, &ButtonControl::clicked, this, [this, priorities]() {
QStringList availablePriorities = {"Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest", "None"};
QStringList selectedPriorities;
int priorityValue = -1;
QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")};
for (int i = 0; i < 3; ++i) {
QString selection = MultiOptionDialog::getSelection(priorityPrompts[i], availablePriorities, "", this);
if (selection.isEmpty()) break;
if (selection == "None") {
priorityValue = 17;
break;
} else if (selection == "Highest") {
priorityValue = 15;
break;
} else if (selection == "Lowest") {
priorityValue = 16;
break;
} else {
selectedPriorities.append(selection);
availablePriorities.removeAll(selection);
availablePriorities.removeAll("Highest");
availablePriorities.removeAll("Lowest");
}
}
if (priorityValue == -1 && !selectedPriorities.isEmpty()) {
QString priorityString = selectedPriorities.join(", ");
priorityValue = priorities.indexOf(priorityString);
}
if (priorityValue != -1) {
slscPriorityButton->setValue(priorities[priorityValue]);
params.putInt("SLCPriority", priorityValue);
updateToggles();
}
});
slscPriorityButton->setValue(priorities[params.getInt("SLCPriority")]);
addItem(slscPriorityButton);
} else {
toggle = new ParamControl(param, title, desc, icon, this);
}
@@ -261,7 +355,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange", "PauseLateralOnSignal"};
lateralTuneKeys = {"AverageCurvature", "NNFF"};
longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
speedLimitControllerKeys = {};
speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"};
visionTurnControlKeys = {};
QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles);
@@ -289,17 +383,57 @@ void FrogPilotControlsPanel::updateMetric() {
double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE;
params.putInt("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion));
params.putInt("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion));
params.putInt("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion));
params.putInt("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion));
params.putInt("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion));
params.putInt("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion));
params.putInt("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion));
}
FrogPilotParamValueControl *offset1Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset1"]);
FrogPilotParamValueControl *offset2Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset2"]);
FrogPilotParamValueControl *offset3Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset3"]);
FrogPilotParamValueControl *offset4Toggle = static_cast<FrogPilotParamValueControl*>(toggles["Offset4"]);
FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(toggles["StoppingDistance"]);
if (isMetric) {
offset1Toggle->setTitle("Speed Limit Offset (0-34 kph)");
offset2Toggle->setTitle("Speed Limit Offset (35-54 kph)");
offset3Toggle->setTitle("Speed Limit Offset (55-64 kph)");
offset4Toggle->setTitle("Speed Limit Offset (65-99 kph)");
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 kph.");
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 kph.");
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 kph.");
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 kph.");
offset1Toggle->updateControl(0, 99, " kph");
offset2Toggle->updateControl(0, 99, " kph");
offset3Toggle->updateControl(0, 99, " kph");
offset4Toggle->updateControl(0, 99, " kph");
stoppingDistanceToggle->updateControl(0, 5, " meters");
} else {
offset1Toggle->setTitle("Speed Limit Offset (0-34 mph)");
offset2Toggle->setTitle("Speed Limit Offset (35-54 mph)");
offset3Toggle->setTitle("Speed Limit Offset (55-64 mph)");
offset4Toggle->setTitle("Speed Limit Offset (65-99 mph)");
offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 mph.");
offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 mph.");
offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 mph.");
offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 mph.");
offset1Toggle->updateControl(0, 99, " mph");
offset2Toggle->updateControl(0, 99, " mph");
offset3Toggle->updateControl(0, 99, " mph");
offset4Toggle->updateControl(0, 99, " mph");
stoppingDistanceToggle->updateControl(0, 10, " feet");
}
offset1Toggle->refresh();
offset2Toggle->refresh();
offset3Toggle->refresh();
offset4Toggle->refresh();
stoppingDistanceToggle->refresh();
previousIsMetric = isMetric;
@@ -310,6 +444,7 @@ void FrogPilotControlsPanel::parentToggleClicked() {
conditionalSpeedsImperial->setVisible(false);
conditionalSpeedsMetric->setVisible(false);
modelSelectorButton->setVisible(false);
slscPriorityButton->setVisible(false);
standardProfile->setVisible(false);
relaxedProfile->setVisible(false);
@@ -321,6 +456,7 @@ void FrogPilotControlsPanel::hideSubToggles() {
conditionalSpeedsImperial->setVisible(false);
conditionalSpeedsMetric->setVisible(false);
modelSelectorButton->setVisible(true);
slscPriorityButton->setVisible(false);
standardProfile->setVisible(false);
relaxedProfile->setVisible(false);

2
selfdrive/frogpilot/ui/control_settings.h
View File

@@ -22,6 +22,8 @@ private:
void updateMetric();
void updateToggles();
ButtonControl *slscPriorityButton;
FrogPilotButtonIconControl *modelSelectorButton;
FrogPilotDualParamControl *aggressiveProfile;

2
selfdrive/manager/process_config.py
View File

@@ -56,7 +56,7 @@ def enable_logging(started, params, CP: car.CarParams) -> bool:
return not (params.get_bool("FireTheBabysitter") and params.get_bool("NoLogging"))
def osm(started, params, CP: car.CarParams) -> bool:
return params.get_bool("RoadNameUI")
return params.get_bool("RoadNameUI") or params.get_bool("SpeedLimitController")
procs = [
DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"),

18
selfdrive/navd/navd.py
View File

@@ -18,6 +18,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param,
parse_banner_instructions)
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
REROUTE_DISTANCE = 25
MANEUVER_TRANSITION_THRESHOLD = 10
REROUTE_COUNTER_MIN = 3
@@ -230,6 +232,12 @@ class RouteEngine:
if self.step_idx is None:
msg.valid = False
SpeedLimitController.load_state()
SpeedLimitController.nav_speed_limit = 0
SpeedLimitController.write_nav_state()
if SpeedLimitController.desired_speed_limit != 0:
msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
self.pm.send('navInstruction', msg)
return
@@ -304,6 +312,16 @@ class RouteEngine:
if ('maxspeed' in closest.annotations) and self.localizer_valid:
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
SpeedLimitController.load_state()
SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed']
SpeedLimitController.write_nav_state()
if not self.localizer_valid or ('maxspeed' not in closest.annotations):
SpeedLimitController.load_state()
SpeedLimitController.nav_speed_limit = 0
SpeedLimitController.write_nav_state()
if SpeedLimitController.desired_speed_limit != 0:
msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit
# Speed limit sign type
if 'speedLimitSign' in step:

49
selfdrive/ui/qt/onroad.cc
View File

@@ -129,16 +129,23 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
QRect speedRect(rect().center().x() - 175, 50, 350, 350);
bool isSpeedClicked = speedRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked) {
// Speed limit offset button
const QRect speedLimitRect(7, 250, 225, 225);
const bool isSpeedLimitClicked = speedLimitRect.contains(e->pos());
if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) {
// Check if the click was within the max speed area
if (isMaxSpeedClicked) {
reverseCruise = !params.getBool("ReverseCruise");
params.putBoolNonBlocking("ReverseCruise", reverseCruise);
paramsMemory.putBoolNonBlocking("FrogPilotTogglesUpdated", true);
// Check if the click was within the speed text area
} else {
} else if (isSpeedClicked) {
speedHidden = !params.getBool("HideSpeed");
params.putBoolNonBlocking("HideSpeed", speedHidden);
} else {
showSLCOffset = !params.getBool("ShowSLCOffset");
params.putBoolNonBlocking("ShowSLCOffset", showSLCOffset);
}
widgetClicked = true;
// If the click wasn't for anything specific, change the value of "ExperimentalMode"
@@ -490,10 +497,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit = slcOverridden ? slcOverriddenSpeed : slcSpeedLimit ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
if (slcSpeedLimit && !slcOverridden) {
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
}
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || slcSpeedLimit;
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
is_metric = s.scene.is_metric;
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
@@ -527,6 +537,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg);
QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
QString speedLimitOffsetStr = (showSLCOffset) ? "+" + QString::number(std::nearbyint(slcSpeedLimitOffset)) : "–";
QString speedStr = QString::number(std::nearbyint(speed));
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
@@ -599,11 +610,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.setPen(QPen(blackColor(), 6));
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.save();
p.setOpacity(slcOverridden ? 0.25 : 1.0);
if (showSLCOffset) {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
p.setFont(InterFont(50, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
} else {
p.setFont(InterFont(28, QFont::DemiBold));
p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
p.setFont(InterFont(70, QFont::Bold));
p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
}
p.restore();
}
// EU (Vienna style) sign
@@ -1106,6 +1129,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() {
reverseCruise = true;
}
if (params.getBool("ShowSLCOffset")) {
showSLCOffset = true;
}
// Custom themes configuration
themeConfiguration = {
{1, {QString("frog_theme"), {QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))},
@@ -1160,6 +1187,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) {
obstacleDistanceStock = scene.obstacle_distance_stock;
roadNameUI = scene.road_name_ui;
showDriverCamera = scene.show_driver_camera;
slcOverridden = scene.speed_limit_overridden;
slcOverriddenSpeed = scene.speed_limit_overridden_speed;
slcSpeedLimit = scene.speed_limit;
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
stoppedEquivalence = scene.stopped_equivalence;
turnSignalLeft = scene.turn_signal_left;
turnSignalRight = scene.turn_signal_right;

5
selfdrive/ui/qt/onroad.h
View File

@@ -18,6 +18,7 @@ const int img_size = (btn_size / 4) * 3;
// FrogPilot global variables
static bool reverseCruise;
static bool showSLCOffset;
static bool speedHidden;
static double fps;
@@ -169,6 +170,7 @@ private:
bool muteDM;
bool roadNameUI;
bool showDriverCamera;
bool slcOverridden;
bool turnSignalLeft;
bool turnSignalRight;
bool useSI;
@@ -176,6 +178,9 @@ private:
float cruiseAdjustment;
float laneWidthLeft;
float laneWidthRight;
float slcOverriddenSpeed;
float slcSpeedLimit;
float slcSpeedLimitOffset;
int bearingDeg;
int cameraView;
int conditionalSpeed;

7
selfdrive/ui/ui.cc
View File

@@ -255,6 +255,12 @@ static void update_state(UIState *s) {
scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock();
scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor();
}
if (scene.speed_limit_controller) {
scene.speed_limit = frogpilotLongitudinalPlan.getSlcSpeedLimit();
scene.speed_limit_offset = frogpilotLongitudinalPlan.getSlcSpeedLimitOffset();
scene.speed_limit_overridden = frogpilotLongitudinalPlan.getSlcOverridden();
scene.speed_limit_overridden_speed = frogpilotLongitudinalPlan.getSlcOverriddenSpeed();
}
scene.adjusted_cruise = frogpilotLongitudinalPlan.getAdjustedCruise();
}
if (sm.updated("liveLocationKalman")) {
@@ -321,6 +327,7 @@ void ui_update_params(UIState *s) {
scene.mute_dm = params.getBool("FireTheBabysitter") && params.getBool("MuteDM");
scene.rotating_wheel = params.getBool("RotatingWheel");
scene.screen_brightness = params.getInt("ScreenBrightness");
scene.speed_limit_controller = params.getBool("SpeedLimitController");
scene.wheel_icon = params.getInt("WheelIcon");
}

5
selfdrive/ui/ui.h
View File

@@ -193,6 +193,8 @@ typedef struct UIScene {
bool rotating_wheel;
bool show_driver_camera;
bool show_fps;
bool speed_limit_controller;
bool speed_limit_overridden;
bool tethering_enabled;
bool turn_signal_left;
bool turn_signal_right;
@@ -205,6 +207,9 @@ typedef struct UIScene {
float path_edge_width;
float path_width;
float road_edge_width;
float speed_limit;
float speed_limit_offset;
float speed_limit_overridden_speed;
int bearing_deg;
int camera_view;
int conditional_speed;