mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-13 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

DEC: Update logic from dragonpilot-community/dragonpilot:lp-dp-beta2

Browse Source
This commit is contained in:
Jason Wen
2023-12-24 16:43:31 +00:00
parent 4778efe4f6
commit d54fd16ae3
2 changed files with 65 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+1 -1
View File
@@ -1,7 +1,7 @@
sunnypilot - 0.9.6.1 (2023-xx-xx)
========================
* UPDATED: Dynamic Experimental Control (DEC)
* Synced with dragonpilot-community/dragonpilot:beta3 commit dd4c663
* Synced with dragonpilot-community/dragonpilot:lp-dp-beta2 commit 578d38b
* UPDATED: Vision-based Turn Speed Control (V-TSC) implementation
* Refactored implementation thanks to pfeiferj!
* More accurate and consistent velocity calculation to achieve smoother longitudinal control in curves
selfdrive/controls/lib/dynamic_experimental_controller.py
+64 -38
View File
@@ -21,7 +21,7 @@
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
#
# Version = 0.1.2
# Version = 2023-12-13
from common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
@@ -38,13 +38,16 @@ SLOWNESS_PROB = 0.6
SLOWNESS_CRUISE_OFFSET = 1.05
DANGEROUS_TTC_WINDOW_SIZE = 5
DANGEROUS_TTC = 1.55
DANGEROUS_TTC = 2.0
HIGHWAY_CRUISE_KPH = 75
STOP_AND_GO_FRAME = 500
STOP_AND_GO_FRAME = 60
MODE_SWITCH_DELAY_FRAME = 500
SET_MODE_TIMEOUT = 10
MPC_FCW_WINDOW_SIZE = 5
MPC_FCW_PROB = 0.6
class SNG_State:
@@ -80,8 +83,6 @@ class DynamicExperimentalController:
self._is_enabled = False
self._mode = 'acc'
self._mode_prev = 'acc'
self._mode_switch_allowed = True
self._mode_switch_frame = 0
self._frame = 0
self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
@@ -90,41 +91,45 @@ class DynamicExperimentalController:
self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
self._has_slow_down = False
self._has_slow_down_prev = False
self._has_blinkers = False
self._has_blinkers_prev = False
self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
self._has_slowness = False
self._has_slowness_prev = False
self._has_nav_enabled = False
self._has_nav_enabled_prev = False
self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
self._has_dangerous_ttc = False
self._has_dangerous_ttc_prev = False
self._v_ego_kph = 0.
self._v_cruise_kph = 0.
self._has_lead = False
self._has_lead_prev = False
self._has_standstill = False
self._has_standstill_prev = False
self._sng_transit_frame = 0
self._sng_state = SNG_State.off
self._mpc_fcw_gmac = GenericMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
self._has_mpc_fcw = False
self._mpc_fcw_crash_cnt = 0
self._set_mode_timeout = 0
pass
def _update(self, car_state, lead_one, md, controls_state, radar_unavailable):
def _update(self, car_state, lead_one, md, controls_state):
self._v_ego_kph = car_state.vEgo * 3.6
self._v_cruise_kph = controls_state.vCruise
self._has_lead = lead_one.status
self._has_standstill = car_state.standstill
# fcw detection
self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB
# nav enable detection
self._has_nav_enabled = md.navEnabled
@@ -154,8 +159,9 @@ class DynamicExperimentalController:
self._sng_transit_frame -= 1
# slowness detection
self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
if not self._has_standstill:
self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
# dangerous TTC detection
if not self._has_lead_filtered and self._has_lead_filtered_prev:
@@ -169,86 +175,94 @@ class DynamicExperimentalController:
# keep prev values
self._has_standstill_prev = self._has_standstill
self._has_slowness_prev = self._has_slowness
self._has_slow_down_prev = self._has_slow_down
self._has_lead_filtered_prev = self._has_lead_filtered
self._frame += 1
def _blended_priority_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
self._set_mode('blended')
return
# when blinker is on and speed is driving below highway cruise speed: blended
# we don't want it to switch mode at higher speed, blended may trigger hard brake
# we dont want it to switch mode at higher speed, blended may trigger hard brake
if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
self._mode = 'blended'
self._set_mode('blended')
return
# when at highway cruise and SNG: blended
# ensuring blended mode is used because acc is bad at catching SNG lead car
# especially those that accel very fast and then brake very hard.
# especially those who accel very fast and then brake very hard.
if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
self._mode = 'blended'
self._set_mode('blended')
return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
# in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
if self._has_standstill:
self._mode = 'blended'
self._set_mode('blended')
return
# when detecting slow down scenario: blended
# e.g. traffic light, curve, stop sign etc.
if self._has_slow_down:
self._mode = 'blended'
self._set_mode('blended')
return
# when detecting lead slow down: blended
# use blended for higher braking capability
if self._has_dangerous_ttc:
self._mode = 'blended'
self._set_mode('blended')
return
# car driving at speed lower than set speed: acc
if self._has_slowness:
self._mode = 'acc'
self._set_mode('acc')
return
self._mode = 'blended'
self._set_mode('blended')
def _acc_priority_mode(self):
# when mpc fcw crash prob is high
# use blended to slow down quickly
if self._has_mpc_fcw:
self._set_mode('blended')
return
# If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
#if self._has_lead_filtered and not self._has_standstill:
# self._mode = 'acc'
# self._set_mode('acc')
# return
# when blinker is on and speed is driving below highway cruise speed: blended
# we don't want it to switch mode at higher speed, blended may trigger hard brake
# we dont want it to switch mode at higher speed, blended may trigger hard brake
if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
self._mode = 'blended'
self._set_mode('blended')
return
# when standstill: blended
# in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
# in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
if self._has_standstill:
self._mode = 'blended'
self._set_mode('blended')
return
# when detecting slow down scenario: blended
# e.g. traffic light, curve, stop sign etc.
if self._has_slow_down:
self._mode = 'blended'
self._set_mode('blended')
return
# car driving at speed lower than set speed: acc
if self._has_slowness:
self._mode = 'acc'
self._set_mode('acc')
return
self._mode = 'acc'
self._set_mode('acc')
def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state):
if self._is_enabled:
self._update(car_state, lead_one, md, controls_state, radar_unavailable)
if self._frame > self._mode_switch_frame:
self._mode_switch_allowed = True
self._update(car_state, lead_one, md, controls_state)
if radar_unavailable:
self._blended_priority_mode()
else:
@@ -262,3 +276,15 @@ class DynamicExperimentalController:
def is_enabled(self):
return self._is_enabled
def set_mpc_fcw_crash_cnt(self, crash_cnt):
self._mpc_fcw_crash_cnt = crash_cnt
def _set_mode(self, mode):
if self._set_mode_timeout == 0:
self._mode = mode
if mode == "blended":
self._set_mode_timeout = SET_MODE_TIMEOUT
if self._set_mode_timeout > 0:
self._set_mode_timeout -= 1