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

Controls - Speed Limit Controller - Override Method

Browse Source
This commit is contained in:
FrogAi 2024-05-26 23:17:10 -07:00
parent df6ad0147c
commit f7c8044c34
5 changed files with 30 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -56,11 +56,13 @@ class FrogPilotPlanner:
self.lead_one = Lead()
self.mtsc = MapTurnSpeedController()
self.override_slc = False
self.slower_lead = False
self.acceleration_jerk = 0
self.frame = 0
self.mtsc_target = 0
self.overridden_speed = 0
self.road_curvature = 0
self.slc_target = 0
self.speed_jerk = 0
@ -214,10 +216,24 @@ class FrogPilotPlanner:
self.params_memory.put_bool("SLCConfirmed", False)
else:
self.slc_target = unconfirmed_slc_target
self.override_slc &= self.overridden_speed > self.slc_target
self.override_slc |= carState.gasPressed and v_ego > self.slc_target
self.override_slc &= controlsState.enabled
if self.override_slc:
if frogpilot_toggles.speed_limit_controller_override == 1:
if carState.gasPressed:
self.overridden_speed = v_ego + v_ego_diff
self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff)
elif frogpilot_toggles.speed_limit_controller_override == 2:
self.overridden_speed = v_cruise + v_cruise_diff
else:
self.overridden_speed = 0
else:
self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0
targets = [self.mtsc_target, self.slc_target - v_ego_diff]
targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff]
filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets]
return min(filtered_targets)
@ -240,6 +256,8 @@ class FrogPilotPlanner:
frogpilotPlan.maxAcceleration = self.max_accel
frogpilotPlan.minAcceleration = self.min_accel
frogpilotPlan.slcOverridden = bool(self.override_slc)
frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed)
frogpilotPlan.slcSpeedLimit = self.slc_target
frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset
frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit

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

@ -376,9 +376,9 @@ 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 = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0;
speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH);
if (speedLimitController) {
if (speedLimitController && !slcOverridden) {
speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0);
}
@ -496,8 +496,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16);
p.save();
p.setOpacity(1.0);
if (speedLimitController && showSLCOffset) {
p.setOpacity(slcOverridden ? 0.25 : 1.0);
if (speedLimitController && showSLCOffset && !slcOverridden) {
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));
@ -523,7 +523,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16));
p.save();
p.setOpacity(1.0);
p.setOpacity(slcOverridden ? 0.25 : 1.0);
p.setPen(blackColor());
if (showSLCOffset) {
p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold));
@ -876,6 +876,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
speedLimitController = scene.speed_limit_controller;
showSLCOffset = speedLimitController && scene.show_slc_offset;
slcOverridden = speedLimitController && scene.speed_limit_overridden;
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
useViennaSLCSign = scene.use_vienna_slc_sign;

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

@ -154,6 +154,7 @@ private:
bool showAlwaysOnLateralStatusBar;
bool showConditionalExperimentalStatusBar;
bool showSLCOffset;
bool slcOverridden;
bool speedLimitController;
bool trafficModeActive;
bool useViennaSLCSign;

2
selfdrive/ui/ui.cc
View File

@ -241,6 +241,8 @@ static void update_state(UIState *s) {
scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise();
scene.speed_limit = frogpilotPlan.getSlcSpeedLimit();
scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset();
scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden();
scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed();
scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit();
}
if (sm.updated("liveLocationKalman")) {

2
selfdrive/ui/ui.h
View File

@ -205,6 +205,7 @@ typedef struct UIScene {
bool show_slc_offset_ui;
bool speed_limit_changed;
bool speed_limit_controller;
bool speed_limit_overridden;
bool tethering_enabled;
bool traffic_mode;
bool traffic_mode_active;
@ -215,6 +216,7 @@ typedef struct UIScene {
float lead_detection_threshold;
float speed_limit;
float speed_limit_offset;
float speed_limit_overridden_speed;
float unconfirmed_speed_limit;
int alert_size;