From 5e1d9da867029bcd673ba6d0a7d8e4da76788718 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 5 Feb 2023 14:17:32 -0500 Subject: [PATCH 1/2] Max Time Offroad --- selfdrive/thermald/power_monitoring.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 85e9510eb7..90dd7d67bd 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -1,6 +1,7 @@ import threading from typing import Optional +from common.numpy_fast import interp from common.params import Params, put_nonblocking from common.realtime import sec_since_boot from system.hardware import HARDWARE @@ -15,7 +16,6 @@ CAR_CHARGING_RATE_W = 45 VBATT_PAUSE_CHARGING = 11.8 # Lower limit on the LPF car battery voltage VBATT_INSTANT_PAUSE_CHARGING = 7.0 # Lower limit on the instant car battery voltage measurements to avoid triggering on instant power loss -MAX_TIME_OFFROAD_S = 30*3600 MIN_ON_TIME_S = 3600 VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 60 @@ -112,13 +112,17 @@ class PowerMonitoring: if offroad_timestamp is None: return False + max_time_offroad_s = interp(int(self.params.get("MaxTimeOffroad", encoding="utf8")), + [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], + [0, 5, 30, 60, 180, 300, 600, 1800, 3600, 10800, 18000, 36000, 108000]) + now = sec_since_boot() should_shutdown = False offroad_time = (now - offroad_timestamp) low_voltage_shutdown = (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3) and self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3) and offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S) - should_shutdown |= offroad_time > MAX_TIME_OFFROAD_S + should_shutdown |= (offroad_time > max_time_offroad_s) if max_time_offroad_s != 0 else False should_shutdown |= low_voltage_shutdown should_shutdown |= (self.car_battery_capacity_uWh <= 0) should_shutdown &= not ignition From 2a841a775759cd2dd20399960c34ae904c967630 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Sun, 5 Feb 2023 14:31:09 -0500 Subject: [PATCH 2/2] Lane Change Timer (#104) --- selfdrive/controls/controlsd.py | 8 ++++++-- selfdrive/controls/lib/desire_helper.py | 20 ++++++++++++++++++-- selfdrive/controls/lib/lateral_planner.py | 1 + 3 files changed, 25 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fff6bcf576..873918f2f3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -293,16 +293,20 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change + lane_change_set_timer = int(self.params.get("AutoLaneChangeTimer", encoding="utf8")) if self.sm['lateralPlan'].laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['lateralPlan'].laneChangeDirection + lc_prev = self.sm['lateralPlan'].laneChangePrev if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): self.events.add(EventName.laneChangeBlocked) else: if direction == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) + self.events.add(EventName.preLaneChangeLeft) if lane_change_set_timer == 0 or lc_prev else \ + self.events.add(EventName.laneChange) else: - self.events.add(EventName.preLaneChangeRight) + self.events.add(EventName.preLaneChangeRight) if lane_change_set_timer == 0 or lc_prev else \ + self.events.add(EventName.laneChange) elif self.sm['lateralPlan'].laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): self.events.add(EventName.laneChange) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 4790b8f0eb..ef050339a8 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,5 +1,6 @@ from cereal import log from common.conversions import Conversions as CV +from common.params import Params from common.realtime import DT_MDL LaneChangeState = log.LateralPlan.LaneChangeState @@ -40,7 +41,15 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.LateralPlan.Desire.none + self.param_s = Params() + self.lane_change_wait_timer = 0 + self.prev_lane_change = False + def update(self, carstate, lateral_active, lane_change_prob): + lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) + lane_change_auto_timer = 0.0 if lane_change_set_timer == 0 else 0.1 if lane_change_set_timer == 1 else \ + 0.5 if lane_change_set_timer == 2 else 1.0 if lane_change_set_timer == 3 else \ + 1.5 if lane_change_set_timer == 4 else 2.0 v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -48,11 +57,13 @@ class DesireHelper: if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none + self.prev_lane_change = False else: # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed and not carstate.brakePressed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 + self.lane_change_wait_timer = 0 # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: @@ -67,10 +78,14 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + self.lane_change_wait_timer += DT_MDL if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off - elif torque_applied and not blindspot_detected: + self.prev_lane_change = False + elif (torque_applied or ((lane_change_auto_timer and self.lane_change_wait_timer > lane_change_auto_timer) and not self.prev_lane_change)) and \ + not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting + self.prev_lane_change = True # LaneChangeState.laneChangeStarting elif self.lane_change_state == LaneChangeState.laneChangeStarting: @@ -92,6 +107,7 @@ class DesireHelper: self.lane_change_state = LaneChangeState.preLaneChange else: self.lane_change_state = LaneChangeState.off + self.prev_lane_change = False if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): self.lane_change_timer = 0.0 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 932ad49535..05c6d6f31a 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -129,5 +129,6 @@ class LateralPlanner: lateralPlan.useLaneLines = False lateralPlan.laneChangeState = self.DH.lane_change_state lateralPlan.laneChangeDirection = self.DH.lane_change_direction + lateralPlan.laneChangePrev = self.DH.prev_lane_change pm.send('lateralPlan', plan_send)