diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 615021110..c66fb7ec5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -131,11 +131,15 @@ class CarInterfaceBase(): events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if cs_out.espDisabled: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if cs_out.gasPressed: + if cs_out.gasPressed and not self.dragon_toyota_stock_dsu and not self.dragon_allow_gas: events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # TODO: move this stuff to the capnp strut - if getattr(self.CS, "steer_error", False): + if not self.dragon_lat_ctrl: + events.append(create_event('manualSteeringRequired', [ET.WARNING])) + elif self.dragon_enable_steering_on_signal and (cs_out.leftBlinker or cs_out.rightBlinker): + events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) + elif getattr(self.CS, "steer_error", False): events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif getattr(self.CS, "steer_warning", False): events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) @@ -143,9 +147,15 @@ class CarInterfaceBase(): # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ - (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if not self.dragon_toyota_stock_dsu: + # DragonAllowGas + if not self.dragon_allow_gas: + if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + else: + if cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) return events diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 35cfde668..c5e02ab3e 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -34,6 +34,7 @@ class LongitudinalMpc(): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False + self.last_cloudlog_t = 0.0 # df from Shane Smiskol diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 80675a2d6..01eb8d258 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -85,7 +85,7 @@ class LongControl(): v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if self.long_control_state == LongCtrlState.off or gas_pressed: + if self.long_control_state == LongCtrlState.off or gas_pressed or brake_pressed: self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 887270ae4..648ab9c06 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -25,16 +25,19 @@ DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, }, LaneChangeDirection.left: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, }, } diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 443047b57..609e6ae53 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -1249,7 +1249,6 @@ static void ui_draw_blank(UIState *s) { } void ui_draw(UIState *s) { - ui_draw_sidebar(s); if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { ui_draw_sidebar(s); ui_draw_vision(s); diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 75e67e086..f1eea6423 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -95,9 +95,9 @@ static void ui_draw_sidebar_battery_text(UIState *s, bool hasSidebar) { const int battery_img_x = hasSidebar ? 150 : -(sbr_w); const int battery_img_y = 305; - char battery_str[6]; - snprintf(battery_str, sizeof(battery_str), "%d%%", s->scene.batteryPercent); - nvgFillColor(s->vg, strcmp(s->scene.batteryStatus, "Charging") == 0? COLOR_GREEN : s->scene.batteryPercent <= 50? COLOR_YELLOW : s->scene.batteryPercent <= 15? COLOR_RED : COLOR_WHITE); + char battery_str[7]; + snprintf(battery_str, sizeof(battery_str), "%d%%%s", s->scene.batteryPercent, strcmp(s->scene.batteryStatus, "Charging") == 0 ? "+" : "-"); + nvgFillColor(s->vg, COLOR_WHITE); nvgFontSize(s->vg, 40); nvgFontFace(s->vg, "sans-regular"); nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); @@ -106,7 +106,7 @@ static void ui_draw_sidebar_battery_text(UIState *s, bool hasSidebar) { static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { const int network_x = hasSidebar ? 50 : -(sbr_w); - const int network_y = 273; + const int network_y = 303; const int network_w = 100; const int network_h = 100; const char *network_types[6] = {"--", "WiFi", "2G", "3G", "4G", "5G"};