From b0dc49a2fc4a4023987698df2a0d54cf07706b74 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 08:40:36 +0100 Subject: [PATCH 1/7] Stop data_send from crashing because of unknown events --- selfdrive/controls/controlsd.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c955e6e34ef036..a8ee8f7bd17102 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -63,9 +63,10 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space CS = CI.update(CC, can_strs) if isinstance(CS, list): # todo: remove all this and make all interfaces return arne182 events CS, CS_arne182 = CS[0], CS[1] - events = list(CS.events) + list(CS_arne182.events) + events = list(CS.events)# + list(CS_arne182.events) gets added later else: events = list(CS.events) + CS_arne182 = None enabled = isEnabled(state) @@ -130,7 +131,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) - return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter + return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, CS_arne182 def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): @@ -527,7 +528,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events - CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ + CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, CS_arne182 =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") @@ -561,7 +562,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ - state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) + state_transition(sm.frame, CS, CP, state, events + list(CS_arne182.events), soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) From fd7f09c6d8a7d34377160542f3f53eadf12777f2 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 08:52:54 +0100 Subject: [PATCH 2/7] Pass in sendevents to state_transition --- selfdrive/controls/controlsd.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a8ee8f7bd17102..22fd03bd1f09b7 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -561,8 +561,12 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state + if CS_arne182 is not None: + sendevents = events + list(CS_arne182.events) + else: + sendevents = events state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ - state_transition(sm.frame, CS, CP, state, events + list(CS_arne182.events), soft_disable_timer, v_cruise_kph, AM) + state_transition(sm.frame, CS, CP, state, sendevents, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) From a85cb56ed67c37ec652ef5e708ea6bff457ac436 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 15:27:39 +0100 Subject: [PATCH 3/7] try prepend --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 22fd03bd1f09b7..17031423c9709a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -562,7 +562,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state if CS_arne182 is not None: - sendevents = events + list(CS_arne182.events) + sendevents = list(CS_arne182.events) + events else: sendevents = events state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ From 896b37e243d25fd5fc5af61a36b8a64ec58d92d7 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 16:52:39 +0100 Subject: [PATCH 4/7] Append lists together --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 17031423c9709a..f81b71b68e7238 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -562,7 +562,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state if CS_arne182 is not None: - sendevents = list(CS_arne182.events) + events + sendevents = list(CS_arne182.events) + líst(events) else: sendevents = events state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ From 5739edec4af45bff332c09a930f2e9add3096ba5 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 16:55:47 +0100 Subject: [PATCH 5/7] Thank goodness for syntax highlighting --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f81b71b68e7238..2595c9f9c366d0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -562,7 +562,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state if CS_arne182 is not None: - sendevents = list(CS_arne182.events) + líst(events) + sendevents = list(CS_arne182.events) + list(events) else: sendevents = events state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ From 29424862274b2372d7c0004efad55ccec9ff65ed Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 21:17:02 +0100 Subject: [PATCH 6/7] fix right hand turns The negative value is already in steering angle which causes tan to be negative so we do not need an extra minus there. --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 9477bbfa04b2dc..26bdb84f5f8781 100644 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -126,7 +126,7 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. elif steeringAngle < -100: # only at high angles - center_y = +1-2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn + center_y = +1+2.5/math.tan(steeringAngle/1800.*math.pi) # Car Width 2m. Right side considered in right hand turn lead1_check = math.sqrt((lead_1.dRel-center_x)**2+(lead_1.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. lead2_check = math.sqrt((lead_2.dRel-center_x)**2+(lead_2.yRel-center_y)**2) < abs(2.5/math.sin(steeringAngle/1800.*math.pi))+1. if enabled: From a5b80ccb3cb7c91c5976153596cb0c65332d6a0f Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Tue, 3 Dec 2019 21:36:46 +0100 Subject: [PATCH 7/7] Completely seperate events_arne182 and events --- selfdrive/controls/controlsd.py | 110 ++++++++++++++++++++++++++++---- 1 file changed, 98 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2595c9f9c366d0..c370a083c0c71e 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -63,10 +63,11 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space CS = CI.update(CC, can_strs) if isinstance(CS, list): # todo: remove all this and make all interfaces return arne182 events CS, CS_arne182 = CS[0], CS[1] - events = list(CS.events)# + list(CS_arne182.events) gets added later + events = list(CS.events) + events_arne182 = list(CS_arne182.events) else: events = list(CS.events) - CS_arne182 = None + events_arne182 = [] enabled = isEnabled(state) @@ -131,10 +132,10 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) - return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, CS_arne182 + return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, events_arne182 -def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): +def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182): """Compute conditional state transitions and execute actions on state transitions""" enabled = isEnabled(state) @@ -218,11 +219,79 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ elif not get_events(events, [ET.PRE_ENABLE]): state = State.enabled + # DISABLED + if state == State.disabled: + if get_events(events_arne182, [ET.ENABLE]): + if get_events(events_arne182, [ET.NO_ENTRY]): + for e in get_events(events_arne182, [ET.NO_ENTRY]): + AM.add(frame, str(e) + "NoEntry", enabled) + + else: + if get_events(events_arne182, [ET.PRE_ENABLE]): + state = State.preEnabled + else: + state = State.enabled + AM.add(frame, "enable", enabled) + v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonevents_arne182, v_cruise_kph_last) + + # ENABLED + elif state == State.enabled: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif get_events(events_arne182, [ET.SOFT_DISABLE]): + state = State.softDisabling + soft_disable_timer = 300 # 3s + for e in get_events(events_arne182, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + # SOFT DISABLING + elif state == State.softDisabling: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events_arne182, [ET.SOFT_DISABLE]): + # no more soft disabling condition, so go back to ENABLED + state = State.enabled + + elif get_events(events_arne182, [ET.SOFT_DISABLE]) and soft_disable_timer > 0: + for e in get_events(events_arne182, [ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif soft_disable_timer <= 0: + state = State.disabled + + # PRE ENABLING + elif state == State.preEnabled: + if get_events(events_arne182, [ET.USER_DISABLE]): + state = State.disabled + AM.add(frame, "disable", enabled) + + elif get_events(events_arne182, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + state = State.disabled + for e in get_events(events_arne182, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]): + AM.add(frame, e, enabled) + + elif not get_events(events_arne182, [ET.PRE_ENABLE]): + state = State.enabled + return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, radar_state, arne_sm, events_arne182): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -258,6 +327,16 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr else: extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" AM.add(frame, e, enabled, extra_text_2=extra_text) + + # parse warnings from car specific interface + for e in get_events(events_arne182, [ET.WARNING]): + extra_text = "" + if e == "belowSteerSpeed": + if is_metric: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph" + else: + extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph" + AM.add(frame, e, enabled, extra_text_2=extra_text) plan_age = DT_CTRL * (frame - rcv_frame['plan']) dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps @@ -291,6 +370,17 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr else: extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) + + # Parse permanent warnings to display constantly + for e in get_events(events_arne182, [ET.PERMANENT]): + extra_text_1, extra_text_2 = "", "" + if e == "calibrationIncomplete": + extra_text_1 = str(cal_perc) + "%" + if is_metric: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph" + else: + extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" + AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) AM.process_alerts(frame) @@ -528,7 +618,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events - CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, CS_arne182 =\ + CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, events_arne182 =\ data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") @@ -561,18 +651,14 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not read_only: # update control state - if CS_arne182 is not None: - sendevents = list(CS_arne182.events) + list(events) - else: - sendevents = events state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ - state_transition(sm.frame, CS, CP, state, sendevents, soft_disable_timer, v_cruise_kph, AM) + state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM, events_arne182) prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm['radarState'], arne_sm, events_arne182) prof.checkpoint("State Control")