zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Controls - Speed Limit Controller - Confirm New Speed Limits

Browse Source 
Don't automatically start using the new speed limit until it's been manually confirmed.
This commit is contained in:
FrogAi 2024-05-11 20:37:19 -07:00
parent fa92c97d8b
commit df6ad0147c
10 changed files with 142 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/car/interfaces.py
View File

@ -548,6 +548,10 @@ class CarStateBase(ABC):
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
# FrogPilot variables
self.cruise_decreased = False
self.cruise_decreased_previously = False
self.cruise_increased = False
self.cruise_increased_previously = False
self.lkas_enabled = False
self.lkas_previously_enabled = False

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

@ -190,6 +190,12 @@ class CarState(CarStateBase):
self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
# FrogPilot carstate functions
self.cruise_decreased_previously = self.cruise_decreased
self.cruise_increased_previously = self.cruise_increased
self.cruise_decreased = self.pcm_acc_status == 10
self.cruise_increased = self.pcm_acc_status == 9
fp_ret.ecoGear = cp.vl["GEAR_PACKET"]['ECON_ON'] == 1
fp_ret.sportGear = cp.vl["GEAR_PACKET"]['SPORT_ON_2' if self.CP.flags & ToyotaFlags.NO_DSU else 'SPORT_ON'] == 1

2
selfdrive/car/toyota/interface.py
View File

@ -173,6 +173,8 @@ class CarInterface(CarInterfaceBase):
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
ret.buttonEvents = [
*create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}),
*create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}),
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]

52
selfdrive/controls/controlsd.py
View File

@ -36,6 +36,8 @@ from openpilot.system.version import get_short_branch
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CRUISING_SPEED, PROBABILITY, FrogPilotVariables
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
@ -78,6 +80,8 @@ class Controls:
self.display_timer = 0
self.drive_distance = 0
self.drive_time = 0
self.previous_speed_limit = 0
self.speed_limit_timer = 0
self.card = CarD(CI)
@ -480,7 +484,7 @@ class Controls:
def state_transition(self, CS):
"""Compute conditional state transitions and execute actions on state transitions"""
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.frogpilot_toggles)
self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_toggles)
# decrement the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
@ -555,7 +559,7 @@ class Controls:
else:
self.state = State.enabled
self.current_alert_types.append(ET.ENABLE)
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_toggles)
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_toggles)
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
@ -928,6 +932,50 @@ class Controls:
self.events.add(EventName.openpilotCrashed)
self.openpilot_crashed_triggered = True
if self.frogpilot_toggles.speed_limit_confirmation:
current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit
desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit
speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1
speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit
speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit
self.previous_speed_limit = desired_speed_limit
if self.CP.pcmCruise and self.FPCC.speedLimitChanged:
if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents):
self.params_memory.put_bool("SLCConfirmed", True)
self.params_memory.put_bool("SLCConfirmedPressed", True)
elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
self.params_memory.put_bool("SLCConfirmed", False)
self.params_memory.put_bool("SLCConfirmedPressed", True)
if speed_limit_changed_lower:
if self.frogpilot_toggles.speed_limit_confirmation_lower:
self.FPCC.speedLimitChanged = True
else:
self.params_memory.put_bool("SLCConfirmed", True)
elif speed_limit_changed_higher:
if self.frogpilot_toggles.speed_limit_confirmation_higher:
self.FPCC.speedLimitChanged = True
else:
self.params_memory.put_bool("SLCConfirmed", True)
if self.params_memory.get_bool("SLCConfirmedPressed") or not self.frogpilot_toggles.speed_limit_confirmation or not abs(current_speed_limit - desired_speed_limit) > 1:
self.FPCC.speedLimitChanged = False
self.params_memory.put_bool("SLCConfirmedPressed", False)
if self.FPCC.speedLimitChanged:
self.speed_limit_timer += DT_CTRL
if self.speed_limit_timer >= 10:
self.FPCC.speedLimitChanged = False
self.speed_limit_timer = 0
else:
self.speed_limit_timer = 0
else:
self.FPCC.speedLimitChanged = False
if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff:
self.events.add(EventName.torqueNNLoad)

24
selfdrive/controls/lib/drive_helpers.py
View File

@ -53,13 +53,13 @@ class VCruiseHelper:
def v_cruise_initialized(self):
return self.v_cruise_kph != V_CRUISE_UNSET
def update_v_cruise(self, CS, enabled, is_metric, frogpilot_variables):
def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
self.v_cruise_kph_last = self.v_cruise_kph
if CS.cruiseState.available:
if not self.CP.pcmCruise:
# if stock cruise is completely disabled, then we can use our own set speed logic
self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_variables)
self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_variables)
self.v_cruise_cluster_kph = self.v_cruise_kph
self.update_button_timers(CS, enabled)
else:
@ -69,7 +69,7 @@ class VCruiseHelper:
self.v_cruise_kph = V_CRUISE_UNSET
self.v_cruise_cluster_kph = V_CRUISE_UNSET
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, frogpilot_variables):
def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables):
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
if not enabled:
@ -96,6 +96,17 @@ class VCruiseHelper:
if button_type is None:
return
# Confirm or deny the new speed limit value
if speed_limit_changed:
if button_type == ButtonType.accelCruise:
self.params_memory.put_bool("SLCConfirmed", True)
self.params_memory.put_bool("SLCConfirmedPressed", True)
return
elif button_type == ButtonType.decelCruise:
self.params_memory.put_bool("SLCConfirmed", False)
self.params_memory.put_bool("SLCConfirmedPressed", True)
return
# Don't adjust speed when pressing resume to exit standstill
cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
if button_type == ButtonType.accelCruise and cruise_standstill:
@ -135,7 +146,7 @@ class VCruiseHelper:
self.button_timers[b.type.raw] = 1 if b.pressed else 0
self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_variables) -> None:
def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None:
# initializing is handled by the PCM
if self.CP.pcmCruise:
return
@ -146,7 +157,10 @@ class VCruiseHelper:
if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250:
self.v_cruise_kph = self.v_cruise_kph_last
else:
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit:
self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH))
else:
self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
self.v_cruise_cluster_kph = self.v_cruise_kph

