diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8b0f1dd01b..7cb8a0717a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -309,16 +309,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 342f343986..e2bde64a31 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 carstate.madsEnabled 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 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 e5bee4b7ea..317eda256f 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -172,6 +172,7 @@ class LateralPlanner: lateralPlan.useLaneLines = self.use_lanelines lateralPlan.laneChangeState = self.DH.lane_change_state lateralPlan.laneChangeDirection = self.DH.lane_change_direction + lateralPlan.laneChangePrev = self.DH.prev_lane_change lateralPlan.dynamicLaneProfile = int(self.dynamic_lane_profile) lateralPlan.dynamicLaneProfileStatus = bool(self.dynamic_lane_profile_status) 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