mawei
/
ods777
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Controls - Conditional Experimental Mode 

Automatically switches to 'Experimental Mode' under predefined conditions.
This commit is contained in:
FrogAi 2024-06-07 15:49:08 -07:00
parent cf92c3af1b
commit e2d1a599d9
10 changed files with 105 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/controls/controlsd.py
View File

@ -861,7 +861,8 @@ class Controls:
def params_thread(self, evt):
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode:
self.experimental_mode = self.params.get_bool("ExperimentalMode")
self.personality = self.read_personality_param()
if self.CP.notCar:
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@ -916,6 +917,9 @@ class Controls:
self.params_tracking.put_int_nonblocking("FrogPilotDrives", self.current_total_drives)
self.drive_added = True
if self.frogpilot_toggles.conditional_experimental_mode:
self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimentalActive
FPCC = custom.FrogPilotCarControl.new_message()
FPCC.alwaysOnLateral = self.always_on_lateral_active

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

@ -14,6 +14,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHA
get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
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
@ -23,6 +24,8 @@ class FrogPilotPlanner:
def __init__(self):
self.params_memory = Params("/dev/shm/params")
self.cem = ConditionalExperimentalMode()
self.tracking_lead = False
self.acceleration_jerk = 0
@ -72,6 +75,9 @@ class FrogPilotPlanner:
self.road_curvature = calculate_road_curvature(modelData, v_ego)
self.v_cruise = self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles)
if frogpilot_toggles.conditional_experimental_mode:
self.cem.update(carState, controlsState.enabled, frogpilotNavigation, self.lead_one, modelData, self.road_curvature, self.slower_lead, v_ego, v_lead, frogpilot_toggles)
if v_ego > CRUISING_SPEED:
self.tracking_lead = self.lead_one.status
@ -99,6 +105,8 @@ class FrogPilotPlanner:
frogpilotPlan.speedJerk = J_EGO_COST * float(self.speed_jerk)
frogpilotPlan.speedJerkStock = J_EGO_COST * float(self.base_speed_jerk)
frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode
frogpilotPlan.laneWidthLeft = self.lane_width_left
frogpilotPlan.laneWidthRight = self.lane_width_right

38
selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py Normal file
View File

@ -0,0 +1,38 @@
from openpilot.common.params import Params
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY
class ConditionalExperimentalMode:
def __init__(self):
self.params_memory = Params("/dev/shm/params")
self.experimental_mode = False
self.lead_detection_mac = MovingAverageCalculator()
def update(self, carState, enabled, frogpilotNavigation, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles) and enabled
self.experimental_mode = condition_met
self.params_memory.put_int("CEStatus", self.status_value if condition_met else 0)
def check_conditions(self, carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles):
if carState.standstill:
self.status_value = 0
return self.experimental_mode
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
return False
def update_conditions(self, lead_distance, lead_status, modelData, road_curvature, slower_lead, standstill, v_ego, v_lead, frogpilot_toggles):
self.lead_detection(lead_status)
def lead_detection(self, lead_status):
self.lead_detection_mac.add_data(lead_status)
self.lead_detected = self.lead_detection_mac.get_moving_average() >= PROBABILITY

8
selfdrive/ui/qt/offroad/settings.cc
View File

