From 260634ce1743a56767a6e27305272e952e738d4b Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Tue, 28 Aug 2018 17:05:43 +0000 Subject: [PATCH 01/17] Adding button settings for autoresume ACC --- selfdrive/car/tesla/ACC_module.py | 2 +- selfdrive/car/tesla/carstate.py | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 63cc3f38831847..ca8c5bd113866f 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -47,7 +47,7 @@ def update_stat(self, CS, enabled): # cruise control should be enabled. Twice in .75 seconds counts as a double # pull. prev_enable_adaptive_cruise = self.enable_adaptive_cruise - self.autoresume = CS.cstm_btns.get_button_label2("acc") == "AutoRes" + self.autoresume = CS.cstm_btns.get_button_label2("acc") in ["AutoOP", "AutoJJ"] curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index cf7f2ca5fc72d8..eb47a81b1f7c1a 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -244,10 +244,14 @@ def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 1) and (btn_status == 0): #don't change status, just model - if (self.cstm_btns.btns[id].btn_label2 == "Mod OP"): - self.cstm_btns.btns[id].btn_label2 = "Mod JJ" + if self.cstm_btns.btns[id].btn_label2 == "OP": + self.cstm_btns.btns[id].btn_label2 = "JJ" + elif self.cstm_btns.btns[id].btn_label2 == "JJ": + self.cstm_btns.btns[id].btn_label2 = "AutoOP" + elif self.cstm_btns.btns[id].btn_label2 == "AutoOP": + self.cstm_btns.btns[id].btn_label2 = "AutoJJ" else: - self.cstm_btns.btns[id].btn_label2 = "Mod OP" + self.cstm_btns.btns[id].btn_label2 = "OP" else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: From 035606a4a358c4a83a2448b9f5ccb622b1240274 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Tue, 28 Aug 2018 18:10:04 +0000 Subject: [PATCH 02/17] Move ACC constants to values.py --- selfdrive/car/tesla/ACC_module.py | 17 ++++++----------- selfdrive/car/tesla/carstate.py | 16 +++++----------- selfdrive/car/tesla/values.py | 23 +++++++++++++++++++++++ 3 files changed, 34 insertions(+), 22 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index ca8c5bd113866f..6c1cb980e5f1dc 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,23 +1,17 @@ from selfdrive.services import service_list -from selfdrive.car.tesla.values import AH, CruiseButtons, CruiseState, CAR +from selfdrive.car.tesla.values import AH, CruiseButtons, CruiseState, CAR, ACCModes, ACCState from selfdrive.config import Conversions as CV import selfdrive.messaging as messaging import os import subprocess import time import zmq - -class ACCState(object): - # Possible state of the ACC system, following the DI_cruiseState naming - # scheme. - OFF = 0 # Disabled by UI. - STANDBY = 1 # Ready to be enaged. - ENABLED = 2 # Engaged. - NOT_READY = 9 # Not ready to be engaged due to the state of the car. + def _current_time_millis(): return int(round(time.time() * 1000)) + class ACCController(object): # Tesla cruise only functions above 18 MPH @@ -47,7 +41,8 @@ def update_stat(self, CS, enabled): # cruise control should be enabled. Twice in .75 seconds counts as a double # pull. prev_enable_adaptive_cruise = self.enable_adaptive_cruise - self.autoresume = CS.cstm_btns.get_button_label2("acc") in ["AutoOP", "AutoJJ"] + acc_mode = CS.cstm_btns.get_button_label2("acc") + self.autoresume = ACCModes[acc_mode].autoresume curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: @@ -177,7 +172,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): elif speed_offset > half_press_kph and half_press_kph < available_speed: # Send cruise stalk up_1st. button_to_press = CruiseButtons.RES_ACCEL - if CS.cstm_btns.get_button_label2("acc") == "Mod JJ": + if CS.cstm_btns.get_button_label2("acc") in ["JJ", "AutoJJ"]: # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the Live20 feed diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index eb47a81b1f7c1a..8a734726284d38 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -2,8 +2,8 @@ from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC -from selfdrive.car.modules.UIBT_module import UIButtons,UIButton +from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC, ACCModes +from selfdrive.car.modules.UIBT_module import UIButtons, UIButton import numpy as np from ctypes import create_string_buffer from selfdrive.car.modules.UIEV_module import UIEvents @@ -243,15 +243,9 @@ def init_ui_buttons(self): def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 1) and (btn_status == 0): - #don't change status, just model - if self.cstm_btns.btns[id].btn_label2 == "OP": - self.cstm_btns.btns[id].btn_label2 = "JJ" - elif self.cstm_btns.btns[id].btn_label2 == "JJ": - self.cstm_btns.btns[id].btn_label2 = "AutoOP" - elif self.cstm_btns.btns[id].btn_label2 == "AutoOP": - self.cstm_btns.btns[id].btn_label2 = "AutoJJ" - else: - self.cstm_btns.btns[id].btn_label2 = "OP" + # don't change status, just model + current_mode = self.cstm_btns.btns[id].btn_label2 + self.cstm_btns.btns[id].btn_label2 = ACCModes[current_mode].next_mode else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 545b9ae41abc8b..f9ff280748a494 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -86,3 +86,26 @@ def is_enabled_or_standby(cls, state): @classmethod def is_faulted(cls, state): return state in [cls.PRE_FAULT, cls.FAULT] + +class ACCState(object): + # Possible state of the ACC system, following the DI_cruiseState naming + # convention. + OFF = 0 # Disabled by UI. + STANDBY = 1 # Ready to be enaged. + ENABLED = 2 # Engaged. + NOT_READY = 9 # Not ready to be engaged due to the state of the car. + + +class ACCMode(object): + def __init__(self, name, autoresume, next_mode): + self.name = name + self.autoresume = autoresume + self.next_mode = next_mode + + +ACCModes = { + "OP": ACCMode(name="OP", autoresume=False, next_mode="JJ"), + "JJ": ACCMode(name="JJ", autoresume=False, next_mode="AutoOP"), + "AutoOP": ACCMode(name="AutoOP", autoresume=True, next_mode="AutoJJ"), + "AutoJJ": ACCMode(name="AutoJJ", autoresume=True, next_mode="OP") +} \ No newline at end of file From 51fe902d9be25da84459cb37c904c345eff0359b Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Tue, 28 Aug 2018 18:10:41 +0000 Subject: [PATCH 03/17] Properly gate ACC auto-resume --- selfdrive/car/tesla/ACC_module.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 6c1cb980e5f1dc..3cdacd0c8796a1 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -137,7 +137,8 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # going fast enough and we are accelerating. if (CS.pcm_acc_status == 1 and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego >= 0.): + and CS.a_ego >= 0. + and self.autoresume): button_to_press = CruiseButtons.DECEL_2ND # If traditional cruise is engaged, then control it. elif (CS.pcm_acc_status == 2 From efbafed2485efea66e80ba1f0a69fd498533ea73 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Tue, 28 Aug 2018 18:11:42 +0000 Subject: [PATCH 04/17] Slightly reduce min ACC speed --- selfdrive/car/tesla/ACC_module.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 3cdacd0c8796a1..5a6629bc588cdf 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -14,8 +14,8 @@ def _current_time_millis(): class ACCController(object): - # Tesla cruise only functions above 18 MPH - MIN_CRUISE_SPEED_MS = 18 * CV.MPH_TO_MS + # Tesla cruise only functions above 17 MPH + MIN_CRUISE_SPEED_MS = 17.5 * CV.MPH_TO_MS def __init__(self, carcontroller): self.CC = carcontroller From 60124f2e3bd0db56d870c4139985b48a148759b9 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Tue, 28 Aug 2018 18:14:03 +0000 Subject: [PATCH 05/17] Gate ACC autoresume in JJ logic --- selfdrive/car/tesla/ACC_module.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 5a6629bc588cdf..70ff1a34c1b26d 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -137,7 +137,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # going fast enough and we are accelerating. if (CS.pcm_acc_status == 1 and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego >= 0. + and CS.a_ego >= 0.05 and self.autoresume): button_to_press = CruiseButtons.DECEL_2ND # If traditional cruise is engaged, then control it. @@ -239,7 +239,8 @@ def calc_follow_button(self, CS): if (CS.pcm_acc_status == 1 and self.enable_adaptive_cruise and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego > 0.12): + and CS.a_ego > 0.05 + and self.autoresume): button = CruiseButtons.DECEL_SET # If traditional cruise is engaged, then control it. elif CS.pcm_acc_status == 2: From d63e131fa635692067a95f40cdc5e80c5f41fd45 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 01:49:47 +0000 Subject: [PATCH 06/17] Don't adjust speed for first second of ACC --- selfdrive/car/tesla/ACC_module.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 70ff1a34c1b26d..1496d1e4d9d0c3 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -21,6 +21,7 @@ def __init__(self, carcontroller): self.CC = carcontroller self.human_cruise_action_time = 0 self.automated_cruise_action_time = 0 + self.enabled_time = 0 self.last_angle = 0. context = zmq.Context() self.poller = zmq.Poller() @@ -58,6 +59,7 @@ def update_stat(self, CS, enabled): if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_adaptive_cruise = True + self.enabled_time = curr_time_ms # Increase ACC speed to match current, if applicable. self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) else: @@ -144,7 +146,8 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): elif (CS.pcm_acc_status == 2 # But don't make adjustments if a human has manually done so in # the last 3 seconds. Human intention should not be overridden. - and current_time_ms > self.human_cruise_action_time + 3000): + and current_time_ms > self.human_cruise_action_time + 3000 + and current_time_ms > self.enabled_time + 1000): if CS.imperial_speed_units: # Imperial unit cars adjust cruise in units of 1 and 5 mph. half_press_kph = 1 * CV.MPH_TO_KPH From 2df9028b2ab8a9569067fbce77c34073b0065a81 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 18:37:27 +0000 Subject: [PATCH 07/17] Hide OP ACC as it's barely usable in 0.5.2 --- selfdrive/car/tesla/ACC_module.py | 100 ++++++++++++++++-------------- selfdrive/car/tesla/carstate.py | 6 +- selfdrive/car/tesla/values.py | 8 +-- 3 files changed, 63 insertions(+), 51 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 1496d1e4d9d0c3..ac75d780465815 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -117,16 +117,13 @@ def update_stat(self, CS, enabled): # Update prev state after all other actions. self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status - + def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # Adaptive cruise control current_time_ms = _current_time_millis() if CruiseButtons.should_be_throttled(CS.cruise_buttons): self.human_cruise_action_time = current_time_ms button_to_press = None - # The difference between OP's target speed and the current cruise - # control speed, in KPH. - speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) if (self.enable_adaptive_cruise # Only do ACC if OP is steering @@ -135,48 +132,10 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # the car think we're doing a 'long press' on the cruise stalk, # resulting in small, jerky speed adjustments. and current_time_ms > self.automated_cruise_action_time + 500): - # Automatically engange traditional cruise if it is idle and we are - # going fast enough and we are accelerating. - if (CS.pcm_acc_status == 1 - and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego >= 0.05 - and self.autoresume): - button_to_press = CruiseButtons.DECEL_2ND - # If traditional cruise is engaged, then control it. - elif (CS.pcm_acc_status == 2 - # But don't make adjustments if a human has manually done so in - # the last 3 seconds. Human intention should not be overridden. - and current_time_ms > self.human_cruise_action_time + 3000 - and current_time_ms > self.enabled_time + 1000): - if CS.imperial_speed_units: - # Imperial unit cars adjust cruise in units of 1 and 5 mph. - half_press_kph = 1 * CV.MPH_TO_KPH - full_press_kph = 5 * CV.MPH_TO_KPH - else: - # Metric cars adjust cruise in units of 1 and 5 kph. - half_press_kph = 1 - full_press_kph = 5 - - # Reduce cruise speed significantly if necessary. Multiply by a % to - # make the car slightly more eager to slow down vs speed up. - if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_2nd. - button_to_press = CruiseButtons.DECEL_2ND - # Reduce speed slightly if necessary. - elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_1st. - button_to_press = CruiseButtons.DECEL_SET - # Increase cruise speed if possible. - elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: - # How much we can accelerate without exceeding max allowed speed. - available_speed = self.acc_speed_kph - CS.v_cruise_actual - if speed_offset > full_press_kph and full_press_kph < available_speed: - # Send cruise stalk up_2nd. - button_to_press = CruiseButtons.RES_ACCEL_2ND - elif speed_offset > half_press_kph and half_press_kph < available_speed: - # Send cruise stalk up_1st. - button_to_press = CruiseButtons.RES_ACCEL - if CS.cstm_btns.get_button_label2("acc") in ["JJ", "AutoJJ"]: + + if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: + button_to_press = self.cal_op_button(CS, pcm_speed, current_time_ms) + elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: # Alternative speed decision logic that uses the lead car's distance # and speed more directly. # Bring in the lead car distance from the Live20 feed @@ -203,6 +162,55 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # self.last_update_time = current_time_ms # print "Desired ACC speed change: %s" % (speed_offset) return button_to_press + + # Adjust speed based off OP's longitudinal model. + def cal_op_button(self, CS, pcm_speed, current_time_ms): + # Automatically engange traditional cruise if it is idle and we are + # going fast enough and we are accelerating. + if (CS.pcm_acc_status == 1 + and CS.v_ego > self.MIN_CRUISE_SPEED_MS + and CS.a_ego >= 0.05 + and self.autoresume): + button_to_press = CruiseButtons.DECEL_2ND + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == 2 + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and current_time_ms > self.human_cruise_action_time + 3000 + and current_time_ms > self.enabled_time + 1000): + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) + + if CS.imperial_speed_units: + # Imperial unit cars adjust cruise in units of 1 and 5 mph. + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + else: + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + + # Reduce cruise speed significantly if necessary. Multiply by a % to + # make the car slightly more eager to slow down vs speed up. + if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: + # How much we can accelerate without exceeding max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + if speed_offset > full_press_kph and full_press_kph < available_speed: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset > half_press_kph and half_press_kph < available_speed: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + return button_to_press # function to calculate the cruise button based on a safe follow distance def calc_follow_button(self, CS): diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 8a734726284d38..ba552250b7835f 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -245,7 +245,11 @@ def update_ui_buttons(self,id,btn_status): if (id == 1) and (btn_status == 0): # don't change status, just model current_mode = self.cstm_btns.btns[id].btn_label2 - self.cstm_btns.btns[id].btn_label2 = ACCModes[current_mode].next_mode + if current_mode in ACCModes: + next_mode = ACCModes[current_mode].next_mode + else: + next_mode = ACCModes.values()[0] + self.cstm_btns.btns[id].btn_label2 = next_mode else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index f9ff280748a494..8c180b53bea5eb 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -104,8 +104,8 @@ def __init__(self, name, autoresume, next_mode): ACCModes = { - "OP": ACCMode(name="OP", autoresume=False, next_mode="JJ"), - "JJ": ACCMode(name="JJ", autoresume=False, next_mode="AutoOP"), - "AutoOP": ACCMode(name="AutoOP", autoresume=True, next_mode="AutoJJ"), - "AutoJJ": ACCMode(name="AutoJJ", autoresume=True, next_mode="OP") + #"OP": ACCMode(name="OP", autoresume=False, next_mode="JJ"), + "FOLLOW": ACCMode(name="FOLLOW", autoresume=False, next_mode="AUTO"), + #"AutoOP": ACCMode(name="AutoOP", autoresume=True, next_mode="AutoJJ"), + "AUTO": ACCMode(name="AUTO", autoresume=True, next_mode="FOLLOW") } \ No newline at end of file From 1a429d595fd73f19b7940ecc11ba8660f770f434 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 19:41:25 +0000 Subject: [PATCH 08/17] Remove ACC single-pull-to-disable --- selfdrive/car/tesla/ACC_module.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index ac75d780465815..5ef10a22e9da1c 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -62,9 +62,6 @@ def update_stat(self, CS, enabled): self.enabled_time = curr_time_ms # Increase ACC speed to match current, if applicable. self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) - else: - # A single pull disables ACC (falling back to just steering). - self.enable_adaptive_cruise = False # Handle pressing the cancel button. elif CS.cruise_buttons == CruiseButtons.CANCEL: self.enable_adaptive_cruise = False From 7545dd0615571d232019d37c234a549bbee5ce59 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 19:59:58 +0000 Subject: [PATCH 09/17] User steering will disable ACC --- selfdrive/car/tesla/ACC_module.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 5ef10a22e9da1c..cc2b811e04f4c7 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -88,11 +88,14 @@ def update_stat(self, CS, enabled): # Clip ACC speed between 0 and 170 KPH. self.acc_speed_kph = min(self.acc_speed_kph, 170) self.acc_speed_kph = max(self.acc_speed_kph, 0) - # If something disabled cruise control, disable ACC too. - elif (self.prev_pcm_acc_status == 2 and - CS.pcm_acc_status != 2 and - not self.autoresume): - self.enable_adaptive_cruise = False + # If autoresume is not enabled, certain user actions disable ACC. + elif not self.autoresume: + # If something disabled cruise control (braking), disable ACC too. + if self.prev_pcm_acc_status == 2 and CS.pcm_acc_status != 2: + self.enable_adaptive_cruise = False + # if user took over steering, disable ACC too. + elif not enabled: + self.enable_adaptive_cruise = False # Notify if ACC was toggled if prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: From c5c4b1e9968f0e697e6f447bcba5a32530996677 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 20:35:46 +0000 Subject: [PATCH 10/17] Remove unused ACC buttons --- selfdrive/car/tesla/ACC_module.py | 4 ++-- selfdrive/car/tesla/values.py | 6 ++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index cc2b811e04f4c7..01d7ca23d2f159 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -134,7 +134,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): and current_time_ms > self.automated_cruise_action_time + 500): if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: - button_to_press = self.cal_op_button(CS, pcm_speed, current_time_ms) + button_to_press = self.calc_op_button(CS, pcm_speed, current_time_ms) elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: # Alternative speed decision logic that uses the lead car's distance # and speed more directly. @@ -164,7 +164,7 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): return button_to_press # Adjust speed based off OP's longitudinal model. - def cal_op_button(self, CS, pcm_speed, current_time_ms): + def calc_op_button(self, CS, pcm_speed, current_time_ms): # Automatically engange traditional cruise if it is idle and we are # going fast enough and we are accelerating. if (CS.pcm_acc_status == 1 diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 8c180b53bea5eb..e1ed814a71d6ba 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -104,8 +104,6 @@ def __init__(self, name, autoresume, next_mode): ACCModes = { - #"OP": ACCMode(name="OP", autoresume=False, next_mode="JJ"), - "FOLLOW": ACCMode(name="FOLLOW", autoresume=False, next_mode="AUTO"), - #"AutoOP": ACCMode(name="AutoOP", autoresume=True, next_mode="AutoJJ"), - "AUTO": ACCMode(name="AUTO", autoresume=True, next_mode="FOLLOW") + "FOLLOW": ACCMode(name="FOLLOW", autoresume=False, next_mode="AUTO"), + "AUTO": ACCMode(name="AUTO", autoresume=True, next_mode="FOLLOW") } \ No newline at end of file From 3dd1ed06bca05f467f6dd12b448af6e1ad2fd268 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 21:43:02 +0000 Subject: [PATCH 11/17] ACC comments --- selfdrive/car/tesla/ACC_module.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 01d7ca23d2f159..81952248c312ef 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -37,6 +37,9 @@ def __init__(self, carcontroller): self.prev_pcm_acc_status = 0 self.acc_speed_kph = 0. + # Updates the internal state of this controller based on user input, + # specifically the steering wheel mounted cruise control stalk, and OpenPilot + # UI buttons. def update_stat(self, CS, enabled): # Check if the cruise stalk was double pulled, indicating that adaptive # cruise control should be enabled. Twice in .75 seconds counts as a double @@ -118,6 +121,8 @@ def update_stat(self, CS, enabled): self.prev_cruise_buttons = CS.cruise_buttons self.prev_pcm_acc_status = CS.pcm_acc_status + # Decide which cruise control buttons to simluate to get the car to the + # desired speed. def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # Adaptive cruise control current_time_ms = _current_time_millis() From 98f05900669ce9f3ac33849e9424c516863fdae9 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 23:12:54 +0000 Subject: [PATCH 12/17] More tolerant ACC auto-engage --- selfdrive/car/tesla/ACC_module.py | 35 ++++++++++++++++++------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 81952248c312ef..940f9b3f44677e 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -170,13 +170,9 @@ def update_acc(self, enabled, CS, frame, actuators, pcm_speed): # Adjust speed based off OP's longitudinal model. def calc_op_button(self, CS, pcm_speed, current_time_ms): - # Automatically engange traditional cruise if it is idle and we are - # going fast enough and we are accelerating. - if (CS.pcm_acc_status == 1 - and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego >= 0.05 - and self.autoresume): - button_to_press = CruiseButtons.DECEL_2ND + # Automatically engange traditional cruise if ACC is active. + if self.should_autoengage_cc(CS, current_time_ms): + button_to_press = CruiseButtons.RES_ACCEL # If traditional cruise is engaged, then control it. elif (CS.pcm_acc_status == 2 # But don't make adjustments if a human has manually done so in @@ -216,6 +212,16 @@ def calc_op_button(self, CS, pcm_speed, current_time_ms): # Send cruise stalk up_1st. button_to_press = CruiseButtons.RES_ACCEL return button_to_press + + def should_autoengage_cc(self, CS, current_time_ms): + autoresume_supressed_by_braking = ( + self.autoresume + and CS.a_ego < 0 + and current_time_ms > self.enabled_time + 300) + return (CS.pcm_acc_status == 1 + and self.enable_adaptive_cruise + and CS.v_ego >= self.MIN_CRUISE_SPEED_MS + and not autoresume_supressed_by_braking) # function to calculate the cruise button based on a safe follow distance def calc_follow_button(self, CS): @@ -250,14 +256,13 @@ def calc_follow_button(self, CS): ### Logic to determine best cruise speed ### - # Automatically engange traditional cruise if it is idle and we are - # going fast enough and accelerating. - if (CS.pcm_acc_status == 1 - and self.enable_adaptive_cruise - and CS.v_ego > self.MIN_CRUISE_SPEED_MS - and CS.a_ego > 0.05 - and self.autoresume): - button = CruiseButtons.DECEL_SET + # Automatically engange traditional cruise if ACC is active. + braking_supresses_autoresume = ( + self.autoresume + and CS.a_ego < 0 + and current_time_ms > self.enabled_time + 300) + if self.should_autoengage_cc(CS, current_time_ms): + button = CruiseButtons.RES_ACCEL # If traditional cruise is engaged, then control it. elif CS.pcm_acc_status == 2: # if cruise is set to faster than the max speed, slow down From bbce310f6e470de4657cbdc7508de4a02b09391f Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 23:55:40 +0000 Subject: [PATCH 13/17] Fix crash if old button pickle is present --- selfdrive/car/tesla/ACC_module.py | 8 +++++--- selfdrive/car/tesla/carstate.py | 7 ++----- selfdrive/car/tesla/values.py | 10 ++++++++-- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 940f9b3f44677e..0c785330d3d362 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,5 +1,5 @@ from selfdrive.services import service_list -from selfdrive.car.tesla.values import AH, CruiseButtons, CruiseState, CAR, ACCModes, ACCState +from selfdrive.car.tesla.values import AH, CruiseButtons, CruiseState, CAR, GetAccMode from selfdrive.config import Conversions as CV import selfdrive.messaging as messaging import os @@ -45,8 +45,10 @@ def update_stat(self, CS, enabled): # cruise control should be enabled. Twice in .75 seconds counts as a double # pull. prev_enable_adaptive_cruise = self.enable_adaptive_cruise - acc_mode = CS.cstm_btns.get_button_label2("acc") - self.autoresume = ACCModes[acc_mode].autoresume + acc_string = CS.cstm_btns.get_button_label2("acc") + acc_mode = GetAccMode(acc_string) + CS.cstm_btns.get_button("acc").btn_label2 = acc_mode.name + self.autoresume = acc_mode.autoresume curr_time_ms = _current_time_millis() speed_uom_kph = 1. if CS.imperial_speed_units: diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index ba552250b7835f..fc7fb221e2af36 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -2,7 +2,7 @@ from common.kalman.simple_kalman import KF1D from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC, ACCModes +from selfdrive.car.tesla.values import CAR, CruiseButtons, DBC, GetAccMode from selfdrive.car.modules.UIBT_module import UIButtons, UIButton import numpy as np from ctypes import create_string_buffer @@ -245,10 +245,7 @@ def update_ui_buttons(self,id,btn_status): if (id == 1) and (btn_status == 0): # don't change status, just model current_mode = self.cstm_btns.btns[id].btn_label2 - if current_mode in ACCModes: - next_mode = ACCModes[current_mode].next_mode - else: - next_mode = ACCModes.values()[0] + next_mode = GetAccMode(current_mode).next_mode self.cstm_btns.btns[id].btn_label2 = next_mode else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index e1ed814a71d6ba..aa46f80f76429a 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -103,7 +103,13 @@ def __init__(self, name, autoresume, next_mode): self.next_mode = next_mode -ACCModes = { +_ACCModes = { "FOLLOW": ACCMode(name="FOLLOW", autoresume=False, next_mode="AUTO"), "AUTO": ACCMode(name="AUTO", autoresume=True, next_mode="FOLLOW") -} \ No newline at end of file +} + +def GetAccMode(name): + if name in _ACCModes: + return _ACCModes[name] + else: + return _ACCModes.values()[0] \ No newline at end of file From aa172d03734c76a0d07a7927fbee76e766ec9af4 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Wed, 29 Aug 2018 23:58:29 +0000 Subject: [PATCH 14/17] Missing ACC import --- selfdrive/car/tesla/ACC_module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 0c785330d3d362..1800d7085229cd 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,5 +1,5 @@ from selfdrive.services import service_list -from selfdrive.car.tesla.values import AH, CruiseButtons, CruiseState, CAR, GetAccMode +from selfdrive.car.tesla.values import ACCState, AH, CruiseButtons, CruiseState, CAR, GetAccMode from selfdrive.config import Conversions as CV import selfdrive.messaging as messaging import os From ad019a0f01a76d8302070a4a5611315cccd0d2bf Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Thu, 30 Aug 2018 00:38:31 +0000 Subject: [PATCH 15/17] Don't allow ACC engage below min speed --- selfdrive/car/tesla/ACC_module.py | 667 +++++++++++++++--------------- 1 file changed, 331 insertions(+), 336 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 1800d7085229cd..e36a13beccde1c 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,337 +1,332 @@ -from selfdrive.services import service_list -from selfdrive.car.tesla.values import ACCState, AH, CruiseButtons, CruiseState, CAR, GetAccMode -from selfdrive.config import Conversions as CV -import selfdrive.messaging as messaging -import os -import subprocess -import time -import zmq - - -def _current_time_millis(): - return int(round(time.time() * 1000)) - - -class ACCController(object): - - # Tesla cruise only functions above 17 MPH - MIN_CRUISE_SPEED_MS = 17.5 * CV.MPH_TO_MS - - def __init__(self, carcontroller): - self.CC = carcontroller - self.human_cruise_action_time = 0 - self.automated_cruise_action_time = 0 - self.enabled_time = 0 - self.last_angle = 0. - context = zmq.Context() - self.poller = zmq.Poller() - self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) - self.lead_1 = None - self.last_update_time = 0 - self.enable_adaptive_cruise = False - # Whether to re-engage automatically after being paused due to low speed or - # user-initated deceleration. - self.autoresume = False - self.last_cruise_stalk_pull_time = 0 - self.prev_cruise_buttons = CruiseButtons.IDLE - self.prev_pcm_acc_status = 0 - self.acc_speed_kph = 0. - - # Updates the internal state of this controller based on user input, - # specifically the steering wheel mounted cruise control stalk, and OpenPilot - # UI buttons. - def update_stat(self, CS, enabled): - # Check if the cruise stalk was double pulled, indicating that adaptive - # cruise control should be enabled. Twice in .75 seconds counts as a double - # pull. - prev_enable_adaptive_cruise = self.enable_adaptive_cruise - acc_string = CS.cstm_btns.get_button_label2("acc") - acc_mode = GetAccMode(acc_string) - CS.cstm_btns.get_button("acc").btn_label2 = acc_mode.name - self.autoresume = acc_mode.autoresume - curr_time_ms = _current_time_millis() - speed_uom_kph = 1. - if CS.imperial_speed_units: - speed_uom_kph = CV.MPH_TO_KPH - # Handle pressing the enable button. - if (CS.cruise_buttons == CruiseButtons.MAIN and - self.prev_cruise_buttons != CruiseButtons.MAIN): - double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 - self.last_cruise_stalk_pull_time = curr_time_ms - ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF and - enabled and - CruiseState.is_enabled_or_standby(CS.pcm_acc_status)) - if ready and double_pull: - # A double pull enables ACC. updating the max ACC speed if necessary. - self.enable_adaptive_cruise = True - self.enabled_time = curr_time_ms - # Increase ACC speed to match current, if applicable. - self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) - # Handle pressing the cancel button. - elif CS.cruise_buttons == CruiseButtons.CANCEL: - self.enable_adaptive_cruise = False - self.acc_speed_kph = 0. - self.last_cruise_stalk_pull_time = 0 - # Handle pressing up and down buttons. - elif (self.enable_adaptive_cruise and - CS.cruise_buttons != self.prev_cruise_buttons): - # Real stalk command while ACC is already enabled. Adjust the max ACC - # speed if necessary. For example if max speed is 50 but you're currently - # only going 30, the cruise speed can be increased without any change to - # max ACC speed. If actual speed is already 50, the code also increases - # the max cruise speed. - if CS.cruise_buttons == CruiseButtons.RES_ACCEL: - requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph - self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) - elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: - requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph - self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) - elif CS.cruise_buttons == CruiseButtons.DECEL_SET: - self.acc_speed_kph -= speed_uom_kph - elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: - self.acc_speed_kph -= 5 * speed_uom_kph - # Clip ACC speed between 0 and 170 KPH. - self.acc_speed_kph = min(self.acc_speed_kph, 170) - self.acc_speed_kph = max(self.acc_speed_kph, 0) - # If autoresume is not enabled, certain user actions disable ACC. - elif not self.autoresume: - # If something disabled cruise control (braking), disable ACC too. - if self.prev_pcm_acc_status == 2 and CS.pcm_acc_status != 2: - self.enable_adaptive_cruise = False - # if user took over steering, disable ACC too. - elif not enabled: - self.enable_adaptive_cruise = False - - # Notify if ACC was toggled - if prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: - CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4) - CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) - elif self.enable_adaptive_cruise and not prev_enable_adaptive_cruise: - CS.UE.custom_alert_message(2, "ACC Enabled", 150) - CS.cstm_btns.set_button_status("acc", ACCState.ENABLED) - - # Update the UI to show whether the current car state allows ACC. - if CS.cstm_btns.get_button_status("acc") in [ACCState.STANDBY, ACCState.NOT_READY]: - if (enabled - and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) - and CS.v_ego > self.MIN_CRUISE_SPEED_MS): - CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) - else: - CS.cstm_btns.set_button_status("acc", ACCState.NOT_READY) - - # Update prev state after all other actions. - self.prev_cruise_buttons = CS.cruise_buttons - self.prev_pcm_acc_status = CS.pcm_acc_status - - # Decide which cruise control buttons to simluate to get the car to the - # desired speed. - def update_acc(self, enabled, CS, frame, actuators, pcm_speed): - # Adaptive cruise control - current_time_ms = _current_time_millis() - if CruiseButtons.should_be_throttled(CS.cruise_buttons): - self.human_cruise_action_time = current_time_ms - button_to_press = None - - if (self.enable_adaptive_cruise - # Only do ACC if OP is steering - and enabled - # And adjust infrequently, since sending repeated adjustments makes - # the car think we're doing a 'long press' on the cruise stalk, - # resulting in small, jerky speed adjustments. - and current_time_ms > self.automated_cruise_action_time + 500): - - if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: - button_to_press = self.calc_op_button(CS, pcm_speed, current_time_ms) - elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: - # Alternative speed decision logic that uses the lead car's distance - # and speed more directly. - # Bring in the lead car distance from the Live20 feed - l20 = None - if enabled: - for socket, _ in self.poller.poll(0): - if socket is self.live20: - l20 = messaging.recv_one(socket) - break - if l20 is not None: - self.lead_1 = l20.live20.leadOne - button_to_press = self.calc_follow_button(CS) - if button_to_press: - self.automated_cruise_action_time = current_time_ms - # If trying to slow below the min cruise speed, just cancel cruise. - # This prevents a SCCM crash which is triggered by repeatedly pressing - # stalk-down when already at min cruise speed. - if (CruiseButtons.is_decel(button_to_press) - and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): - button_to_press = CruiseButtons.CANCEL - # Debug logging (disable in production to reduce latency of commands) - #print "***ACC command: %s***" % button_to_press - #elif (current_time_ms > self.last_update_time + 1000): - # self.last_update_time = current_time_ms - # print "Desired ACC speed change: %s" % (speed_offset) - return button_to_press - - # Adjust speed based off OP's longitudinal model. - def calc_op_button(self, CS, pcm_speed, current_time_ms): - # Automatically engange traditional cruise if ACC is active. - if self.should_autoengage_cc(CS, current_time_ms): - button_to_press = CruiseButtons.RES_ACCEL - # If traditional cruise is engaged, then control it. - elif (CS.pcm_acc_status == 2 - # But don't make adjustments if a human has manually done so in - # the last 3 seconds. Human intention should not be overridden. - and current_time_ms > self.human_cruise_action_time + 3000 - and current_time_ms > self.enabled_time + 1000): - # The difference between OP's target speed and the current cruise - # control speed, in KPH. - speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) - - if CS.imperial_speed_units: - # Imperial unit cars adjust cruise in units of 1 and 5 mph. - half_press_kph = 1 * CV.MPH_TO_KPH - full_press_kph = 5 * CV.MPH_TO_KPH - else: - # Metric cars adjust cruise in units of 1 and 5 kph. - half_press_kph = 1 - full_press_kph = 5 - - # Reduce cruise speed significantly if necessary. Multiply by a % to - # make the car slightly more eager to slow down vs speed up. - if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_2nd. - button_to_press = CruiseButtons.DECEL_2ND - # Reduce speed slightly if necessary. - elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_1st. - button_to_press = CruiseButtons.DECEL_SET - # Increase cruise speed if possible. - elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: - # How much we can accelerate without exceeding max allowed speed. - available_speed = self.acc_speed_kph - CS.v_cruise_actual - if speed_offset > full_press_kph and full_press_kph < available_speed: - # Send cruise stalk up_2nd. - button_to_press = CruiseButtons.RES_ACCEL_2ND - elif speed_offset > half_press_kph and half_press_kph < available_speed: - # Send cruise stalk up_1st. - button_to_press = CruiseButtons.RES_ACCEL - return button_to_press - - def should_autoengage_cc(self, CS, current_time_ms): - autoresume_supressed_by_braking = ( - self.autoresume - and CS.a_ego < 0 - and current_time_ms > self.enabled_time + 300) - return (CS.pcm_acc_status == 1 - and self.enable_adaptive_cruise - and CS.v_ego >= self.MIN_CRUISE_SPEED_MS - and not autoresume_supressed_by_braking) - - # function to calculate the cruise button based on a safe follow distance - def calc_follow_button(self, CS): - follow_time = 2.0 # in seconds - current_time_ms = _current_time_millis() - # Make sure we were able to populate lead_1. - if self.lead_1 is None: - return None - # dRel is in meters. - lead_dist = self.lead_1.dRel - # Grab the relative speed. - rel_speed = self.lead_1.vRel * CV.MS_TO_KPH - # Current speed in kph - cur_speed = CS.v_ego * CV.MS_TO_KPH - # v_ego is in m/s, so safe_dist_mance is in meters. - safe_dist_m = CS.v_ego * follow_time - # How much we can accelerate without exceeding the max allowed speed. - available_speed = self.acc_speed_kph - CS.v_cruise_actual - # Metric cars adjust cruise in units of 1 and 5 kph. - half_press_kph = 1 - full_press_kph = 5 - # Imperial unit cars adjust cruise in units of 1 and 5 mph - if CS.imperial_speed_units: - half_press_kph = 1 * CV.MPH_TO_KPH - full_press_kph = 5 * CV.MPH_TO_KPH - # button to issue - button = None - # debug msg - msg = None - - #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK - - ### Logic to determine best cruise speed ### - - # Automatically engange traditional cruise if ACC is active. - braking_supresses_autoresume = ( - self.autoresume - and CS.a_ego < 0 - and current_time_ms > self.enabled_time + 300) - if self.should_autoengage_cc(CS, current_time_ms): - button = CruiseButtons.RES_ACCEL - # If traditional cruise is engaged, then control it. - elif CS.pcm_acc_status == 2: - # if cruise is set to faster than the max speed, slow down - if CS.v_cruise_actual > self.acc_speed_kph: - msg = "Slow to max" - button = CruiseButtons.DECEL_SET - # If lead_dist is reported as 0, no one is detected in front of you so you - # can speed up don't speed up when steer-angle > 2; vision radar often - # loses lead car in a turn. - elif lead_dist == 0 and self.enable_adaptive_cruise and CS.angle_steers < 2.0: - if full_press_kph < available_speed: - msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL_2ND - elif half_press_kph < available_speed: - msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL - - # if we have a populated lead_distance - elif (lead_dist > 0 - # and we only issue commands every 300ms - and current_time_ms > self.automated_cruise_action_time + 300): - ### Slowing down ### - # Reduce speed significantly if lead_dist < 50% of safe dist, no matter - # the rel_speed - if CS.v_cruise_actual > full_press_kph: - if lead_dist < (safe_dist_m * 0.3) and rel_speed < 2: - msg = "50pct down" - button = CruiseButtons.DECEL_2ND - # Reduce speed significantly if lead_dist < 60% of safe dist - # and if the lead car isn't pulling away - elif lead_dist < (safe_dist_m * 0.5) and rel_speed < 0: - msg = "70pct down" - button = CruiseButtons.DECEL_SET - #Reduce speed if rel_speed < -15kph so you don't rush up to lead car - elif rel_speed < -15: - msg = "relspd -15 down" - button = CruiseButtons.DECEL_SET - # we're close to the safe distance, so make slow adjustments - # only adjust every 1 secs - elif (lead_dist < (safe_dist_m * 0.9) and rel_speed < 0 - and current_time_ms > self.automated_cruise_action_time + 1000): - msg = "90pct down" - button = CruiseButtons.DECEL_SET - - ### Speed up ### - # don't speed up again until you have more than a safe distance in front - # only adjust every 2 sec - elif ((lead_dist > (safe_dist_m * 0.8) or rel_speed > 5) and half_press_kph < available_speed - and current_time_ms > self.automated_cruise_action_time + 100): - msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL - - # if we don't need to do any of the above, then we're at a pretty good - # speed make sure if we're at this point that the set cruise speed isn't - # set too low or high - if (cur_speed - CS.v_cruise_actual) > 5 and button == None: - # Send cruise stalk up_1st if the set speed is too low to bring it up - msg = "cruise rectify" - button = CruiseButtons.RES_ACCEL - - if (current_time_ms > self.last_update_time + 1000): - ratio = 0 - if safe_dist_m > 0: - ratio = (lead_dist / safe_dist_m) * 100 - print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) - self.last_update_time = current_time_ms - if msg != None: - print msg - +from selfdrive.services import service_list +from selfdrive.car.tesla.values import ACCState, AH, CruiseButtons, CruiseState, CAR, GetAccMode +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import os +import subprocess +import time +import zmq + + +def _current_time_millis(): + return int(round(time.time() * 1000)) + + +class ACCController(object): + + # Tesla cruise only functions above 17 MPH + MIN_CRUISE_SPEED_MS = 17.5 * CV.MPH_TO_MS + + def __init__(self, carcontroller): + self.CC = carcontroller + self.human_cruise_action_time = 0 + self.automated_cruise_action_time = 0 + self.enabled_time = 0 + self.last_angle = 0. + context = zmq.Context() + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.lead_1 = None + self.last_update_time = 0 + self.enable_adaptive_cruise = False + # Whether to re-engage automatically after being paused due to low speed or + # user-initated deceleration. + self.autoresume = False + self.last_cruise_stalk_pull_time = 0 + self.prev_cruise_buttons = CruiseButtons.IDLE + self.prev_pcm_acc_status = 0 + self.acc_speed_kph = 0. + + # Updates the internal state of this controller based on user input, + # specifically the steering wheel mounted cruise control stalk, and OpenPilot + # UI buttons. + def update_stat(self, CS, enabled): + # Check if the cruise stalk was double pulled, indicating that adaptive + # cruise control should be enabled. Twice in .75 seconds counts as a double + # pull. + prev_enable_adaptive_cruise = self.enable_adaptive_cruise + acc_string = CS.cstm_btns.get_button_label2("acc") + acc_mode = GetAccMode(acc_string) + CS.cstm_btns.get_button("acc").btn_label2 = acc_mode.name + self.autoresume = acc_mode.autoresume + curr_time_ms = _current_time_millis() + speed_uom_kph = 1. + if CS.imperial_speed_units: + speed_uom_kph = CV.MPH_TO_KPH + # Handle pressing the enable button. + if (CS.cruise_buttons == CruiseButtons.MAIN and + self.prev_cruise_buttons != CruiseButtons.MAIN): + double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 + self.last_cruise_stalk_pull_time = curr_time_ms + ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF + and enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS) + if ready and double_pull: + # A double pull enables ACC. updating the max ACC speed if necessary. + self.enable_adaptive_cruise = True + self.enabled_time = curr_time_ms + # Increase ACC speed to match current, if applicable. + self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) + # Handle pressing the cancel button. + elif CS.cruise_buttons == CruiseButtons.CANCEL: + self.enable_adaptive_cruise = False + self.acc_speed_kph = 0. + self.last_cruise_stalk_pull_time = 0 + # Handle pressing up and down buttons. + elif (self.enable_adaptive_cruise and + CS.cruise_buttons != self.prev_cruise_buttons): + # Real stalk command while ACC is already enabled. Adjust the max ACC + # speed if necessary. For example if max speed is 50 but you're currently + # only going 30, the cruise speed can be increased without any change to + # max ACC speed. If actual speed is already 50, the code also increases + # the max cruise speed. + if CS.cruise_buttons == CruiseButtons.RES_ACCEL: + requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph + self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) + elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: + requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph + self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) + elif CS.cruise_buttons == CruiseButtons.DECEL_SET: + self.acc_speed_kph -= speed_uom_kph + elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: + self.acc_speed_kph -= 5 * speed_uom_kph + # Clip ACC speed between 0 and 170 KPH. + self.acc_speed_kph = min(self.acc_speed_kph, 170) + self.acc_speed_kph = max(self.acc_speed_kph, 0) + # If autoresume is not enabled, certain user actions disable ACC. + elif not self.autoresume: + # If something disabled cruise control (braking), disable ACC too. + if self.prev_pcm_acc_status == 2 and CS.pcm_acc_status != 2: + self.enable_adaptive_cruise = False + # if user took over steering, disable ACC too. + elif not enabled: + self.enable_adaptive_cruise = False + + # Notify if ACC was toggled + if prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: + CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4) + CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) + elif self.enable_adaptive_cruise and not prev_enable_adaptive_cruise: + CS.UE.custom_alert_message(2, "ACC Enabled", 150) + CS.cstm_btns.set_button_status("acc", ACCState.ENABLED) + + # Update the UI to show whether the current car state allows ACC. + if CS.cstm_btns.get_button_status("acc") in [ACCState.STANDBY, ACCState.NOT_READY]: + if (enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS): + CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) + else: + CS.cstm_btns.set_button_status("acc", ACCState.NOT_READY) + + # Update prev state after all other actions. + self.prev_cruise_buttons = CS.cruise_buttons + self.prev_pcm_acc_status = CS.pcm_acc_status + + # Decide which cruise control buttons to simluate to get the car to the + # desired speed. + def update_acc(self, enabled, CS, frame, actuators, pcm_speed): + # Adaptive cruise control + current_time_ms = _current_time_millis() + if CruiseButtons.should_be_throttled(CS.cruise_buttons): + self.human_cruise_action_time = current_time_ms + button_to_press = None + + if (self.enable_adaptive_cruise + # Only do ACC if OP is steering + and enabled + # And adjust infrequently, since sending repeated adjustments makes + # the car think we're doing a 'long press' on the cruise stalk, + # resulting in small, jerky speed adjustments. + and current_time_ms > self.automated_cruise_action_time + 500): + + if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: + button_to_press = self.calc_op_button(CS, pcm_speed, current_time_ms) + elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: + # Alternative speed decision logic that uses the lead car's distance + # and speed more directly. + # Bring in the lead car distance from the Live20 feed + l20 = None + if enabled: + for socket, _ in self.poller.poll(0): + if socket is self.live20: + l20 = messaging.recv_one(socket) + break + if l20 is not None: + self.lead_1 = l20.live20.leadOne + button_to_press = self.calc_follow_button(CS) + if button_to_press: + self.automated_cruise_action_time = current_time_ms + # If trying to slow below the min cruise speed, just cancel cruise. + # This prevents a SCCM crash which is triggered by repeatedly pressing + # stalk-down when already at min cruise speed. + if (CruiseButtons.is_decel(button_to_press) + and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): + button_to_press = CruiseButtons.CANCEL + # Debug logging (disable in production to reduce latency of commands) + #print "***ACC command: %s***" % button_to_press + #elif (current_time_ms > self.last_update_time + 1000): + # self.last_update_time = current_time_ms + # print "Desired ACC speed change: %s" % (speed_offset) + return button_to_press + + # Adjust speed based off OP's longitudinal model. + def calc_op_button(self, CS, pcm_speed, current_time_ms): + # Automatically engange traditional cruise if ACC is active. + if self.should_autoengage_cc(CS, current_time_ms): + button_to_press = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == 2 + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and current_time_ms > self.human_cruise_action_time + 3000 + and current_time_ms > self.enabled_time + 1000): + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) + + if CS.imperial_speed_units: + # Imperial unit cars adjust cruise in units of 1 and 5 mph. + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + else: + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + + # Reduce cruise speed significantly if necessary. Multiply by a % to + # make the car slightly more eager to slow down vs speed up. + if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: + # How much we can accelerate without exceeding max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + if speed_offset > full_press_kph and full_press_kph < available_speed: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset > half_press_kph and half_press_kph < available_speed: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + return button_to_press + + def should_autoengage_cc(self, CS, current_time_ms): + autoresume_supressed_by_braking = ( + self.autoresume + and CS.a_ego < 0 + and current_time_ms > self.enabled_time + 300) + return (CS.pcm_acc_status == 1 + and self.enable_adaptive_cruise + and CS.v_ego >= self.MIN_CRUISE_SPEED_MS + and not autoresume_supressed_by_braking) + + # function to calculate the cruise button based on a safe follow distance + def calc_follow_button(self, CS): + follow_time = 2.0 # in seconds + current_time_ms = _current_time_millis() + # Make sure we were able to populate lead_1. + if self.lead_1 is None: + return None + # dRel is in meters. + lead_dist = self.lead_1.dRel + # Grab the relative speed. + rel_speed = self.lead_1.vRel * CV.MS_TO_KPH + # Current speed in kph + cur_speed = CS.v_ego * CV.MS_TO_KPH + # v_ego is in m/s, so safe_dist_mance is in meters. + safe_dist_m = CS.v_ego * follow_time + # How much we can accelerate without exceeding the max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + # Imperial unit cars adjust cruise in units of 1 and 5 mph + if CS.imperial_speed_units: + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + # button to issue + button = None + # debug msg + msg = None + + #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK + + ### Logic to determine best cruise speed ### + if self.should_autoengage_cc(CS, current_time_ms): + button = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif CS.pcm_acc_status == 2: + # if cruise is set to faster than the max speed, slow down + if CS.v_cruise_actual > self.acc_speed_kph: + msg = "Slow to max" + button = CruiseButtons.DECEL_SET + # If lead_dist is reported as 0, no one is detected in front of you so you + # can speed up don't speed up when steer-angle > 2; vision radar often + # loses lead car in a turn. + elif lead_dist == 0 and self.enable_adaptive_cruise and CS.angle_steers < 2.0: + if full_press_kph < available_speed: + msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL_2ND + elif half_press_kph < available_speed: + msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + # if we have a populated lead_distance + elif (lead_dist > 0 + # and we only issue commands every 300ms + and current_time_ms > self.automated_cruise_action_time + 300): + ### Slowing down ### + # Reduce speed significantly if lead_dist < 50% of safe dist, no matter + # the rel_speed + if CS.v_cruise_actual > full_press_kph: + if lead_dist < (safe_dist_m * 0.3) and rel_speed < 2: + msg = "50pct down" + button = CruiseButtons.DECEL_2ND + # Reduce speed significantly if lead_dist < 60% of safe dist + # and if the lead car isn't pulling away + elif lead_dist < (safe_dist_m * 0.5) and rel_speed < 0: + msg = "70pct down" + button = CruiseButtons.DECEL_SET + #Reduce speed if rel_speed < -15kph so you don't rush up to lead car + elif rel_speed < -15: + msg = "relspd -15 down" + button = CruiseButtons.DECEL_SET + # we're close to the safe distance, so make slow adjustments + # only adjust every 1 secs + elif (lead_dist < (safe_dist_m * 0.9) and rel_speed < 0 + and current_time_ms > self.automated_cruise_action_time + 1000): + msg = "90pct down" + button = CruiseButtons.DECEL_SET + + ### Speed up ### + # don't speed up again until you have more than a safe distance in front + # only adjust every 2 sec + elif ((lead_dist > (safe_dist_m * 0.8) or rel_speed > 5) and half_press_kph < available_speed + and current_time_ms > self.automated_cruise_action_time + 100): + msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + # if we don't need to do any of the above, then we're at a pretty good + # speed make sure if we're at this point that the set cruise speed isn't + # set too low or high + if (cur_speed - CS.v_cruise_actual) > 5 and button == None: + # Send cruise stalk up_1st if the set speed is too low to bring it up + msg = "cruise rectify" + button = CruiseButtons.RES_ACCEL + + if (current_time_ms > self.last_update_time + 1000): + ratio = 0 + if safe_dist_m > 0: + ratio = (lead_dist / safe_dist_m) * 100 + print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) + self.last_update_time = current_time_ms + if msg != None: + print msg + return button \ No newline at end of file From b2e6df9a4149b2954d208bd0b5d918f657f51694 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Thu, 30 Aug 2018 00:45:39 +0000 Subject: [PATCH 16/17] Revert poorly formatted commit This reverts commit ad019a0f01a76d8302070a4a5611315cccd0d2bf. --- selfdrive/car/tesla/ACC_module.py | 667 +++++++++++++++--------------- 1 file changed, 336 insertions(+), 331 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index e36a13beccde1c..1800d7085229cd 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -1,332 +1,337 @@ -from selfdrive.services import service_list -from selfdrive.car.tesla.values import ACCState, AH, CruiseButtons, CruiseState, CAR, GetAccMode -from selfdrive.config import Conversions as CV -import selfdrive.messaging as messaging -import os -import subprocess -import time -import zmq - - -def _current_time_millis(): - return int(round(time.time() * 1000)) - - -class ACCController(object): - - # Tesla cruise only functions above 17 MPH - MIN_CRUISE_SPEED_MS = 17.5 * CV.MPH_TO_MS - - def __init__(self, carcontroller): - self.CC = carcontroller - self.human_cruise_action_time = 0 - self.automated_cruise_action_time = 0 - self.enabled_time = 0 - self.last_angle = 0. - context = zmq.Context() - self.poller = zmq.Poller() - self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) - self.lead_1 = None - self.last_update_time = 0 - self.enable_adaptive_cruise = False - # Whether to re-engage automatically after being paused due to low speed or - # user-initated deceleration. - self.autoresume = False - self.last_cruise_stalk_pull_time = 0 - self.prev_cruise_buttons = CruiseButtons.IDLE - self.prev_pcm_acc_status = 0 - self.acc_speed_kph = 0. - - # Updates the internal state of this controller based on user input, - # specifically the steering wheel mounted cruise control stalk, and OpenPilot - # UI buttons. - def update_stat(self, CS, enabled): - # Check if the cruise stalk was double pulled, indicating that adaptive - # cruise control should be enabled. Twice in .75 seconds counts as a double - # pull. - prev_enable_adaptive_cruise = self.enable_adaptive_cruise - acc_string = CS.cstm_btns.get_button_label2("acc") - acc_mode = GetAccMode(acc_string) - CS.cstm_btns.get_button("acc").btn_label2 = acc_mode.name - self.autoresume = acc_mode.autoresume - curr_time_ms = _current_time_millis() - speed_uom_kph = 1. - if CS.imperial_speed_units: - speed_uom_kph = CV.MPH_TO_KPH - # Handle pressing the enable button. - if (CS.cruise_buttons == CruiseButtons.MAIN and - self.prev_cruise_buttons != CruiseButtons.MAIN): - double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 - self.last_cruise_stalk_pull_time = curr_time_ms - ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF - and enabled - and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) - and CS.v_ego > self.MIN_CRUISE_SPEED_MS) - if ready and double_pull: - # A double pull enables ACC. updating the max ACC speed if necessary. - self.enable_adaptive_cruise = True - self.enabled_time = curr_time_ms - # Increase ACC speed to match current, if applicable. - self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) - # Handle pressing the cancel button. - elif CS.cruise_buttons == CruiseButtons.CANCEL: - self.enable_adaptive_cruise = False - self.acc_speed_kph = 0. - self.last_cruise_stalk_pull_time = 0 - # Handle pressing up and down buttons. - elif (self.enable_adaptive_cruise and - CS.cruise_buttons != self.prev_cruise_buttons): - # Real stalk command while ACC is already enabled. Adjust the max ACC - # speed if necessary. For example if max speed is 50 but you're currently - # only going 30, the cruise speed can be increased without any change to - # max ACC speed. If actual speed is already 50, the code also increases - # the max cruise speed. - if CS.cruise_buttons == CruiseButtons.RES_ACCEL: - requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph - self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) - elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: - requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph - self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) - elif CS.cruise_buttons == CruiseButtons.DECEL_SET: - self.acc_speed_kph -= speed_uom_kph - elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: - self.acc_speed_kph -= 5 * speed_uom_kph - # Clip ACC speed between 0 and 170 KPH. - self.acc_speed_kph = min(self.acc_speed_kph, 170) - self.acc_speed_kph = max(self.acc_speed_kph, 0) - # If autoresume is not enabled, certain user actions disable ACC. - elif not self.autoresume: - # If something disabled cruise control (braking), disable ACC too. - if self.prev_pcm_acc_status == 2 and CS.pcm_acc_status != 2: - self.enable_adaptive_cruise = False - # if user took over steering, disable ACC too. - elif not enabled: - self.enable_adaptive_cruise = False - - # Notify if ACC was toggled - if prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: - CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4) - CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) - elif self.enable_adaptive_cruise and not prev_enable_adaptive_cruise: - CS.UE.custom_alert_message(2, "ACC Enabled", 150) - CS.cstm_btns.set_button_status("acc", ACCState.ENABLED) - - # Update the UI to show whether the current car state allows ACC. - if CS.cstm_btns.get_button_status("acc") in [ACCState.STANDBY, ACCState.NOT_READY]: - if (enabled - and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) - and CS.v_ego > self.MIN_CRUISE_SPEED_MS): - CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) - else: - CS.cstm_btns.set_button_status("acc", ACCState.NOT_READY) - - # Update prev state after all other actions. - self.prev_cruise_buttons = CS.cruise_buttons - self.prev_pcm_acc_status = CS.pcm_acc_status - - # Decide which cruise control buttons to simluate to get the car to the - # desired speed. - def update_acc(self, enabled, CS, frame, actuators, pcm_speed): - # Adaptive cruise control - current_time_ms = _current_time_millis() - if CruiseButtons.should_be_throttled(CS.cruise_buttons): - self.human_cruise_action_time = current_time_ms - button_to_press = None - - if (self.enable_adaptive_cruise - # Only do ACC if OP is steering - and enabled - # And adjust infrequently, since sending repeated adjustments makes - # the car think we're doing a 'long press' on the cruise stalk, - # resulting in small, jerky speed adjustments. - and current_time_ms > self.automated_cruise_action_time + 500): - - if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: - button_to_press = self.calc_op_button(CS, pcm_speed, current_time_ms) - elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: - # Alternative speed decision logic that uses the lead car's distance - # and speed more directly. - # Bring in the lead car distance from the Live20 feed - l20 = None - if enabled: - for socket, _ in self.poller.poll(0): - if socket is self.live20: - l20 = messaging.recv_one(socket) - break - if l20 is not None: - self.lead_1 = l20.live20.leadOne - button_to_press = self.calc_follow_button(CS) - if button_to_press: - self.automated_cruise_action_time = current_time_ms - # If trying to slow below the min cruise speed, just cancel cruise. - # This prevents a SCCM crash which is triggered by repeatedly pressing - # stalk-down when already at min cruise speed. - if (CruiseButtons.is_decel(button_to_press) - and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): - button_to_press = CruiseButtons.CANCEL - # Debug logging (disable in production to reduce latency of commands) - #print "***ACC command: %s***" % button_to_press - #elif (current_time_ms > self.last_update_time + 1000): - # self.last_update_time = current_time_ms - # print "Desired ACC speed change: %s" % (speed_offset) - return button_to_press - - # Adjust speed based off OP's longitudinal model. - def calc_op_button(self, CS, pcm_speed, current_time_ms): - # Automatically engange traditional cruise if ACC is active. - if self.should_autoengage_cc(CS, current_time_ms): - button_to_press = CruiseButtons.RES_ACCEL - # If traditional cruise is engaged, then control it. - elif (CS.pcm_acc_status == 2 - # But don't make adjustments if a human has manually done so in - # the last 3 seconds. Human intention should not be overridden. - and current_time_ms > self.human_cruise_action_time + 3000 - and current_time_ms > self.enabled_time + 1000): - # The difference between OP's target speed and the current cruise - # control speed, in KPH. - speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) - - if CS.imperial_speed_units: - # Imperial unit cars adjust cruise in units of 1 and 5 mph. - half_press_kph = 1 * CV.MPH_TO_KPH - full_press_kph = 5 * CV.MPH_TO_KPH - else: - # Metric cars adjust cruise in units of 1 and 5 kph. - half_press_kph = 1 - full_press_kph = 5 - - # Reduce cruise speed significantly if necessary. Multiply by a % to - # make the car slightly more eager to slow down vs speed up. - if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_2nd. - button_to_press = CruiseButtons.DECEL_2ND - # Reduce speed slightly if necessary. - elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: - # Send cruise stalk dn_1st. - button_to_press = CruiseButtons.DECEL_SET - # Increase cruise speed if possible. - elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: - # How much we can accelerate without exceeding max allowed speed. - available_speed = self.acc_speed_kph - CS.v_cruise_actual - if speed_offset > full_press_kph and full_press_kph < available_speed: - # Send cruise stalk up_2nd. - button_to_press = CruiseButtons.RES_ACCEL_2ND - elif speed_offset > half_press_kph and half_press_kph < available_speed: - # Send cruise stalk up_1st. - button_to_press = CruiseButtons.RES_ACCEL - return button_to_press - - def should_autoengage_cc(self, CS, current_time_ms): - autoresume_supressed_by_braking = ( - self.autoresume - and CS.a_ego < 0 - and current_time_ms > self.enabled_time + 300) - return (CS.pcm_acc_status == 1 - and self.enable_adaptive_cruise - and CS.v_ego >= self.MIN_CRUISE_SPEED_MS - and not autoresume_supressed_by_braking) - - # function to calculate the cruise button based on a safe follow distance - def calc_follow_button(self, CS): - follow_time = 2.0 # in seconds - current_time_ms = _current_time_millis() - # Make sure we were able to populate lead_1. - if self.lead_1 is None: - return None - # dRel is in meters. - lead_dist = self.lead_1.dRel - # Grab the relative speed. - rel_speed = self.lead_1.vRel * CV.MS_TO_KPH - # Current speed in kph - cur_speed = CS.v_ego * CV.MS_TO_KPH - # v_ego is in m/s, so safe_dist_mance is in meters. - safe_dist_m = CS.v_ego * follow_time - # How much we can accelerate without exceeding the max allowed speed. - available_speed = self.acc_speed_kph - CS.v_cruise_actual - # Metric cars adjust cruise in units of 1 and 5 kph. - half_press_kph = 1 - full_press_kph = 5 - # Imperial unit cars adjust cruise in units of 1 and 5 mph - if CS.imperial_speed_units: - half_press_kph = 1 * CV.MPH_TO_KPH - full_press_kph = 5 * CV.MPH_TO_KPH - # button to issue - button = None - # debug msg - msg = None - - #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK - - ### Logic to determine best cruise speed ### - if self.should_autoengage_cc(CS, current_time_ms): - button = CruiseButtons.RES_ACCEL - # If traditional cruise is engaged, then control it. - elif CS.pcm_acc_status == 2: - # if cruise is set to faster than the max speed, slow down - if CS.v_cruise_actual > self.acc_speed_kph: - msg = "Slow to max" - button = CruiseButtons.DECEL_SET - # If lead_dist is reported as 0, no one is detected in front of you so you - # can speed up don't speed up when steer-angle > 2; vision radar often - # loses lead car in a turn. - elif lead_dist == 0 and self.enable_adaptive_cruise and CS.angle_steers < 2.0: - if full_press_kph < available_speed: - msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL_2ND - elif half_press_kph < available_speed: - msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL - - # if we have a populated lead_distance - elif (lead_dist > 0 - # and we only issue commands every 300ms - and current_time_ms > self.automated_cruise_action_time + 300): - ### Slowing down ### - # Reduce speed significantly if lead_dist < 50% of safe dist, no matter - # the rel_speed - if CS.v_cruise_actual > full_press_kph: - if lead_dist < (safe_dist_m * 0.3) and rel_speed < 2: - msg = "50pct down" - button = CruiseButtons.DECEL_2ND - # Reduce speed significantly if lead_dist < 60% of safe dist - # and if the lead car isn't pulling away - elif lead_dist < (safe_dist_m * 0.5) and rel_speed < 0: - msg = "70pct down" - button = CruiseButtons.DECEL_SET - #Reduce speed if rel_speed < -15kph so you don't rush up to lead car - elif rel_speed < -15: - msg = "relspd -15 down" - button = CruiseButtons.DECEL_SET - # we're close to the safe distance, so make slow adjustments - # only adjust every 1 secs - elif (lead_dist < (safe_dist_m * 0.9) and rel_speed < 0 - and current_time_ms > self.automated_cruise_action_time + 1000): - msg = "90pct down" - button = CruiseButtons.DECEL_SET - - ### Speed up ### - # don't speed up again until you have more than a safe distance in front - # only adjust every 2 sec - elif ((lead_dist > (safe_dist_m * 0.8) or rel_speed > 5) and half_press_kph < available_speed - and current_time_ms > self.automated_cruise_action_time + 100): - msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) - button = CruiseButtons.RES_ACCEL - - # if we don't need to do any of the above, then we're at a pretty good - # speed make sure if we're at this point that the set cruise speed isn't - # set too low or high - if (cur_speed - CS.v_cruise_actual) > 5 and button == None: - # Send cruise stalk up_1st if the set speed is too low to bring it up - msg = "cruise rectify" - button = CruiseButtons.RES_ACCEL - - if (current_time_ms > self.last_update_time + 1000): - ratio = 0 - if safe_dist_m > 0: - ratio = (lead_dist / safe_dist_m) * 100 - print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) - self.last_update_time = current_time_ms - if msg != None: - print msg - +from selfdrive.services import service_list +from selfdrive.car.tesla.values import ACCState, AH, CruiseButtons, CruiseState, CAR, GetAccMode +from selfdrive.config import Conversions as CV +import selfdrive.messaging as messaging +import os +import subprocess +import time +import zmq + + +def _current_time_millis(): + return int(round(time.time() * 1000)) + + +class ACCController(object): + + # Tesla cruise only functions above 17 MPH + MIN_CRUISE_SPEED_MS = 17.5 * CV.MPH_TO_MS + + def __init__(self, carcontroller): + self.CC = carcontroller + self.human_cruise_action_time = 0 + self.automated_cruise_action_time = 0 + self.enabled_time = 0 + self.last_angle = 0. + context = zmq.Context() + self.poller = zmq.Poller() + self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) + self.lead_1 = None + self.last_update_time = 0 + self.enable_adaptive_cruise = False + # Whether to re-engage automatically after being paused due to low speed or + # user-initated deceleration. + self.autoresume = False + self.last_cruise_stalk_pull_time = 0 + self.prev_cruise_buttons = CruiseButtons.IDLE + self.prev_pcm_acc_status = 0 + self.acc_speed_kph = 0. + + # Updates the internal state of this controller based on user input, + # specifically the steering wheel mounted cruise control stalk, and OpenPilot + # UI buttons. + def update_stat(self, CS, enabled): + # Check if the cruise stalk was double pulled, indicating that adaptive + # cruise control should be enabled. Twice in .75 seconds counts as a double + # pull. + prev_enable_adaptive_cruise = self.enable_adaptive_cruise + acc_string = CS.cstm_btns.get_button_label2("acc") + acc_mode = GetAccMode(acc_string) + CS.cstm_btns.get_button("acc").btn_label2 = acc_mode.name + self.autoresume = acc_mode.autoresume + curr_time_ms = _current_time_millis() + speed_uom_kph = 1. + if CS.imperial_speed_units: + speed_uom_kph = CV.MPH_TO_KPH + # Handle pressing the enable button. + if (CS.cruise_buttons == CruiseButtons.MAIN and + self.prev_cruise_buttons != CruiseButtons.MAIN): + double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 + self.last_cruise_stalk_pull_time = curr_time_ms + ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF and + enabled and + CruiseState.is_enabled_or_standby(CS.pcm_acc_status)) + if ready and double_pull: + # A double pull enables ACC. updating the max ACC speed if necessary. + self.enable_adaptive_cruise = True + self.enabled_time = curr_time_ms + # Increase ACC speed to match current, if applicable. + self.acc_speed_kph = max(CS.v_ego_raw * CV.MS_TO_KPH, self.acc_speed_kph) + # Handle pressing the cancel button. + elif CS.cruise_buttons == CruiseButtons.CANCEL: + self.enable_adaptive_cruise = False + self.acc_speed_kph = 0. + self.last_cruise_stalk_pull_time = 0 + # Handle pressing up and down buttons. + elif (self.enable_adaptive_cruise and + CS.cruise_buttons != self.prev_cruise_buttons): + # Real stalk command while ACC is already enabled. Adjust the max ACC + # speed if necessary. For example if max speed is 50 but you're currently + # only going 30, the cruise speed can be increased without any change to + # max ACC speed. If actual speed is already 50, the code also increases + # the max cruise speed. + if CS.cruise_buttons == CruiseButtons.RES_ACCEL: + requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + speed_uom_kph + self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) + elif CS.cruise_buttons == CruiseButtons.RES_ACCEL_2ND: + requested_speed_kph = CS.v_ego * CV.MS_TO_KPH + 5 * speed_uom_kph + self.acc_speed_kph = max(self.acc_speed_kph, requested_speed_kph) + elif CS.cruise_buttons == CruiseButtons.DECEL_SET: + self.acc_speed_kph -= speed_uom_kph + elif CS.cruise_buttons == CruiseButtons.DECEL_2ND: + self.acc_speed_kph -= 5 * speed_uom_kph + # Clip ACC speed between 0 and 170 KPH. + self.acc_speed_kph = min(self.acc_speed_kph, 170) + self.acc_speed_kph = max(self.acc_speed_kph, 0) + # If autoresume is not enabled, certain user actions disable ACC. + elif not self.autoresume: + # If something disabled cruise control (braking), disable ACC too. + if self.prev_pcm_acc_status == 2 and CS.pcm_acc_status != 2: + self.enable_adaptive_cruise = False + # if user took over steering, disable ACC too. + elif not enabled: + self.enable_adaptive_cruise = False + + # Notify if ACC was toggled + if prev_enable_adaptive_cruise and not self.enable_adaptive_cruise: + CS.UE.custom_alert_message(3, "ACC Disabled", 150, 4) + CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) + elif self.enable_adaptive_cruise and not prev_enable_adaptive_cruise: + CS.UE.custom_alert_message(2, "ACC Enabled", 150) + CS.cstm_btns.set_button_status("acc", ACCState.ENABLED) + + # Update the UI to show whether the current car state allows ACC. + if CS.cstm_btns.get_button_status("acc") in [ACCState.STANDBY, ACCState.NOT_READY]: + if (enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS): + CS.cstm_btns.set_button_status("acc", ACCState.STANDBY) + else: + CS.cstm_btns.set_button_status("acc", ACCState.NOT_READY) + + # Update prev state after all other actions. + self.prev_cruise_buttons = CS.cruise_buttons + self.prev_pcm_acc_status = CS.pcm_acc_status + + # Decide which cruise control buttons to simluate to get the car to the + # desired speed. + def update_acc(self, enabled, CS, frame, actuators, pcm_speed): + # Adaptive cruise control + current_time_ms = _current_time_millis() + if CruiseButtons.should_be_throttled(CS.cruise_buttons): + self.human_cruise_action_time = current_time_ms + button_to_press = None + + if (self.enable_adaptive_cruise + # Only do ACC if OP is steering + and enabled + # And adjust infrequently, since sending repeated adjustments makes + # the car think we're doing a 'long press' on the cruise stalk, + # resulting in small, jerky speed adjustments. + and current_time_ms > self.automated_cruise_action_time + 500): + + if CS.cstm_btns.get_button_label2("acc") in ["OP", "AutoOP"]: + button_to_press = self.calc_op_button(CS, pcm_speed, current_time_ms) + elif CS.cstm_btns.get_button_label2("acc") in ["FOLLOW", "AUTO"]: + # Alternative speed decision logic that uses the lead car's distance + # and speed more directly. + # Bring in the lead car distance from the Live20 feed + l20 = None + if enabled: + for socket, _ in self.poller.poll(0): + if socket is self.live20: + l20 = messaging.recv_one(socket) + break + if l20 is not None: + self.lead_1 = l20.live20.leadOne + button_to_press = self.calc_follow_button(CS) + if button_to_press: + self.automated_cruise_action_time = current_time_ms + # If trying to slow below the min cruise speed, just cancel cruise. + # This prevents a SCCM crash which is triggered by repeatedly pressing + # stalk-down when already at min cruise speed. + if (CruiseButtons.is_decel(button_to_press) + and CS.v_cruise_actual - 1 < self.MIN_CRUISE_SPEED_MS * CV.MS_TO_KPH): + button_to_press = CruiseButtons.CANCEL + # Debug logging (disable in production to reduce latency of commands) + #print "***ACC command: %s***" % button_to_press + #elif (current_time_ms > self.last_update_time + 1000): + # self.last_update_time = current_time_ms + # print "Desired ACC speed change: %s" % (speed_offset) + return button_to_press + + # Adjust speed based off OP's longitudinal model. + def calc_op_button(self, CS, pcm_speed, current_time_ms): + # Automatically engange traditional cruise if ACC is active. + if self.should_autoengage_cc(CS, current_time_ms): + button_to_press = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif (CS.pcm_acc_status == 2 + # But don't make adjustments if a human has manually done so in + # the last 3 seconds. Human intention should not be overridden. + and current_time_ms > self.human_cruise_action_time + 3000 + and current_time_ms > self.enabled_time + 1000): + # The difference between OP's target speed and the current cruise + # control speed, in KPH. + speed_offset = (pcm_speed * CV.MS_TO_KPH - CS.v_cruise_actual) + + if CS.imperial_speed_units: + # Imperial unit cars adjust cruise in units of 1 and 5 mph. + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + else: + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + + # Reduce cruise speed significantly if necessary. Multiply by a % to + # make the car slightly more eager to slow down vs speed up. + if speed_offset < -0.6 * full_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_2nd. + button_to_press = CruiseButtons.DECEL_2ND + # Reduce speed slightly if necessary. + elif speed_offset < -0.9 * half_press_kph and CS.v_cruise_actual > 0: + # Send cruise stalk dn_1st. + button_to_press = CruiseButtons.DECEL_SET + # Increase cruise speed if possible. + elif CS.v_ego > self.MIN_CRUISE_SPEED_MS: + # How much we can accelerate without exceeding max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + if speed_offset > full_press_kph and full_press_kph < available_speed: + # Send cruise stalk up_2nd. + button_to_press = CruiseButtons.RES_ACCEL_2ND + elif speed_offset > half_press_kph and half_press_kph < available_speed: + # Send cruise stalk up_1st. + button_to_press = CruiseButtons.RES_ACCEL + return button_to_press + + def should_autoengage_cc(self, CS, current_time_ms): + autoresume_supressed_by_braking = ( + self.autoresume + and CS.a_ego < 0 + and current_time_ms > self.enabled_time + 300) + return (CS.pcm_acc_status == 1 + and self.enable_adaptive_cruise + and CS.v_ego >= self.MIN_CRUISE_SPEED_MS + and not autoresume_supressed_by_braking) + + # function to calculate the cruise button based on a safe follow distance + def calc_follow_button(self, CS): + follow_time = 2.0 # in seconds + current_time_ms = _current_time_millis() + # Make sure we were able to populate lead_1. + if self.lead_1 is None: + return None + # dRel is in meters. + lead_dist = self.lead_1.dRel + # Grab the relative speed. + rel_speed = self.lead_1.vRel * CV.MS_TO_KPH + # Current speed in kph + cur_speed = CS.v_ego * CV.MS_TO_KPH + # v_ego is in m/s, so safe_dist_mance is in meters. + safe_dist_m = CS.v_ego * follow_time + # How much we can accelerate without exceeding the max allowed speed. + available_speed = self.acc_speed_kph - CS.v_cruise_actual + # Metric cars adjust cruise in units of 1 and 5 kph. + half_press_kph = 1 + full_press_kph = 5 + # Imperial unit cars adjust cruise in units of 1 and 5 mph + if CS.imperial_speed_units: + half_press_kph = 1 * CV.MPH_TO_KPH + full_press_kph = 5 * CV.MPH_TO_KPH + # button to issue + button = None + # debug msg + msg = None + + #print "dRel: ", self.lead_1.dRel," yRel: ", self.lead_1.yRel, " vRel: ", self.lead_1.vRel, " aRel: ", self.lead_1.aRel, " vLead: ", self.lead_1.vLead, " vLeadK: ", self.lead_1.vLeadK, " aLeadK: ", self.lead_1.aLeadK + + ### Logic to determine best cruise speed ### + + # Automatically engange traditional cruise if ACC is active. + braking_supresses_autoresume = ( + self.autoresume + and CS.a_ego < 0 + and current_time_ms > self.enabled_time + 300) + if self.should_autoengage_cc(CS, current_time_ms): + button = CruiseButtons.RES_ACCEL + # If traditional cruise is engaged, then control it. + elif CS.pcm_acc_status == 2: + # if cruise is set to faster than the max speed, slow down + if CS.v_cruise_actual > self.acc_speed_kph: + msg = "Slow to max" + button = CruiseButtons.DECEL_SET + # If lead_dist is reported as 0, no one is detected in front of you so you + # can speed up don't speed up when steer-angle > 2; vision radar often + # loses lead car in a turn. + elif lead_dist == 0 and self.enable_adaptive_cruise and CS.angle_steers < 2.0: + if full_press_kph < available_speed: + msg = "5 MPH UP full: ","{0:.1f}kph".format(full_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL_2ND + elif half_press_kph < available_speed: + msg = "1 MPH UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + # if we have a populated lead_distance + elif (lead_dist > 0 + # and we only issue commands every 300ms + and current_time_ms > self.automated_cruise_action_time + 300): + ### Slowing down ### + # Reduce speed significantly if lead_dist < 50% of safe dist, no matter + # the rel_speed + if CS.v_cruise_actual > full_press_kph: + if lead_dist < (safe_dist_m * 0.3) and rel_speed < 2: + msg = "50pct down" + button = CruiseButtons.DECEL_2ND + # Reduce speed significantly if lead_dist < 60% of safe dist + # and if the lead car isn't pulling away + elif lead_dist < (safe_dist_m * 0.5) and rel_speed < 0: + msg = "70pct down" + button = CruiseButtons.DECEL_SET + #Reduce speed if rel_speed < -15kph so you don't rush up to lead car + elif rel_speed < -15: + msg = "relspd -15 down" + button = CruiseButtons.DECEL_SET + # we're close to the safe distance, so make slow adjustments + # only adjust every 1 secs + elif (lead_dist < (safe_dist_m * 0.9) and rel_speed < 0 + and current_time_ms > self.automated_cruise_action_time + 1000): + msg = "90pct down" + button = CruiseButtons.DECEL_SET + + ### Speed up ### + # don't speed up again until you have more than a safe distance in front + # only adjust every 2 sec + elif ((lead_dist > (safe_dist_m * 0.8) or rel_speed > 5) and half_press_kph < available_speed + and current_time_ms > self.automated_cruise_action_time + 100): + msg = "120pct UP half: ","{0:.1f}kph".format(half_press_kph), " avail: {0:.1f}kph".format(available_speed) + button = CruiseButtons.RES_ACCEL + + # if we don't need to do any of the above, then we're at a pretty good + # speed make sure if we're at this point that the set cruise speed isn't + # set too low or high + if (cur_speed - CS.v_cruise_actual) > 5 and button == None: + # Send cruise stalk up_1st if the set speed is too low to bring it up + msg = "cruise rectify" + button = CruiseButtons.RES_ACCEL + + if (current_time_ms > self.last_update_time + 1000): + ratio = 0 + if safe_dist_m > 0: + ratio = (lead_dist / safe_dist_m) * 100 + print "Ratio: {0:.1f}%".format(ratio), " lead: ","{0:.1f}m".format(lead_dist)," avail: ","{0:.1f}kph".format(available_speed), " Rel Speed: ","{0:.1f}kph".format(rel_speed), " Angle: {0:.1f}deg".format(CS.angle_steers) + self.last_update_time = current_time_ms + if msg != None: + print msg + return button \ No newline at end of file From b0164b56fb164b2e1e92249a7b2e8563e77205c8 Mon Sep 17 00:00:00 2001 From: David Abrahams Date: Thu, 30 Aug 2018 00:47:43 +0000 Subject: [PATCH 17/17] Prevent ACC engagement below min speed --- selfdrive/car/tesla/ACC_module.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 1800d7085229cd..0a41ba01112b84 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -58,9 +58,10 @@ def update_stat(self, CS, enabled): self.prev_cruise_buttons != CruiseButtons.MAIN): double_pull = curr_time_ms - self.last_cruise_stalk_pull_time < 750 self.last_cruise_stalk_pull_time = curr_time_ms - ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF and - enabled and - CruiseState.is_enabled_or_standby(CS.pcm_acc_status)) + ready = (CS.cstm_btns.get_button_status("acc") > ACCState.OFF + and enabled + and CruiseState.is_enabled_or_standby(CS.pcm_acc_status) + and CS.v_ego > self.MIN_CRUISE_SPEED_MS) if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_adaptive_cruise = True @@ -259,10 +260,6 @@ def calc_follow_button(self, CS): ### Logic to determine best cruise speed ### # Automatically engange traditional cruise if ACC is active. - braking_supresses_autoresume = ( - self.autoresume - and CS.a_ego < 0 - and current_time_ms > self.enabled_time + 300) if self.should_autoengage_cc(CS, current_time_ms): button = CruiseButtons.RES_ACCEL # If traditional cruise is engaged, then control it.