Controls - Speed Limit Controller
Automatically adjust the max speed to match the current speed limit using 'Open Street Maps', 'Navigate On openpilot', or your car's dashboard (Toyotas/Lexus/HKG only). Credit goes to Pfeiferj! https: //github.com/pfeiferj Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
parent
b262df3088
commit
e47c6ae852
|
|
@ -877,7 +877,7 @@ class Controls:
|
|||
while not evt.is_set():
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode:
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode")
|
||||
self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.frogpilot_toggles.speed_limit_controller and SpeedLimitController.experimental_mode
|
||||
self.personality = self.read_personality_param()
|
||||
if self.CP.notCar:
|
||||
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im
|
|||
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
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
|
|
@ -60,6 +61,7 @@ class FrogPilotPlanner:
|
|||
self.acceleration_jerk = 0
|
||||
self.mtsc_target = 0
|
||||
self.road_curvature = 0
|
||||
self.slc_target = 0
|
||||
self.speed_jerk = 0
|
||||
|
||||
def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles):
|
||||
|
|
@ -197,7 +199,14 @@ class FrogPilotPlanner:
|
|||
else:
|
||||
self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
|
||||
|
||||
targets = [self.mtsc_target]
|
||||
# Pfeiferj's Speed Limit Controller
|
||||
if frogpilot_toggles.speed_limit_controller:
|
||||
SpeedLimitController.update(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles)
|
||||
self.slc_target = SpeedLimitController.desired_speed_limit
|
||||
else:
|
||||
self.slc_target = v_cruise + v_ego_diff if v_cruise != V_CRUISE_UNSET else 0
|
||||
|
||||
targets = [self.mtsc_target, self.slc_target - v_ego_diff]
|
||||
filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
|
||||
|
||||
return min(filtered_targets)
|
||||
|
|
@ -222,6 +231,9 @@ class FrogPilotPlanner:
|
|||
frogpilotPlan.maxAcceleration = self.max_accel
|
||||
frogpilotPlan.minAcceleration = self.min_accel
|
||||
|
||||
frogpilotPlan.slcSpeedLimit = self.slc_target
|
||||
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
|
||||
|
||||
frogpilotPlan.tFollow = float(self.t_follow)
|
||||
|
||||
frogpilotPlan.vCruise = float(self.v_cruise)
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
|
|||
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
|
||||
|
||||
class ConditionalExperimentalMode:
|
||||
def __init__(self):
|
||||
|
|
@ -46,6 +47,10 @@ class ConditionalExperimentalMode:
|
|||
self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8
|
||||
return True
|
||||
|
||||
if SpeedLimitController.experimental_mode:
|
||||
self.status_value = 9
|
||||
return True
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,121 @@
|
|||
# PFEIFER - SLC - Modified by FrogAi for FrogPilot
|
||||
import json
|
||||
import math
|
||||
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
|
||||
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
|
||||
|
||||
R = 6373000.0 # approximate radius of earth in meters
|
||||
TO_RADIANS = math.pi / 180
|
||||
|
||||
def distance_to_point(ax, ay, bx, by):
|
||||
a = math.sin((bx - ax) / 2)**2 + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2)**2
|
||||
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||
return R * c # in meters
|
||||
|
||||
class SpeedLimitController:
|
||||
def __init__(self):
|
||||
self.frogpilot_toggles = FrogPilotVariables.toggles
|
||||
|
||||
self.params = Params()
|
||||
self.params_memory = Params("/dev/shm/params")
|
||||
|
||||
self.map_speed_limit = 0 # m/s
|
||||
self.nav_speed_limit = 0 # m/s
|
||||
self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit")
|
||||
|
||||
def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles):
|
||||
self.write_map_state(v_ego)
|
||||
self.nav_speed_limit = navigationSpeedLimit
|
||||
|
||||
self.frogpilot_toggles = frogpilot_toggles
|
||||
|
||||
def get_param_memory(self, key, is_json=False, default=None):
|
||||
try:
|
||||
data = self.params_memory.get(key)
|
||||
if data is None:
|
||||
return default
|
||||
return json.loads(data) if is_json else float(data.decode('utf-8'))
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
def write_map_state(self, v_ego):
|
||||
self.map_speed_limit = self.get_param_memory("MapSpeedLimit")
|
||||
|
||||
next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True, default={})
|
||||
next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0)
|
||||
next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0)
|
||||
next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0)
|
||||
|
||||
position = self.get_param_memory("LastGPSPosition", is_json=True, default={})
|
||||
lat = position.get("latitude", 0)
|
||||
lon = position.get("longitude", 0)
|
||||
|
||||
if next_map_speed_limit_value > 1:
|
||||
d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS)
|
||||
|
||||
if self.prv_speed_limit < next_map_speed_limit_value:
|
||||
max_d = v_ego * self.frogpilot_toggles.map_speed_lookahead_higher
|
||||
else:
|
||||
max_d = v_ego * self.frogpilot_toggles.map_speed_lookahead_lower
|
||||
|
||||
if d < max_d:
|
||||
self.map_speed_limit = next_map_speed_limit_value
|
||||
|
||||
@property
|
||||
def speed_limit(self):
|
||||
limits = [self.map_speed_limit, self.nav_speed_limit]
|
||||
filtered_limits = [limit for limit in limits if limit is not None and limit > 1]
|
||||
|
||||
if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits:
|
||||
return float(max(filtered_limits))
|
||||
if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits:
|
||||
return float(min(filtered_limits))
|
||||
|
||||
speed_limits = {
|
||||
"Offline Maps": self.map_speed_limit,
|
||||
"Navigation": self.nav_speed_limit,
|
||||
}
|
||||
|
||||
for priority in [
|
||||
self.frogpilot_toggles.speed_limit_priority1,
|
||||
self.frogpilot_toggles.speed_limit_priority2,
|
||||
self.frogpilot_toggles.speed_limit_priority3,
|
||||
]:
|
||||
if speed_limits.get(priority, 0) in filtered_limits:
|
||||
return float(speed_limits[priority])
|
||||
|
||||
if self.frogpilot_toggles.use_previous_limit:
|
||||
return float(self.prv_speed_limit)
|
||||
|
||||
return 0
|
||||
|
||||
@property
|
||||
def offset(self):
|
||||
if self.speed_limit < 13.5:
|
||||
return self.frogpilot_toggles.offset1
|
||||
if self.speed_limit < 24:
|
||||
return self.frogpilot_toggles.offset2
|
||||
if self.speed_limit < 29:
|
||||
return self.frogpilot_toggles.offset3
|
||||
return self.frogpilot_toggles.offset4
|
||||
|
||||
@property
|
||||
def desired_speed_limit(self):
|
||||
if self.speed_limit > 1:
|
||||
self.update_previous_limit(self.speed_limit)
|
||||
return self.speed_limit + self.offset
|
||||
return 0
|
||||
|
||||
def update_previous_limit(self, speed_limit):
|
||||
if self.prv_speed_limit != speed_limit:
|
||||
self.params.put_float("PreviousSpeedLimit", speed_limit)
|
||||
self.prv_speed_limit = speed_limit
|
||||
|
||||
@property
|
||||
def experimental_mode(self):
|
||||
return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode
|
||||
|
||||
SpeedLimitController = SpeedLimitController()
|
||||
|
|
@ -73,6 +73,8 @@ class RouteEngine:
|
|||
self.approaching_intersection = False
|
||||
self.approaching_turn = False
|
||||
|
||||
self.nav_speed_limit = 0
|
||||
|
||||
def update(self):
|
||||
self.sm.update(0)
|
||||
|
||||
|
|
@ -272,6 +274,7 @@ class RouteEngine:
|
|||
|
||||
if self.step_idx is None:
|
||||
msg.valid = False
|
||||
self.nav_speed_limit = 0
|
||||
self.pm.send('navInstruction', msg)
|
||||
return
|
||||
|
||||
|
|
@ -346,6 +349,9 @@ class RouteEngine:
|
|||
|
||||
if ('maxspeed' in closest.annotations) and self.localizer_valid:
|
||||
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
||||
self.nav_speed_limit = closest.annotations['maxspeed']
|
||||
if not self.localizer_valid or ('maxspeed' not in closest.annotations):
|
||||
self.nav_speed_limit = 0
|
||||
|
||||
# Speed limit sign type
|
||||
if 'speedLimitSign' in step:
|
||||
|
|
@ -401,6 +407,7 @@ class RouteEngine:
|
|||
|
||||
frogpilotNavigation.approachingIntersection = self.approaching_intersection
|
||||
frogpilotNavigation.approachingTurn = self.approaching_turn
|
||||
frogpilotNavigation.navigationSpeedLimit = self.nav_speed_limit
|
||||
|
||||
self.pm.send('frogpilotNavigation', frogpilot_plan_send)
|
||||
|
||||
|
|
|
|||
|
|
@ -54,11 +54,14 @@ 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 = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
|
||||
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
if (speedLimitController) {
|
||||
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
|
||||
}
|
||||
|
||||
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD);
|
||||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA);
|
||||
has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign);
|
||||
has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) && !(speedLimitController && !useViennaSLCSign) || (speedLimitController && useViennaSLCSign);
|
||||
is_metric = s.scene.is_metric;
|
||||
speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph");
|
||||
hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE);
|
||||
|
|
@ -94,6 +97,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 = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
|
||||
QString speedStr = QString::number(std::nearbyint(speed));
|
||||
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";
|
||||
|
||||
|
|
@ -169,11 +173,20 @@ 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);
|
||||
if (speedLimitController && 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);
|
||||
}
|
||||
}
|
||||
|
||||
// EU (Vienna style) sign
|
||||
|
|
@ -184,9 +197,16 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
|
|||
p.setPen(QPen(Qt::red, 20));
|
||||
p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
|
||||
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.setPen(blackColor());
|
||||
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||||
if (showSLCOffset) {
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr);
|
||||
p.setFont(InterFont(40, QFont::DemiBold));
|
||||
p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr);
|
||||
} else {
|
||||
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
|
||||
p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr);
|
||||
}
|
||||
}
|
||||
|
||||
// current speed
|
||||
|
|
@ -539,6 +559,11 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(const UIScene &scene) {
|
|||
|
||||
reverseCruise = scene.reverse_cruise;
|
||||
|
||||
speedLimitController = scene.speed_limit_controller;
|
||||
showSLCOffset = speedLimitController && scene.show_slc_offset;
|
||||
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
|
||||
useViennaSLCSign = scene.use_vienna_slc_sign;
|
||||
|
||||
trafficModeActive = scene.traffic_mode_active;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -62,11 +62,15 @@ private:
|
|||
bool reverseCruise;
|
||||
bool showAlwaysOnLateralStatusBar;
|
||||
bool showConditionalExperimentalStatusBar;
|
||||
bool showSLCOffset;
|
||||
bool speedLimitController;
|
||||
bool trafficModeActive;
|
||||
bool useViennaSLCSign;
|
||||
|
||||
float accelerationConversion;
|
||||
float cruiseAdjustment;
|
||||
float distanceConversion;
|
||||
float slcSpeedLimitOffset;
|
||||
float speedConversion;
|
||||
|
||||
int alertSize;
|
||||
|
|
|
|||
|
|
@ -88,6 +88,9 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
|||
QRect maxSpeedRect(7, 25, 225, 225);
|
||||
bool isMaxSpeedClicked = maxSpeedRect.contains(e->pos()) && scene.reverse_cruise_ui;
|
||||
|
||||
QRect speedLimitRect(7, 250, 225, 225);
|
||||
bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()) && scene.show_slc_offset_ui;
|
||||
|
||||
if (isMaxSpeedClicked) {
|
||||
scene.reverse_cruise = !scene.reverse_cruise;
|
||||
params.putBoolNonBlocking("ReverseCruise", scene.reverse_cruise);
|
||||
|
|
@ -95,6 +98,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) {
|
|||
return;
|
||||
}
|
||||
|
||||
if (isSpeedLimitClicked) {
|
||||
scene.show_slc_offset = !scene.show_slc_offset;
|
||||
params.putBoolNonBlocking("ShowSLCOffset", scene.show_slc_offset);
|
||||
return;
|
||||
}
|
||||
|
||||
if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) {
|
||||
if (clickTimer.isActive()) {
|
||||
clickTimer.stop();
|
||||
|
|
|
|||
|
|
@ -236,6 +236,8 @@ static void update_state(UIState *s) {
|
|||
if (sm.updated("frogpilotPlan")) {
|
||||
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
|
||||
scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
|
||||
scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
|
||||
scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
|
||||
}
|
||||
if (sm.updated("liveLocationKalman")) {
|
||||
auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
|
||||
|
|
@ -293,6 +295,11 @@ void ui_update_frogpilot_params(UIState *s) {
|
|||
scene.reverse_cruise = quality_of_life_controls && params.getBool("ReverseCruise");
|
||||
scene.reverse_cruise_ui = params.getBool("ReverseCruiseUI");
|
||||
|
||||
scene.speed_limit_controller = scene.longitudinal_control && params.getBool("SpeedLimitController");
|
||||
scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset");
|
||||
scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI");
|
||||
scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna");
|
||||
|
||||
scene.tethering_config = params.getInt("TetheringEnabled");
|
||||
if (scene.tethering_config == 2) {
|
||||
WifiManager(s).setTetheringEnabled(true);
|
||||
|
|
|
|||
|
|
@ -136,13 +136,19 @@ typedef struct UIScene {
|
|||
bool right_hand_drive;
|
||||
bool show_aol_status_bar;
|
||||
bool show_cem_status_bar;
|
||||
bool show_slc_offset;
|
||||
bool show_slc_offset_ui;
|
||||
bool speed_limit_controller;
|
||||
bool tethering_enabled;
|
||||
bool traffic_mode;
|
||||
bool traffic_mode_active;
|
||||
bool use_kaofui_icons;
|
||||
bool use_vienna_slc_sign;
|
||||
|
||||
float adjusted_cruise;
|
||||
float lead_detection_threshold;
|
||||
float speed_limit;
|
||||
float speed_limit_offset;
|
||||
|
||||
int alert_size;
|
||||
int conditional_speed;
|
||||
|
|
|
|||
Loading…
Reference in New Issue