mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-24 04:23:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into dev-priv/master

Browse Source
This commit is contained in:
Jason Wen
2023-02-05 15:16:42 -05:00
parent ff1eebcc47 2a841a7757
commit 0ca23636d4
4 changed files with 30 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+6 -2
View File
@@ -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)
selfdrive/controls/lib/desire_helper.py
+17 -1
View File
@@ -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
selfdrive/controls/lib/lateral_planner.py
+1
View File
@@ -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)
selfdrive/thermald/power_monitoring.py
+6 -2
View File
@@ -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