10
selfdrive/frogpilot/controls/frogpilot_planner.py
View File

@ -206,7 +206,14 @@ class FrogPilotPlanner:
# 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
unconfirmed_slc_target = SpeedLimitController.desired_speed_limit
if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0:
if self.params_memory.get_bool("SLCConfirmed"):
self.slc_target = unconfirmed_slc_target
self.params_memory.put_bool("SLCConfirmed", False)
else:
self.slc_target = unconfirmed_slc_target
else:
self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
@ -235,6 +242,7 @@ class FrogPilotPlanner:
frogpilotPlan.slcSpeedLimit = self.slc_target
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit
frogpilotPlan.vCruise = float(self.v_cruise)

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

@ -101,12 +101,25 @@ void OnroadWindow::updateState(const UIState &s) {
void OnroadWindow::mousePressEvent(QMouseEvent* e) {
// FrogPilot clickable widgets
QSize size = this->size();
QRect leftRect(0, 0, size.width() / 2, size.height());
QRect rightRect = leftRect.translated(size.width() / 2, 0);
bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed;
bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed;
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 (isLeftSideClicked || isRightSideClicked) {
bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive;
paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed);
paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true);
return;
}
if (isMaxSpeedClicked) {
bool currentReverseCruise = scene.reverse_cruise;
uiState()->scene.reverse_cruise = !currentReverseCruise;
@ -874,6 +887,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
drawStatusBar(p);
}
if (scene.speed_limit_changed) {
drawSLCConfirmation(p);
}
bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons;
distance_btn->setVisible(enableDistanceButton);
if (enableDistanceButton) {
@ -958,6 +975,36 @@ void DistanceButton::paintEvent(QPaintEvent *event) {
}
}
void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) {
p.save();
QSize size = this->size();
QRect leftRect(0, 0, size.width() / 2, size.height());
QRect rightRect = leftRect.translated(size.width() / 2, 0);
p.setOpacity(0.5);
p.fillRect(leftRect, rightHandDM ? redColor() : greenColor());
p.fillRect(rightRect, rightHandDM ? greenColor() : redColor());
p.setOpacity(1.0);
p.setFont(InterFont(75, QFont::Bold));
p.setPen(Qt::white);
QString unitText = is_metric ? tr("kph") : tr("mph");
QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText;
QString confirmText = tr("Confirm speed limit\n") + speedText;
QString ignoreText = tr("Ignore speed limit\n") + speedText;
QRect textLeftRect = QRect(leftRect.left(), leftRect.top() + leftRect.height() / 2 - 225, leftRect.width(), leftRect.height() / 2);
QRect textRightRect = QRect(rightRect.left(), rightRect.top() + rightRect.height() / 2 - 225, rightRect.width(), rightRect.height() / 2);
p.drawText(textLeftRect, Qt::AlignCenter, rightHandDM ? ignoreText : confirmText);
p.drawText(textRightRect, Qt::AlignCenter, rightHandDM ? confirmText : ignoreText);
p.restore();
}
void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
p.save();

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

@ -136,6 +136,7 @@ private:
void paintFrogPilotWidgets(QPainter &p);
void updateFrogPilotWidgets();
void drawSLCConfirmation(QPainter &p);
void drawStatusBar(QPainter &p);
// FrogPilot variables

2
selfdrive/ui/ui.cc
View File

@ -230,6 +230,7 @@ static void update_state(UIState *s) {
if (sm.updated("frogpilotCarControl")) {
auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
scene.speed_limit_changed = scene.speed_limit_controller && frogpilotCarControl.getSpeedLimitChanged();
scene.traffic_mode_active = frogpilotCarControl.getTrafficModeActive();
}
if (sm.updated("frogpilotCarState")) {
@ -240,6 +241,7 @@ static void update_state(UIState *s) {
scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
}
if (sm.updated("liveLocationKalman")) {
auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();

2
selfdrive/ui/ui.h
View File

@ -203,6 +203,7 @@ typedef struct UIScene {
bool show_cem_status_bar;
bool show_slc_offset;
bool show_slc_offset_ui;
bool speed_limit_changed;
bool speed_limit_controller;
bool tethering_enabled;
bool traffic_mode;
@ -214,6 +215,7 @@ typedef struct UIScene {
float lead_detection_threshold;
float speed_limit;
float speed_limit_offset;
float unconfirmed_speed_limit;
int alert_size;
int conditional_speed;