diff --git a/CHANGELOGS.md b/CHANGELOGS.md index fdce578946..9c387e097b 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -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 diff --git a/selfdrive/controls/lib/dynamic_experimental_controller.py b/selfdrive/controls/lib/dynamic_experimental_controller.py index c3e1dd31a7..aa2e239b38 100644 --- a/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -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