@ -176,7 +176,13 @@ void TogglesPanel::updateToggles() {
op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
if (hasLongitudinalControl(CP)) {
// normal description and toggle
experimental_mode_toggle->setEnabled(true);
bool conditional_experimental = params.getBool("ConditionalExperimental");
if (conditional_experimental) {
params.putBool("ExperimentalMode", true);
params.putBool("ExperimentalModeConfirmed", true);
experimental_mode_toggle->refresh();
}
experimental_mode_toggle->setEnabled(!conditional_experimental);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(true);
} else {

3
selfdrive/ui/qt/onroad/alerts.cc
View File

@ -16,6 +16,7 @@ void OnroadAlerts::updateState(const UIState &s) {
const UIScene &scene = s.scene;
showAOLStatusBar = scene.show_aol_status_bar;
showCEMStatusBar = scene.show_cem_status_bar;
}
void OnroadAlerts::clear() {
@ -72,7 +73,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
int margin = 40;
int radius = 30;
int offset = showAOLStatusBar ? 25 : 0;
int offset = showAOLStatusBar || showCEMStatusBar ? 25 : 0;
if (alert.size == cereal::ControlsState::AlertSize::FULL) {
margin = 0;
radius = 0;

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

@ -42,4 +42,5 @@ protected:
// FrogPilot variables
bool showAOLStatusBar;
bool showCEMStatusBar;
};

31
selfdrive/ui/qt/onroad/annotated_camera.cc
View File

@ -289,7 +289,7 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
// base icon
int offset = UI_BORDER_SIZE + btn_size / 2;
int x = rightHandDM ? width() - offset : offset;
offset += showAlwaysOnLateralStatusBar ? 25 : 0;
offset += showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar ? 25 : 0;
int y = height() - offset;
float opacity = dmActive ? 0.65 : 0.2;
drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity);
@ -497,13 +497,18 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(const UIScene &scene) {
alwaysOnLateralActive = scene.always_on_lateral_active;
showAlwaysOnLateralStatusBar = scene.show_aol_status_bar;
conditionalSpeed = scene.conditional_speed;
conditionalSpeedLead = scene.conditional_speed_lead;
conditionalStatus = scene.conditional_status;
showConditionalExperimentalStatusBar = scene.show_cem_status_bar;
experimentalMode = scene.experimental_mode;
mapOpen = scene.map_open;
}
void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
if (showAlwaysOnLateralStatusBar) {
if (showAlwaysOnLateralStatusBar || showConditionalExperimentalStatusBar) {
drawStatusBar(p);
}
@ -534,8 +539,30 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
p.setOpacity(1.0);
p.drawRoundedRect(statusBarRect, 30, 30);
const std::map<int, QString> conditionalStatusMap = {
{0, tr("Conditional Experimental Mode ready")},
{1, tr("Conditional Experimental overridden")},
{2, tr("Experimental Mode manually activated")},
{3, tr("Conditional Experimental overridden")},
{4, tr("Experimental Mode manually activated")},
{5, tr("Conditional Experimental overridden")},
{6, tr("Experimental Mode manually activated")},
{7, tr("Experimental Mode activated for") + (mapOpen ? tr(" intersection") : tr(" upcoming intersection"))},
{8, tr("Experimental Mode activated for") + (mapOpen ? tr(" turn") : tr(" upcoming turn"))},
{9, tr("Experimental Mode activated due to") + (mapOpen ? tr(" SLC") : tr(" no speed limit set"))},
{10, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(conditionalSpeedLead ) + (is_metric ? tr(" kph") : tr(" mph")))},
{11, tr("Experimental Mode activated due to") + (mapOpen ? tr(" speed") : tr(" speed being less than ") + QString::number(conditionalSpeed) + (is_metric ? tr(" kph") : tr(" mph")))},
{12, tr("Experimental Mode activated for stopped lead")},
{13, tr("Experimental Mode activated for slower lead")},
{14, tr("Experimental Mode activated for turn") + (mapOpen ? "" : tr(" / lane change"))},
{15, tr("Experimental Mode activated for curve")},
{16, tr("Experimental Mode activated for stop") + (mapOpen ? "" : tr(" sign / stop light"))},
};
if (alwaysOnLateralActive && showAlwaysOnLateralStatusBar) {
newStatus = tr("Always On Lateral active") + (mapOpen ? "" : tr(". Press the \"Cruise Control\" button to disable"));
} else if (showConditionalExperimentalStatusBar) {
newStatus = conditionalStatusMap.at(status != STATUS_DISENGAGED ? conditionalStatus : 0);
}
if (newStatus != lastShownStatus) {

4
selfdrive/ui/qt/onroad/annotated_camera.h
View File

@ -57,12 +57,16 @@ private:
bool experimentalMode;
bool mapOpen;
bool showAlwaysOnLateralStatusBar;
bool showConditionalExperimentalStatusBar;
float accelerationConversion;
float distanceConversion;
float speedConversion;
int alertSize;
int conditionalSpeed;
int conditionalSpeedLead;
int conditionalStatus;
QString accelerationUnit;
QString leadDistanceUnit;

6
selfdrive/ui/ui.cc
View File

@ -264,6 +264,11 @@ void ui_update_frogpilot_params(UIState *s) {
bool always_on_lateral = params.getBool("AlwaysOnLateral");
scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar");
scene.conditional_experimental = scene.longitudinal_control && params.getBool("ConditionalExperimental");
scene.conditional_speed = scene.conditional_experimental ? params.getInt("CESpeed") : 0;
scene.conditional_speed_lead = scene.conditional_experimental ? params.getInt("CESpeedLead") : 0;
scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar");
scene.tethering_config = params.getInt("TetheringEnabled");
if (scene.tethering_config == 2) {
WifiManager(s).setTetheringEnabled(true);
@ -341,6 +346,7 @@ void UIState::update() {
}
// FrogPilot variables that need to be constantly updated
scene.conditional_status = scene.conditional_experimental && scene.enabled ? paramsMemory.getInt("CEStatus") : 0;
}
void UIState::setPrimeType(PrimeType type) {

5
selfdrive/ui/ui.h
View File

@ -118,6 +118,7 @@ typedef struct UIScene {
// FrogPilot variables
bool always_on_lateral_active;
bool conditional_experimental;
bool enabled;
bool experimental_mode;
bool map_open;
@ -125,9 +126,13 @@ typedef struct UIScene {
bool parked;
bool right_hand_drive;
bool show_aol_status_bar;
bool show_cem_status_bar;
bool tethering_enabled;
int alert_size;
int conditional_speed;
int conditional_speed_lead;
int conditional_status;
int tethering_config;
} UIScene;