From 91572da16c1af19fddd3b3ef607c8006aaaca774 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 15 Oct 2018 19:39:29 -0400 Subject: [PATCH 01/20] UIBT reset file if new buttons; ALCA dev (not finished) --- selfdrive/car/modules/ALCA_module.py | 37 ++++++++++++------- selfdrive/car/modules/UIBT_module.py | 30 +++++++++++---- selfdrive/car/tesla/PCC_module.py | 55 ++-------------------------- selfdrive/car/tesla/interface.py | 4 +- 4 files changed, 52 insertions(+), 74 deletions(-) diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 8cd819c3ec3273..b4c64fa4034983 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -29,6 +29,8 @@ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +v3.5 - changing the start angle to keep turning until we reach MAX_ANGLE_DELTA +v3.4 - read steerRatio from each car parameters file v3.3 - re-entry logic changed for smoothness v3.2 - angle adjustment to compesate for road curvature change v3.1 - new angle logic for a smoother re-entry @@ -41,15 +43,18 @@ from selfdrive.controls.lib.pid import PIController # max REAL delta angle for correction vs actuator -CL_MAX_ANGLE_DELTA = 1.2 +CL_MAX_ANGLE_DELTA = 1.5 # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line CL_LANE_DETECT_BP = [10., 44.] -CL_LANE_DETECT_FACTOR = [1.3, .75] +CL_LANE_DETECT_FACTOR = [1.5, .75] + +CL_LANE_PASS_BP = [10., 44.] +CL_LANE_PASS_TIME = [20., 4.] # change lane delta angles and other params CL_MAXD_BP = [10., 32., 44.] -CL_MAXD_A = [.308, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 +CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s CL_MAX_A = 10. # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed @@ -62,7 +67,7 @@ #duration after we cross the line until we release is a factor of speed CL_TIMEA_BP = [10., 32., 44.] -CL_TIMEA_T = [0.9 ,0.35, 0.25] +CL_TIMEA_T = [0.7 ,0.30, 0.20] class ALCAController(object): def __init__(self,carcontroller,alcaEnabled,steerByAngle): @@ -88,7 +93,7 @@ def __init__(self,carcontroller,alcaEnabled,steerByAngle): self.laneChange_lw = 3.7 # lane width in meters self.laneChange_angle = 0. # saves the last angle from actuators before lane change starts self.laneChange_angled = 0. # angle delta - self.laneChange_steerr = 16.75 # steer ratio for lane change + self.laneChange_steerr = 15.75 # steer ratio for lane change starting with the Tesla one self.laneChange_direction = 0 # direction of the lane change self.prev_right_blinker_on = False # local variable for prev position self.prev_left_blinker_on = False # local variable for prev position @@ -124,6 +129,7 @@ def update_status(self,alcaEnabled): self.alcaEnabled = alcaEnabled def set_pid(self,CS): + self.laneChange_steerr = CS.CP.steerRatio self.pid = PIController((CS.CP.steerKpBP, CS.CP.steerKpV), (CS.CP.steerKiBP, CS.CP.steerKiV), k_f=CS.CP.steerKf, pos_limit=1.0) @@ -224,17 +230,18 @@ def update_angle(self,enabled,CS,frame,actuators): if self.laneChange_counter ==1: CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (5)",800) self.laneChange_counter += 1 + cl_lane_pass_time = interp(CS.v_ego,CL_LANE_PASS_BP,CL_LANE_PASS_TIME) # check if angle continues to decrease current_delta = abs(self.laneChange_angle + laneChange_angle + (-actuators.steerAngle)) previous_delta = abs(self.laneChange_last_sent_angle - self.laneChange_last_actuator_angle) - if (self.laneChange_counter > 4): + if (self.laneChange_counter > cl_lane_pass_time): # continue to half the angle between our angle and actuator laneChange_angle = (-actuators.steerAngle - self.laneChange_angle)/2 #self.laneChange_angled self.laneChange_angle += laneChange_angle else: laneChange_angle = self.laneChange_angled # wait 0.05 sec before starting to check if angle increases or if we are within 5 deg of actuator.angleSteer - if ((current_delta > previous_delta) or (current_delta <= 5.)) and (self.laneChange_counter > 5): + if ((current_delta > previous_delta) or (current_delta <= 5.)) and (self.laneChange_counter > cl_lane_pass_time): self.laneChange_enabled = 7 self.laneChange_counter = 1 self.laneChange_direction = 0 @@ -311,26 +318,30 @@ def update_angle(self,enabled,CS,frame,actuators): if self.laneChange_enabled == 4: if self.laneChange_counter == 1: self.laneChange_angle = -actuators.steerAngle - CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (3)",100) self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A) # if angle more than max angle allowed cancel; last chance to cancel on road curvature - if (abs(self.laneChange_angle) > CL_MAX_A): + if self.laneChange_counter == 2: + CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (3)",100) + if (abs(self.laneChange_angle) > CL_MAX_A): CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (a)",200,5) self.laneChange_enabled = 1 self.laneChange_counter = 0 self.laneChange_direction = 0 CS.cstm_btns.set_button_status("alca",1) - laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50 self.laneChange_counter += 1 - delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled) + #laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50 + #delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled) + laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 100 + delta_change = abs(self.laneChange_angle + laneChange_angle + actuators.steerAngle) - CL_MAX_ANGLE_DELTA * self.laneChange_steerr if (self.laneChange_counter == 100) or (delta_change >= 0): if (delta_change < 0): # didn't achieve desired angle yet, so repeat self.laneChange_counter = 1 else: self.laneChange_enabled = 3 - self.laneChange_counter = 1 - self.laneChange_angled = laneChange_angle + self.laneChange_counter = 2 + self.laneChange_angle += laneChange_angle + laneChange_angle = 0. # this is the first stage of the ALCAS # here we wait for x seconds before we start the turn # if at any point we detect hand on wheel, we let go of control and notify user diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index d24d74197da1c9..d4217591920f92 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -29,16 +29,26 @@ def read_buttons_labels_from_file(self): fi = open(self.buttons_labels_path, buttons_file_r) indata = fi.read() fi.close() + file_matches = True if len(indata) == btn_msg_len * 6 : - #we have all the data - self.btns = [] + #check if it matches the current setup for i in range(0, len(indata), btn_msg_len): - name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) - self.btns.append(UIButton(name.rstrip("\0"),label.rstrip("\0"),0,label2.rstrip("\0"),0)) - #now read the last saved statuses + j = int(i/btn_msg_len) + name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) + if (self.btns[j].btn_name != name.rstrip("\0")): + file_matches = False + #we have all the da;ta and it matches + if file_matches: + for i in range(0, len(indata), btn_msg_len): + j = int(i/btn_msg_len) + name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) + self.btns[j].btn_label = label.rstrip("\0") + self.btns[j].btn_label2 = label2.rstrip("\0") + return file_matches else: #we don't have all the data, ignore print "labels file is bad" + return False def write_buttons_out_file(self): @@ -78,9 +88,15 @@ def __init__(self, carstate,car,folder): self.hasChanges = True self.last_in_read_time = datetime.min if os.path.exists(self.buttons_labels_path): + self.btns = self.CS.init_ui_buttons() #there is a file, load it - self.read_buttons_labels_from_file() - self.read_buttons_out_file() + if self.read_buttons_labels_from_file(): + self.read_buttons_out_file() + else: + #no match, so write the new ones + self.hasChanges = True + self.write_buttons_labels_to_file() + self.write_buttons_out_file() else: #there is no file, create it self.btns = self.CS.init_ui_buttons() diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 7026f34431fccc..12ad76fddd3b46 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -18,18 +18,13 @@ # TODO: these should end up in values.py at some point, probably variable by trim # Accel limits ACCEL_HYST_GAP = 0.5 # don't change accel command for small oscilalitons within this value -ACCEL_MAX = 1. -ACCEL_MIN = -1. -ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) -ACCEL_REWIND_MAX = 4. -PEDAL_DEADZONE = 3. + PEDAL_MAX_UP = 2. PEDAL_MAX_DOWN = 5. #BB MIN_SAFE_DIST_M = 4. # min safe distance in meters -FRAMES_PER_SEC = 5. -BRAKE_THRESHOLD =0.1 -ACCEL_THRESHOLD = 0.1 +FRAMES_PER_SEC = 100. + SPEED_UP = 1. #3. / FRAMES_PER_SEC # 2 m/s = 7.5 mph = 12 kph SPEED_DOWN = 1. #6. / FRAMES_PER_SEC @@ -42,22 +37,10 @@ MIN_PCC_V = 0. # MAX_PCC_V = 170. -STOPPING_EGO_SPEED = 0.5 MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface -STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 -STARTING_TARGET_SPEED = 0.5 -BRAKE_THRESHOLD_TO_PID = 0.2 -#### from planner -# -_DT = 0.01 # 100Hz -_DT_MPC = 0.02 # 50Hz -MAX_SPEED_ERROR = 2.0 AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted -_LEAD_ACCEL_TAU = 1.5 - -GPS_PLANNER_ADDR = "192.168.5.1" # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits @@ -80,26 +63,6 @@ # max acceleration allowed in acc, which happens in restart A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) -def calc_cruise_accel_limits(v_ego, following): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) - if following: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) - else: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) - return np.vstack([a_cruise_min, a_cruise_max]) - -def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): - """ - This function returns a limited long acceleration allow ed, depending on the existing lateral acceleration - this should avoid accelerating when losing the target i n turns - """ - - a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) - a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) - - a_target[1] = min(a_target[1], a_x_allowed) - return a_target class PCCState(object): # Possible state of the ACC system, following the DI_cruiseState naming @@ -317,18 +280,6 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): accel_max = interp(CS.v_ego, CS.CP.gasMaxBP, CS.CP.gasMaxV) brake_max = interp(CS.v_ego, CS.CP.brakeMaxBP, CS.CP.brakeMaxV) - following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.v_ego and self.lead_1.aLeadK > 0.0 - - - accel_limits = map(float, calc_cruise_accel_limits(CS.v_ego, following)) - # TODO: make a separate lookup for jerk tuning - jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] - accel_limits = limit_accel_in_turns(CS.v_ego,CS.angle_steers, accel_limits, CS.CP) - - if brake_max <= 0.2: - # if required so, force a smooth deceleration - accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) - accel_limits[0] = min(accel_limits[0], accel_limits[1]) output_gb = 0 #################################################################### # this mode (Follow) uses the Follow logic created by JJ for ACC diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 12e2d27bbc7c5c..3f40b7036800fb 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -119,7 +119,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = centerToFront_models ret.steerRatio = 15.75 # Kp and Ki for the lateral control for 0, 20, 40, 60 mph - ret.steerKpV, ret.steerKiV = [[0.60, 0.40, 0.30, 0.15], [0.08, 0.06, 0.04, 0.02]] + ret.steerKpV, ret.steerKiV = [[1.20, 0.80, 0.60, 0.30], [0.16, 0.12, 0.08, 0.04]] ret.steerKf = 0.00006 # Initial test value TODO: investigate FF steer control for Model S? ret.steerActuatorDelay = 0.09 @@ -168,7 +168,7 @@ def get_params(candidate, fingerprint): # no max steer limit VS speed ret.steerMaxBP = [0.,15.] # m/s - ret.steerMaxV = [17.,17.] # max steer allowed + ret.steerMaxV = [420.,420.] # max steer allowed ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] #if ret.enableGasInterceptor else [0.] # max gas allowed From 4e9e2af4ae9508f7fee2463e3be5457dcaa52a2f Mon Sep 17 00:00:00 2001 From: BogGyver Date: Tue, 16 Oct 2018 13:50:58 -0400 Subject: [PATCH 02/20] ALCA v3.6 with car specific prams --- selfdrive/car/honda/carstate.py | 44 +++++++++++ selfdrive/car/modules/ALCA_module.py | 107 ++++++++++++++++++--------- selfdrive/car/tesla/carstate.py | 44 +++++++++++ selfdrive/car/toyota/carstate.py | 45 ++++++++++- 4 files changed, 202 insertions(+), 38 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index de50c277fb7144..494f6b6a3efca5 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -135,6 +135,50 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): + #if (CP.carFingerprint == CAR.MODELS): + # ALCA PARAMS + # max REAL delta angle for correction vs actuator + self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] + self.CL_MAX_ANGLE_DELTA = [2., .8] + + # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother + self.CL_ADJUST_FACTOR_BP = [10., 44.] + self.CL_ADJUST_FACTOR = [16. , 16.] + + + # reenrey angle when to let go + self.CL_REENTRY_ANGLE_BP = [10., 44.] + self.CL_REENTRY_ANGLE = [5. , 5.] + + # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line + self.CL_LANE_DETECT_BP = [10., 44.] + self.CL_LANE_DETECT_FACTOR = [1.5, .75] + + self.CL_LANE_PASS_BP = [10., 44.] + self.CL_LANE_PASS_TIME = [100., 5.] + + # change lane delta angles and other params + self.CL_MAXD_BP = [10., 32., 44.] + self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + + self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + + # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + self.CL_MAX_A_BP = [10., 44.] + self.CL_MAX_A = [10., 10.] + + # define limits for angle change every 0.1 s + # we need to force correction above 10 deg but less than 20 + # anything more means we are going to steep or not enough in a turn + self.CL_MAX_ACTUATOR_DELTA = 2. + self.CL_MIN_ACTUATOR_DELTA = 0. + self.CL_CORRECTION_FACTOR = 1. + + #duration after we cross the line until we release is a factor of speed + self.CL_TIMEA_BP = [10., 32., 44.] + self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + #END OF ALCA PARAMS + self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index b4c64fa4034983..7a64142f8038df 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -29,6 +29,9 @@ (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +HISTORY +------- +v3.6 - moved parameters to carstate.py v3.5 - changing the start angle to keep turning until we reach MAX_ANGLE_DELTA v3.4 - read steerRatio from each car parameters file v3.3 - re-entry logic changed for smoothness @@ -37,40 +40,62 @@ v3.0 - better lane dettection logic v2.0 - detection of lane crossing v1.0 - fixed angle move -""" -from common.numpy_fast import interp -from selfdrive.controls.lib.pid import PIController +######################################################## +The following parameters need to be added to carstate.py +and based on each model you want to define +######################################################## # max REAL delta angle for correction vs actuator -CL_MAX_ANGLE_DELTA = 1.5 +CL_MAX_ANGLE_DELTA_BP = [10., 44.] +CL_MAX_ANGLE_DELTA = [2., .8] + +# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother +CL_ADJUST_FACTOR_BP = [10., 44.] +CL_ADJUST_FACTOR = [16. , 16.] + + +# reenrey angle when to let go +CL_REENTRY_ANGLE_BP = [10., 44.] +CL_REENTRY_ANGLE = [5. , 5.] # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line CL_LANE_DETECT_BP = [10., 44.] CL_LANE_DETECT_FACTOR = [1.5, .75] CL_LANE_PASS_BP = [10., 44.] -CL_LANE_PASS_TIME = [20., 4.] +CL_LANE_PASS_TIME = [100., 5.] # change lane delta angles and other params CL_MAXD_BP = [10., 32., 44.] CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s -CL_MAX_A = 10. # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + +# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed +CL_MAX_A_BP = [10., 44.] +CL_MAX_A = [10., 10.] # define limits for angle change every 0.1 s # we need to force correction above 10 deg but less than 20 # anything more means we are going to steep or not enough in a turn -MAX_ACTUATOR_DELTA = 2. -MIN_ACTUATOR_DELTA = 0. -CORRECTION_FACTOR = 1. +CL_MAX_ACTUATOR_DELTA = 2. +CL_MIN_ACTUATOR_DELTA = 0. +CL_CORRECTION_FACTOR = 1. #duration after we cross the line until we release is a factor of speed CL_TIMEA_BP = [10., 32., 44.] CL_TIMEA_T = [0.7 ,0.30, 0.20] + +""" + +from common.numpy_fast import interp +from selfdrive.controls.lib.pid import PIController + class ALCAController(object): def __init__(self,carcontroller,alcaEnabled,steerByAngle): + #import settings self.CC = carcontroller # added to start, will see if we need it actually # variables for lane change self.angle_offset = 0. #added when one needs to compensate for missalignment @@ -135,9 +160,19 @@ def set_pid(self,CS): k_f=CS.CP.steerKf, pos_limit=1.0) def update_angle(self,enabled,CS,frame,actuators): + # speed variable parameters + cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) + cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) + cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) + cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) + cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP, CS.CL_LANE_DETECT_FACTOR) + cl_max_a = interp(CS.v_ego, CS.CL_MAX_A, CS.CL_MAX_A_BP) + cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR) + cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP, CS.CL_REENTRY_ANGLE) + cl_min_v = CS.CL_MIN_V + + # Basic highway lane change logic - changing_lanes = CS.right_blinker_on or CS.left_blinker_on - #self.laneChange_steerr = CS.CP.steerRatio actuator_delta = 0. laneChange_angle = 0. turn_signal_needed = 0 # send 1 for left, 2 for right 0 for not needed @@ -161,15 +196,15 @@ def update_angle(self,enabled,CS,frame,actuators): if (CS.cstm_btns.get_button_status("alca") > 0) and self.alcaEnabled and (self.laneChange_enabled == 1): - if ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or \ - (abs(CS.angle_steers)>= CL_MAX_A) or (not enabled)): + if ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or \ + (abs(CS.angle_steers)>= cl_max_a) or (not enabled)): CS.cstm_btns.set_button_status("alca",9) else: CS.cstm_btns.set_button_status("alca",1) if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \ ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \ - ((CS.v_ego < CL_MIN_V) or (abs(actuators.steerAngle) >= CL_MAX_A) or (abs(CS.angle_steers) >=CL_MAX_A)): + ((CS.v_ego < cl_min_v) or (abs(actuators.steerAngle) >= cl_max_a) or (abs(CS.angle_steers) >=cl_max_a)): # something is not right, the speed or angle is limitting CS.UE.custom_alert_message(3,"Auto Lane Change Unavailable!",500,3) CS.cstm_btns.set_button_status("alca",9) @@ -177,7 +212,7 @@ def update_angle(self,enabled,CS,frame,actuators): if self.alcaEnabled and enabled and (((not self.prev_right_blinker_on) and CS.right_blinker_on) or \ ((not self.prev_left_blinker_on) and CS.left_blinker_on)) and \ - (CS.v_ego >= CL_MIN_V) and (abs(actuators.steerAngle) < CL_MAX_A): + (CS.v_ego >= cl_min_v) and (abs(actuators.steerAngle) < cl_max_a): # start blinker, speed and angle is within limits, let's go laneChange_direction = 1 # changing lanes @@ -199,7 +234,7 @@ def update_angle(self,enabled,CS,frame,actuators): self.laneChange_direction = laneChange_direction self.laneChange_avg_angle = 0. self.laneChange_avg_count = 0. - self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A) + self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a self.laneChange_last_actuator_angle = 0. self.laneChange_last_actuator_delta = 0. self.laneChange_over_the_line = 0 @@ -214,7 +249,7 @@ def update_angle(self,enabled,CS,frame,actuators): # lane change in process if self.laneChange_enabled > 1: - if (CS.steer_override or (CS.v_ego < CL_MIN_V)): + if (CS.steer_override or (CS.v_ego < cl_min_v)): CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (u)",200,3) # if any steer override cancel process or if speed less than min speed self.laneChange_counter = 0 @@ -227,26 +262,24 @@ def update_angle(self,enabled,CS,frame,actuators): # 3. we check to see if the delta between our steering and the actuator is less than 5 deg # 4. we disengage after 0.5s and let OP take over if self.laneChange_enabled ==2: + laneChange_angle = self.laneChange_angled if self.laneChange_counter ==1: CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (5)",800) self.laneChange_counter += 1 - cl_lane_pass_time = interp(CS.v_ego,CL_LANE_PASS_BP,CL_LANE_PASS_TIME) # check if angle continues to decrease current_delta = abs(self.laneChange_angle + laneChange_angle + (-actuators.steerAngle)) previous_delta = abs(self.laneChange_last_sent_angle - self.laneChange_last_actuator_angle) if (self.laneChange_counter > cl_lane_pass_time): # continue to half the angle between our angle and actuator - laneChange_angle = (-actuators.steerAngle - self.laneChange_angle)/2 #self.laneChange_angled - self.laneChange_angle += laneChange_angle - else: - laneChange_angle = self.laneChange_angled - # wait 0.05 sec before starting to check if angle increases or if we are within 5 deg of actuator.angleSteer - if ((current_delta > previous_delta) or (current_delta <= 5.)) and (self.laneChange_counter > cl_lane_pass_time): + delta_angle = (-actuators.steerAngle - self.laneChange_angle - self.laneChange_angled)/cl_adjust_factor + self.laneChange_angle += delta_angle + # wait 0.05 sec before starting to check if angle increases or if we are within X deg of actuator.angleSteer + if ((current_delta > previous_delta) or (current_delta <= cl_reentry_angle)) and (self.laneChange_counter > cl_lane_pass_time): self.laneChange_enabled = 7 self.laneChange_counter = 1 self.laneChange_direction = 0 # we crossed the line, so x sec later give control back to OP - laneChange_after_lane_duration = interp(CS.v_ego,CL_TIMEA_BP,CL_TIMEA_T) * self.laneChange_after_lane_duration_mult + laneChange_after_lane_duration = cl_timea_t * self.laneChange_after_lane_duration_mult if (self.laneChange_counter > laneChange_after_lane_duration * 100): self.laneChange_enabled = 7 self.laneChange_counter = 1 @@ -269,7 +302,6 @@ def update_angle(self,enabled,CS,frame,actuators): self.keep_angle = False self.laneChange_counter += 1 laneChange_angle = self.laneChange_angled - cl_lane_detect_factor = interp(CS.v_ego, CL_LANE_DETECT_BP, CL_LANE_DETECT_FACTOR) if (self.laneChange_over_the_line == 0): # we didn't cross the line, so keep computing the actuator delta until it flips actuator_delta = self.laneChange_direction * (-actuators.steerAngle - self.laneChange_last_actuator_angle) @@ -288,19 +320,19 @@ def update_angle(self,enabled,CS,frame,actuators): c = 5. if a == 0: a = 0.00001 - if (abs(a) < MIN_ACTUATOR_DELTA) and (self.keep_angle == False): - c = (a/abs(a)) * (MIN_ACTUATOR_DELTA - abs(a)) / 10 - self.laneChange_angle = self.laneChange_angle -self.laneChange_direction * CORRECTION_FACTOR * c * 10 + if (abs(a) < CL_MIN_ACTUATOR_DELTA) and (self.keep_angle == False): + c = (a/abs(a)) * (CL_MIN_ACTUATOR_DELTA - abs(a)) / 10 + self.laneChange_angle = self.laneChange_angle -self.laneChange_direction * CL_CORRECTION_FACTOR * c * 10 self.last10delta_correct(c) #print a, c, actuator_delta, self.laneChange_angle """ # code for v3.2 and above a_delta = self.laneChange_direction * (self.laneChange_angle + laneChange_angle - (-actuators.steerAngle)) - if (self.laneChange_over_the_line == 0) and ((abs(a_delta) > CL_MAX_ANGLE_DELTA * self.laneChange_steerr) or (self.keep_angle)): + if (self.laneChange_over_the_line == 0) and ((abs(a_delta) > cl_max_angle_delta * self.laneChange_steerr) or (self.keep_angle)): #steering more than what we wanted, need to adjust self.keep_angle = True - self.laneChange_angle = -actuators.steerAngle + self.laneChange_direction * CL_MAX_ANGLE_DELTA * self.laneChange_steerr - laneChange_angle + self.laneChange_angle = -actuators.steerAngle + self.laneChange_direction * cl_max_angle_delta * self.laneChange_steerr - laneChange_angle if self.laneChange_counter > (self.laneChange_duration) * 100: self.laneChange_enabled = 1 self.laneChange_counter = 0 @@ -318,11 +350,11 @@ def update_angle(self,enabled,CS,frame,actuators): if self.laneChange_enabled == 4: if self.laneChange_counter == 1: self.laneChange_angle = -actuators.steerAngle - self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * interp(CS.v_ego, CL_MAXD_BP, CL_MAXD_A) + self.laneChange_angled = self.laneChange_direction * self.laneChange_steerr * cl_maxd_a # if angle more than max angle allowed cancel; last chance to cancel on road curvature if self.laneChange_counter == 2: CS.UE.custom_alert_message(2,"Auto Lane Change Engaged! (3)",100) - if (abs(self.laneChange_angle) > CL_MAX_A): + if (abs(self.laneChange_angle) > cl_max_a) and (self.laneChange_counter == 1): CS.UE.custom_alert_message(4,"Auto Lane Change Canceled! (a)",200,5) self.laneChange_enabled = 1 self.laneChange_counter = 0 @@ -332,16 +364,17 @@ def update_angle(self,enabled,CS,frame,actuators): #laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 50 #delta_change = abs(self.laneChange_angle+ laneChange_angle + actuators.steerAngle) - self.laneChange_strStartMultiplier * abs(self.laneChange_angled) laneChange_angle = self.laneChange_strStartFactor * self.laneChange_angled * self.laneChange_counter / 100 - delta_change = abs(self.laneChange_angle + laneChange_angle + actuators.steerAngle) - CL_MAX_ANGLE_DELTA * self.laneChange_steerr + delta_change = abs(self.laneChange_angle + laneChange_angle + actuators.steerAngle) - cl_max_angle_delta * self.laneChange_steerr if (self.laneChange_counter == 100) or (delta_change >= 0): if (delta_change < 0): # didn't achieve desired angle yet, so repeat - self.laneChange_counter = 1 - else: - self.laneChange_enabled = 3 self.laneChange_counter = 2 self.laneChange_angle += laneChange_angle laneChange_angle = 0. + else: + self.laneChange_enabled = 3 + self.laneChange_counter = 1 + self.laneChange_angled = laneChange_angle # this is the first stage of the ALCAS # here we wait for x seconds before we start the turn # if at any point we detect hand on wheel, we let go of control and notify user diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index d2ac2ad8a72b41..1897498b6996e4 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -166,6 +166,50 @@ def get_epas_parser(CP): class CarState(object): def __init__(self, CP): + if (CP.carFingerprint == CAR.MODELS): + # ALCA PARAMS + # max REAL delta angle for correction vs actuator + self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] + self.CL_MAX_ANGLE_DELTA = [1.8, .3] + + # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother + self.CL_ADJUST_FACTOR_BP = [10., 44.] + self.CL_ADJUST_FACTOR = [16. , 8.] + + + # reenrey angle when to let go + self.CL_REENTRY_ANGLE_BP = [10., 44.] + self.CL_REENTRY_ANGLE = [5. , 5.] + + # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line + self.CL_LANE_DETECT_BP = [10., 44.] + self.CL_LANE_DETECT_FACTOR = [1.5, .75] + + self.CL_LANE_PASS_BP = [10., 44.] + self.CL_LANE_PASS_TIME = [60., 3.] + + # change lane delta angles and other params + self.CL_MAXD_BP = [10., 32., 44.] + self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + + self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + + # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + self.CL_MAX_A_BP = [10., 44.] + self.CL_MAX_A = [10., 10.] + + # define limits for angle change every 0.1 s + # we need to force correction above 10 deg but less than 20 + # anything more means we are going to steep or not enough in a turn + self.CL_MAX_ACTUATOR_DELTA = 2. + self.CL_MIN_ACTUATOR_DELTA = 0. + self.CL_CORRECTION_FACTOR = 1. + + #duration after we cross the line until we release is a factor of speed + self.CL_TIMEA_BP = [10., 32., 44.] + self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + #END OF ALCA PARAMS + self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 self.CP = CP diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 519daadf2d6838..51876c5430c029 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -70,7 +70,50 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): - + #if (CP.carFingerprint == CAR.MODELS): + # ALCA PARAMS + # max REAL delta angle for correction vs actuator + self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] + self.CL_MAX_ANGLE_DELTA = [2., .8] + + # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother + self.CL_ADJUST_FACTOR_BP = [10., 44.] + self.CL_ADJUST_FACTOR = [16. , 16.] + + + # reenrey angle when to let go + self.CL_REENTRY_ANGLE_BP = [10., 44.] + self.CL_REENTRY_ANGLE = [5. , 5.] + + # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line + self.CL_LANE_DETECT_BP = [10., 44.] + self.CL_LANE_DETECT_FACTOR = [1.5, .75] + + self.CL_LANE_PASS_BP = [10., 44.] + self.CL_LANE_PASS_TIME = [100., 5.] + + # change lane delta angles and other params + self.CL_MAXD_BP = [10., 32., 44.] + self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75 + + self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s + + # do not turn if actuator wants more than x deg for going straight; this should be interp based on speed + self.CL_MAX_A_BP = [10., 44.] + self.CL_MAX_A = [10., 10.] + + # define limits for angle change every 0.1 s + # we need to force correction above 10 deg but less than 20 + # anything more means we are going to steep or not enough in a turn + self.CL_MAX_ACTUATOR_DELTA = 2. + self.CL_MIN_ACTUATOR_DELTA = 0. + self.CL_CORRECTION_FACTOR = 1. + + #duration after we cross the line until we release is a factor of speed + self.CL_TIMEA_BP = [10., 32., 44.] + self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + #END OF ALCA PARAMS + self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR'] From aa411041b6e8b9e34a6fc86dcf3f674ba1620c0f Mon Sep 17 00:00:00 2001 From: BogGyver Date: Tue, 16 Oct 2018 14:12:49 -0400 Subject: [PATCH 03/20] ALCA params for Tesla @ all speeds; need check > 70mph --- selfdrive/car/tesla/carstate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 1897498b6996e4..a68ff385a32953 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -183,10 +183,10 @@ def __init__(self, CP): # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line self.CL_LANE_DETECT_BP = [10., 44.] - self.CL_LANE_DETECT_FACTOR = [1.5, .75] + self.CL_LANE_DETECT_FACTOR = [1.5, 1.5] - self.CL_LANE_PASS_BP = [10., 44.] - self.CL_LANE_PASS_TIME = [60., 3.] + self.CL_LANE_PASS_BP = [10., 20., 44.] + self.CL_LANE_PASS_TIME = [40.,10., 3.] # change lane delta angles and other params self.CL_MAXD_BP = [10., 32., 44.] From 01366cbc120f8cf805a7e802c421788b2f7001d2 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Tue, 16 Oct 2018 14:41:51 -0400 Subject: [PATCH 04/20] added base params for toyota and honda; to be optimized by others --- selfdrive/car/honda/carstate.py | 13 ++++++++----- selfdrive/car/modules/ALCA_module.py | 1 + selfdrive/car/tesla/carstate.py | 3 +++ selfdrive/car/toyota/carstate.py | 13 ++++++++----- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 494f6b6a3efca5..f295e266b0b5ba 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -139,11 +139,11 @@ def __init__(self, CP): # ALCA PARAMS # max REAL delta angle for correction vs actuator self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] - self.CL_MAX_ANGLE_DELTA = [2., .8] + self.CL_MAX_ANGLE_DELTA = [1.8, .3] # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother self.CL_ADJUST_FACTOR_BP = [10., 44.] - self.CL_ADJUST_FACTOR = [16. , 16.] + self.CL_ADJUST_FACTOR = [16. , 8.] # reenrey angle when to let go @@ -152,10 +152,10 @@ def __init__(self, CP): # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line self.CL_LANE_DETECT_BP = [10., 44.] - self.CL_LANE_DETECT_FACTOR = [1.5, .75] + self.CL_LANE_DETECT_FACTOR = [1.5, 1.5] - self.CL_LANE_PASS_BP = [10., 44.] - self.CL_LANE_PASS_TIME = [100., 5.] + self.CL_LANE_PASS_BP = [10., 20., 44.] + self.CL_LANE_PASS_TIME = [40.,10., 3.] # change lane delta angles and other params self.CL_MAXD_BP = [10., 32., 44.] @@ -177,6 +177,9 @@ def __init__(self, CP): #duration after we cross the line until we release is a factor of speed self.CL_TIMEA_BP = [10., 32., 44.] self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + + #duration to wait (in seconds) with blinkers on before starting to turn + self.CL_WAIT_BEFORE_START = 1 #END OF ALCA PARAMS self.CP = CP diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 7a64142f8038df..85fa99994072bc 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -170,6 +170,7 @@ def update_angle(self,enabled,CS,frame,actuators): cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR) cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP, CS.CL_REENTRY_ANGLE) cl_min_v = CS.CL_MIN_V + self.laneChange_wait = CS.CL_WAIT_BEFORE_START # Basic highway lane change logic diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index a68ff385a32953..bc1c81d64e0766 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -209,6 +209,9 @@ def __init__(self, CP): self.CL_TIMEA_BP = [10., 32., 44.] self.CL_TIMEA_T = [0.7 ,0.30, 0.20] #END OF ALCA PARAMS + + #duration to wait (in seconds) with blinkers on before starting to turn + self.CL_WAIT_BEFORE_START = 1 self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 51876c5430c029..3cf4d70f536463 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -74,11 +74,11 @@ def __init__(self, CP): # ALCA PARAMS # max REAL delta angle for correction vs actuator self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] - self.CL_MAX_ANGLE_DELTA = [2., .8] + self.CL_MAX_ANGLE_DELTA = [1.8, .3] # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother self.CL_ADJUST_FACTOR_BP = [10., 44.] - self.CL_ADJUST_FACTOR = [16. , 16.] + self.CL_ADJUST_FACTOR = [16. , 8.] # reenrey angle when to let go @@ -87,10 +87,10 @@ def __init__(self, CP): # a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line self.CL_LANE_DETECT_BP = [10., 44.] - self.CL_LANE_DETECT_FACTOR = [1.5, .75] + self.CL_LANE_DETECT_FACTOR = [1.5, 1.5] - self.CL_LANE_PASS_BP = [10., 44.] - self.CL_LANE_PASS_TIME = [100., 5.] + self.CL_LANE_PASS_BP = [10., 20., 44.] + self.CL_LANE_PASS_TIME = [40.,10., 3.] # change lane delta angles and other params self.CL_MAXD_BP = [10., 32., 44.] @@ -112,6 +112,9 @@ def __init__(self, CP): #duration after we cross the line until we release is a factor of speed self.CL_TIMEA_BP = [10., 32., 44.] self.CL_TIMEA_T = [0.7 ,0.30, 0.20] + + #duration to wait (in seconds) with blinkers on before starting to turn + self.CL_WAIT_BEFORE_START = 1 #END OF ALCA PARAMS self.CP = CP From 1566523107812a6e7e15c2e9fc196a3a5fe13a77 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Tue, 16 Oct 2018 15:29:46 -0400 Subject: [PATCH 05/20] minor tweak on param --- selfdrive/car/tesla/carstate.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index bc1c81d64e0766..7746f97c164b35 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -208,10 +208,11 @@ def __init__(self, CP): #duration after we cross the line until we release is a factor of speed self.CL_TIMEA_BP = [10., 32., 44.] self.CL_TIMEA_T = [0.7 ,0.30, 0.20] - #END OF ALCA PARAMS #duration to wait (in seconds) with blinkers on before starting to turn self.CL_WAIT_BEFORE_START = 1 + + #END OF ALCA PARAMS self.brake_only = CP.enableCruise self.last_cruise_stalk_pull_time = 0 From ab3313c2e9881784402c3feabd59e8398a09cb13 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Thu, 18 Oct 2018 20:24:10 -0400 Subject: [PATCH 06/20] pedal tuneing for Tesla --- selfdrive/car/tesla/PCC_module.py | 100 +++++++++++++++++++++++++++--- selfdrive/car/tesla/carstate.py | 23 +++++-- 2 files changed, 110 insertions(+), 13 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 12ad76fddd3b46..27cd44793cf7ce 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -6,6 +6,7 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother +from common.realtime import sec_since_boot from common.numpy_fast import clip import selfdrive.messaging as messaging import os @@ -15,6 +16,9 @@ import math import numpy as np + +MPC_BRAKE_MULTIPLIER = 2. + # TODO: these should end up in values.py at some point, probably variable by trim # Accel limits ACCEL_HYST_GAP = 0.5 # don't change accel command for small oscilalitons within this value @@ -63,6 +67,32 @@ # max acceleration allowed in acc, which happens in restart A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) +_DT = 0.01 # 100Hz +_DT_MPC = 0.2 # 5Hz + +def calc_cruise_accel_limits(v_ego, following): + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + + if following: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + return np.vstack([a_cruise_min, a_cruise_max]) + + +def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): + """ + This function returns a limited long acceleration allowed, depending on the existing lateral acceleration + this should avoid accelerating when losing the target in turns + """ + + a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.)) + + a_target[1] = min(a_target[1], a_x_allowed) + return a_target + class PCCState(object): # Possible state of the ACC system, following the DI_cruiseState naming @@ -133,6 +163,17 @@ def __init__(self,carcontroller): self.wd5=0 self.wd6=0 self.wd7=0 + #for smoothing the changes in speed + self.v_acc_start = 0.0 + self.a_acc_start = 0.0 + self.acc_start_time = sec_since_boot() + self.v_acc = 0.0 + self.v_acc_sol = 0.0 + self.v_acc_future = 0.0 + self.a_acc = 0.0 + self.a_acc_sol = 0.0 + self.v_cruise = 0.0 + self.a_cruise = 0.0 def reset(self, v_pid): self.pid.reset() @@ -144,7 +185,7 @@ def update_stat(self,CS, enabled, sendcan): self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), (CP.longitudinalKiBP, CP.longitudinalKiV), rate=100.0, - sat_limit=0.8) #,convert=compute_gb) + sat_limit=0.8) self.reset(0.) can_sends = [] @@ -199,6 +240,7 @@ def update_stat(self,CS, enabled, sendcan): if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True + self.pid.reset() # Increase ACC speed to match current, if applicable. self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.pedal_speed_kph) else: @@ -257,13 +299,12 @@ def update_stat(self,CS, enabled, sendcan): def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): - + cur_time = sec_since_boot() idx = self.pedal_idx self.pedal_idx = (self.pedal_idx + 1) % 16 if not self.pedal_hardware_present or not enabled: return 0.,0,idx - - + following = False # 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 @@ -291,12 +332,50 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): self.v_pid, self.b_pid = self.calc_follow_speed(CS) # cruise speed can't be negative even is user is distracted self.v_pid = max(self.v_pid, 0.) + v_cruise_setpoint = self.v_pid self.pid.pos_limit = accel_max self.pid.neg_limit = - brake_max v_ego_pid = max(CS.v_ego, MIN_CAN_SPEED) + if self.enable_pedal_cruise: + accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) + # TODO: make a separate lookup for jerk tuning + jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] + accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) + v_cruise_setpoint = self.v_pid + self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, + v_cruise_setpoint, + accel_limits[1], accel_limits[0], + jerk_limits[1], jerk_limits[0], + _DT_MPC) + + # cruise speed can't be negative even is user is distracted + self.v_cruise = max(self.v_cruise, 0.) + else: + a_ego = min(CS.aEgo, 0.0) + reset_speed = CS.vEgo + reset_accel = a_ego + self.v_acc = reset_speed + self.a_acc = reset_accel + self.v_acc_start = reset_speed + self.a_acc_start = reset_accel + self.v_cruise = reset_speed + self.a_cruise = reset_accel + self.v_acc_sol = reset_speed + self.a_acc_sol = reset_accel + self.pid.reset() + self.v_pid = v_ego_pid + + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise + + # Interpolation of trajectory + dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) + self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 + deadzone = interp(v_ego_pid, CS.CP.longPidDeadzoneBP, CS.CP.longPidDeadzoneV) #BBAD adding overridet to pid to see if we can engage sooner @@ -304,9 +383,12 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... # it's all about testing now. - - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= 0.00000005 * self.prev_tesla_accel, freeze_integrator=prevent_overshoot) - + aTarget = self.a_acc_sol + vTarget = self.v_acc_sol + vTargetFuture = self.v_acc_future + prevent_overshoot = not CS.CP.stoppingControl and CS.v_ego < 1.5 and vTargetFuture < 0.7 + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= a_target, freeze_integrator=prevent_overshoot) + self.v_pid = v_ego if prevent_overshoot: output_gb = min(output_gb, 0.0) ############################################################## @@ -315,7 +397,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # we use the values from actuator.accel and actuator.brake ############################################################## elif (CS.cstm_btns.get_button_label2("pedal") == "Lng MPC"): - self.b_pid = 1. + self.b_pid = MPC_BRAKE_MULTIPLIER output_gb = actuators.gas - actuators.brake @@ -362,7 +444,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): def decrement_wd(self): self.wd1 = max (self.wd1-1 , 0) self.wd2 = max (self.wd2-1 , 0) - self.d3 = max (self.wd3-1 , 0) + self.wd3 = max (self.wd3-1 , 0) self.wd4 = max (self.wd4-1 , 0) self.wd5 = max (self.wd5-1 , 0) self.wd6 = max (self.wd6-1 , 0) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 7746f97c164b35..cf841483e4e5e2 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -166,11 +166,16 @@ def get_epas_parser(CP): class CarState(object): def __init__(self, CP): + #labels for ALCA modes + self.alcaLabels = ["MadMax","Normal","Wifey"] + self.alcaMode = 0 + if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS + # max REAL delta angle for correction vs actuator self.CL_MAX_ANGLE_DELTA_BP = [10., 44.] - self.CL_MAX_ANGLE_DELTA = [1.8, .3] + self.CL_MAX_ANGLE_DELTA = [2.2, .3] # adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother self.CL_ADJUST_FACTOR_BP = [10., 44.] @@ -298,9 +303,10 @@ def __init__(self, CP): # Actual cruise speed currently active on the car. self.v_cruise_actual = 0.0 + def init_ui_buttons(self): btns = [] - btns.append(UIButton("alca","ALC",0,"",0)) + btns.append(UIButton("alca","ALC",0,self.alcaLabels[self.alcaMode],0)) if self.pedal_hardware_present: btns.append(UIButton("pedal","PDL",0,"Longit",1)) else: @@ -332,7 +338,16 @@ def config_ui_buttons(self,pedalPresent): def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: - if (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="acc": + if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: + self.alcaMode += 1 + if self.alcaMode >= 3: + self.alcaMode = 0 + else: + self.alcaMode = 0 + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] + self.cstm_btns.hasChanges = True + elif (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="acc": #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" @@ -405,7 +420,7 @@ def update(self, cp, epas_cp): self.a_ego = float(v_ego_x[1]) #BB this is a hack for the interceptor - self.pedal_hardware_present = epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] != 0 + self.pedal_hardware_present = True #epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] != 0 #print "Pedal present? %s" % (self.pedal_hardware_present) #print "Pedal value = %s" % (epas_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']) if self.pedal_hardware_present != self.pedal_hardware_present_prev: From 716f62d65a4b0dce48a3be6884973c07258478e3 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Thu, 18 Oct 2018 20:56:09 -0400 Subject: [PATCH 07/20] 3x brake multiplier --- selfdrive/car/tesla/PCC_module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 27cd44793cf7ce..a897102fb3b539 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -17,7 +17,7 @@ import numpy as np -MPC_BRAKE_MULTIPLIER = 2. +MPC_BRAKE_MULTIPLIER = 3. # TODO: these should end up in values.py at some point, probably variable by trim # Accel limits From 65228490b21f26ff7ca2cfd59266917f16b855eb Mon Sep 17 00:00:00 2001 From: BogGyver Date: Fri, 19 Oct 2018 00:35:26 -0400 Subject: [PATCH 08/20] adding ACC girly mode for girly cars; toggle between MadMax, Normal and Wifey --- selfdrive/car/honda/carstate.py | 13 ++++++++++++- selfdrive/car/modules/ALCA_module.py | 23 +++++++++++++++++------ selfdrive/car/tesla/carstate.py | 4 +--- selfdrive/car/toyota/carstate.py | 13 ++++++++++++- 4 files changed, 42 insertions(+), 11 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f295e266b0b5ba..caa7a785f29b7e 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -135,6 +135,9 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): + #labels for ALCA modes + self.alcaLabels = ["MadMax","Normal","Wifey"] + self.alcaMode = 0 #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -226,7 +229,7 @@ def __init__(self, CP): #BB init ui buttons def init_ui_buttons(self): btns = [] - btns.append(UIButton("alca", "ALC", 0, "", 0)) + btns.append(UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 0)) btns.append(UIButton("", "", 0, "", 1)) btns.append(UIButton("", "", 0, "", 2)) btns.append(UIButton("sound", "SND", 1, "", 3)) @@ -237,6 +240,14 @@ def init_ui_buttons(self): #BB update ui buttons def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: + if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: + self.alcaMode = (self.alcaMode + 1 ) % 3 + else: + self.alcaMode = 0 + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] + self.cstm_btns.hasChanges = True + else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: self.cstm_btns.btns[id].btn_status = btn_status diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 85fa99994072bc..007494cf0e0729 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -160,13 +160,24 @@ def set_pid(self,CS): k_f=CS.CP.steerKf, pos_limit=1.0) def update_angle(self,enabled,CS,frame,actuators): + alca_m1 = 1. + alca_m2 = 1. + alca_m3 = 1. + if CS.alcaMode == 1: + alca_m1 =.9 + alca_m2 = 1. + alca_m3 = 0.5 + if CS.alcaMode == 2: + alca_m1 = 0.8 + alca_m2 = 1.7 + alca_m3 = 0.5 # speed variable parameters - cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) - cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) - cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) - cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) + cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) * alca_m1 + cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) * alca_m3 + cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) * alca_m2 + cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) * alca_m2 cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP, CS.CL_LANE_DETECT_FACTOR) - cl_max_a = interp(CS.v_ego, CS.CL_MAX_A, CS.CL_MAX_A_BP) + cl_max_a = interp(CS.v_ego, CS.CL_MAX_A_BP, CS.CL_MAX_A) cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR) cl_reentry_angle = interp(CS.v_ego, CS.CL_REENTRY_ANGLE_BP, CS.CL_REENTRY_ANGLE) cl_min_v = CS.CL_MIN_V @@ -435,7 +446,7 @@ def update(self,enabled,CS,frame,actuators): steers_max = interp(CS.v_ego, CS.CP.steerMaxBP, CS.CP.steerMaxV) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - output_steer = self.pid.update(new_angle, CS.angle_steers , check_saturation=(CS.v_ego > 10), override=CS.steer_override, feedforward=new_angle, speed=CS.v_ego, deadzone=0.0) + output_steer = self.pid.update(new_angle, CS.angle_steers , check_saturation=(CS.v_ego > 10), override=CS.steer_override, feedforward=new_angle * (CS.v_ego ** 2), speed=CS.v_ego, deadzone=0.0) else: output_steer = actuators.steer return [new_angle,output_steer,new_ALCA_Enabled,new_turn_signal] diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index cf841483e4e5e2..6e3446e2dc906f 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -340,9 +340,7 @@ def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: - self.alcaMode += 1 - if self.alcaMode >= 3: - self.alcaMode = 0 + self.alcaMode = (self.alcaMode + 1 ) % 3 else: self.alcaMode = 0 self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3cf4d70f536463..44f7025fa53ab8 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -70,6 +70,9 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): + #labels for ALCA modes + self.alcaLabels = ["MadMax","Normal","Wifey"] + self.alcaMode = 0 #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -150,7 +153,7 @@ def __init__(self, CP): #BB init ui buttons def init_ui_buttons(self): btns = [] - btns.append(UIButton("alca", "ALC", 0, "", 0)) + btns.append(UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 0)) btns.append(UIButton("", "", 0, "", 1)) btns.append(UIButton("", "", 0, "", 2)) btns.append(UIButton("sound", "SND", 1, "", 3)) @@ -161,6 +164,14 @@ def init_ui_buttons(self): #BB update ui buttons def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: + if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: + self.alcaMode = (self.alcaMode + 1 ) % 3 + else: + self.alcaMode = 0 + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] + self.cstm_btns.hasChanges = True + else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: self.cstm_btns.btns[id].btn_status = btn_status From d627302595683eb937d6418ad0f4cb30d7ae82f3 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Fri, 19 Oct 2018 12:43:59 -0400 Subject: [PATCH 09/20] changed some logic for ALCA button handling for mode --- selfdrive/car/honda/carstate.py | 13 ++++++------- selfdrive/car/modules/ALCA_module.py | 29 ++++++++++++++-------------- selfdrive/car/tesla/PCC_module.py | 2 +- selfdrive/car/tesla/carstate.py | 15 +++++++------- selfdrive/car/toyota/carstate.py | 13 ++++++------- 5 files changed, 34 insertions(+), 38 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index caa7a785f29b7e..809d4e53f92faa 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -137,7 +137,6 @@ class CarState(object): def __init__(self, CP): #labels for ALCA modes self.alcaLabels = ["MadMax","Normal","Wifey"] - self.alcaMode = 0 #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -241,12 +240,12 @@ def init_ui_buttons(self): def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: - self.alcaMode = (self.alcaMode + 1 ) % 3 - else: - self.alcaMode = 0 - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] - self.cstm_btns.hasChanges = True + alcaMode = 0 + for i in range(0,len(self.alcaLabels)): + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: + alcaMode = (i + 1 ) % len(self.alcaLabels) + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] + self.cstm_btns.hasChanges = True else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 007494cf0e0729..66f1d431092bcd 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -160,22 +160,21 @@ def set_pid(self,CS): k_f=CS.CP.steerKf, pos_limit=1.0) def update_angle(self,enabled,CS,frame,actuators): - alca_m1 = 1. - alca_m2 = 1. - alca_m3 = 1. - if CS.alcaMode == 1: - alca_m1 =.9 - alca_m2 = 1. - alca_m3 = 0.5 - if CS.alcaMode == 2: - alca_m1 = 0.8 - alca_m2 = 1.7 - alca_m3 = 0.5 + alca_b = CS.cstm_btns.get_button("alca") + alcaMode = 0 + if alca_b: + for i in range(0,len(CS.alcaLabels)): + if alca_b.btn_label2 == CS.alcaLabels[i]: + alcaMode = i + #parameters that define the speed/aggressiveness of lane change modes + alca_m1 = [1., .9, .8] + alca_m2 = [1., 1., 1.7] + alca_m3 = [1., 0.5, 0.5] # speed variable parameters - cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) * alca_m1 - cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) * alca_m3 - cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) * alca_m2 - cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) * alca_m2 + cl_max_angle_delta = interp(CS.v_ego,CS.CL_MAX_ANGLE_DELTA_BP,CS.CL_MAX_ANGLE_DELTA) * alca_m1[alcaMode] + cl_maxd_a = interp(CS.v_ego, CS.CL_MAXD_BP, CS.CL_MAXD_A) * alca_m3[alcaMode] + cl_lane_pass_time = interp(CS.v_ego,CS.CL_LANE_PASS_BP,CS.CL_LANE_PASS_TIME) * alca_m2[alcaMode] + cl_timea_t = interp(CS.v_ego,CS.CL_TIMEA_BP,CS.CL_TIMEA_T) * alca_m2[alcaMode] cl_lane_detect_factor = interp(CS.v_ego, CS.CL_LANE_DETECT_BP, CS.CL_LANE_DETECT_FACTOR) cl_max_a = interp(CS.v_ego, CS.CL_MAX_A_BP, CS.CL_MAX_A) cl_adjust_factor = interp(CS.v_ego, CS.CL_ADJUST_FACTOR_BP, CS.CL_ADJUST_FACTOR) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index a897102fb3b539..43578222ec8741 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -17,7 +17,7 @@ import numpy as np -MPC_BRAKE_MULTIPLIER = 3. +MPC_BRAKE_MULTIPLIER = 6. # TODO: these should end up in values.py at some point, probably variable by trim # Accel limits diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 6e3446e2dc906f..16813ecbad791b 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -168,7 +168,6 @@ class CarState(object): def __init__(self, CP): #labels for ALCA modes self.alcaLabels = ["MadMax","Normal","Wifey"] - self.alcaMode = 0 if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS @@ -306,7 +305,7 @@ def __init__(self, CP): def init_ui_buttons(self): btns = [] - btns.append(UIButton("alca","ALC",0,self.alcaLabels[self.alcaMode],0)) + btns.append(UIButton("alca","ALC",0,self.alcaLabels[1],0)) if self.pedal_hardware_present: btns.append(UIButton("pedal","PDL",0,"Longit",1)) else: @@ -339,12 +338,12 @@ def config_ui_buttons(self,pedalPresent): def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: - self.alcaMode = (self.alcaMode + 1 ) % 3 - else: - self.alcaMode = 0 - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] - self.cstm_btns.hasChanges = True + alcaMode = 0 + for i in range(0,len(self.alcaLabels)): + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: + alcaMode = (i + 1 ) % len(self.alcaLabels) + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] + self.cstm_btns.hasChanges = True elif (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="acc": #don't change status, just model if (self.cstm_btns.btns[id].btn_label2 == "Mod OP"): diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 44f7025fa53ab8..b813d4ad66fbc1 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -72,7 +72,6 @@ class CarState(object): def __init__(self, CP): #labels for ALCA modes self.alcaLabels = ["MadMax","Normal","Wifey"] - self.alcaMode = 0 #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -165,12 +164,12 @@ def init_ui_buttons(self): def update_ui_buttons(self,id,btn_status): if self.cstm_btns.btns[id].btn_status > 0: if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[self.alcaMode]: - self.alcaMode = (self.alcaMode + 1 ) % 3 - else: - self.alcaMode = 0 - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[self.alcaMode] - self.cstm_btns.hasChanges = True + alcaMode = 0 + for i in range(0,len(self.alcaLabels)): + if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: + alcaMode = (i + 1 ) % len(self.alcaLabels) + self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] + self.cstm_btns.hasChanges = True else: self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status else: From c88f1a3b4db158b31845e81c99ae54038285cc16 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Fri, 19 Oct 2018 12:50:12 -0400 Subject: [PATCH 10/20] 6x brake acutator to Regen (testing); fix for some Follow errors --- selfdrive/car/tesla/PCC_module.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 43578222ec8741..249d8bfe7c4e7b 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -340,7 +340,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): v_ego_pid = max(CS.v_ego, MIN_CAN_SPEED) if self.enable_pedal_cruise: - accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) + accel_limits = map(float, calc_cruise_accel_limits(CS.v_ego, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) @@ -355,7 +355,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): self.v_cruise = max(self.v_cruise, 0.) else: a_ego = min(CS.aEgo, 0.0) - reset_speed = CS.vEgo + reset_speed = CS.v_ego reset_accel = a_ego self.v_acc = reset_speed self.a_acc = reset_accel @@ -388,7 +388,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): vTargetFuture = self.v_acc_future prevent_overshoot = not CS.CP.stoppingControl and CS.v_ego < 1.5 and vTargetFuture < 0.7 output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= a_target, freeze_integrator=prevent_overshoot) - self.v_pid = v_ego + self.v_pid = CS.v_ego if prevent_overshoot: output_gb = min(output_gb, 0.0) ############################################################## From b976132f7fad55ec2f0faf1b8d3f1a7ed0b94c99 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 10:58:29 -0400 Subject: [PATCH 11/20] refactoring UIBT module for multi-label --- selfdrive/car/modules/ALCA_module.py | 7 +-- selfdrive/car/modules/UIBT_module.py | 42 +++++++++++++++-- selfdrive/car/tesla/ACC_module.py | 2 +- selfdrive/car/tesla/PCC_module.py | 2 +- selfdrive/car/tesla/carstate.py | 67 ++++++++-------------------- 5 files changed, 61 insertions(+), 59 deletions(-) diff --git a/selfdrive/car/modules/ALCA_module.py b/selfdrive/car/modules/ALCA_module.py index 66f1d431092bcd..95f076c2c420a4 100644 --- a/selfdrive/car/modules/ALCA_module.py +++ b/selfdrive/car/modules/ALCA_module.py @@ -160,12 +160,7 @@ def set_pid(self,CS): k_f=CS.CP.steerKf, pos_limit=1.0) def update_angle(self,enabled,CS,frame,actuators): - alca_b = CS.cstm_btns.get_button("alca") - alcaMode = 0 - if alca_b: - for i in range(0,len(CS.alcaLabels)): - if alca_b.btn_label2 == CS.alcaLabels[i]: - alcaMode = i + alcaMode = CS.cstm_btns.get_button_label2_index("alca") #parameters that define the speed/aggressiveness of lane change modes alca_m1 = [1., .9, .8] alca_m2 = [1., 1., 1.7] diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index d4217591920f92..ea41fa5d800a12 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -88,7 +88,7 @@ def __init__(self, carstate,car,folder): self.hasChanges = True self.last_in_read_time = datetime.min if os.path.exists(self.buttons_labels_path): - self.btns = self.CS.init_ui_buttons() + self.btns = self.init_ui_buttons() #there is a file, load it if self.read_buttons_labels_from_file(): self.read_buttons_out_file() @@ -99,7 +99,7 @@ def __init__(self, carstate,car,folder): self.write_buttons_out_file() else: #there is no file, create it - self.btns = self.CS.init_ui_buttons() + self.btns = self.init_ui_buttons() self.hasChanges = True self.write_buttons_labels_to_file() self.write_buttons_out_file() @@ -108,6 +108,12 @@ def __init__(self, carstate,car,folder): self.send_button_info() self.CS.UE.uiSetCarEvent(self.car_folder,self.car_name) + def init_ui_buttons(self): + self.btns = [] + self.CS.init_ui_buttons() + for i in range(0,len(self.CS.btns_init)): + self.btns.append(UIButton(self.CS.btns_init[i][0],self.CS.btns_init[i][1],1,self.CS.btns_init[i][2][0],i)) + def get_button(self, btn_name): for button in self.btns: if button.btn_name.strip() == btn_name: @@ -135,15 +141,18 @@ def set_button_status(self,btn_name,btn_status): def set_button_status_from_ui(self,id,btn_status): old_btn_status = self.btns[id].btn_status + old_btn_label2 = self.btns[id].btn_label2 if old_btn_status * btn_status == 0 and old_btn_status != btn_status: self.hasChanges = True - self.CS.update_ui_buttons(id,btn_status) + self.update_ui_buttons(id,btn_status) new_btn_status = self.btns[id].btn_status if new_btn_status * btn_status == 0 and new_btn_status != btn_status: self.hasChanges = True if self.hasChanges: self.CS.UE.uiButtonInfoEvent(id,self.btns[id].btn_name, \ self.btns[id].btn_label,self.btns[id].btn_status,self.btns[id].btn_label2) + if old_btn_label2 != self.btns[id].btn_label2: + self.write_buttons_labels_to_file() self.write_buttons_out_file() @@ -153,3 +162,30 @@ def get_button_label2(self, btn_name): return btn.btn_label2 else: return -1 + + def get_button_label2_index(self, btn_name): + btn = self.get_button(btn_name) + if btn: + b_index = -1 + bpos = self.CS.cstm_btns.index(btn) + for i in range(0,len(self.CS.btns_init[bpos][2])): + if btn.btn_label2 == self.CS.btns_init[bpos][2][i]: + b_index = i + return b_index + else: + return -1 + + def update_ui_buttons(self,id,btn_status): + if self.CS.cstm_btns.btns[id].btn_status > 0: + #if we have more than one Label2 then we toggle + if (len(self.CS.btns_init[id][2]) > 1): + status2 = 0 + for i in range(0,len(self.CS.btns_init[id][2])): + if self.CS.cstm_btns.btns[id].btn_label2 == self.CS.btns_init[id][2][i]: + status2 = (i + 1 ) % len(self.CS.btns_init[id][2]) + self.CS.cstm_btns.btns[id].btn_label2 = self.CS.btns_init[id][2][status2] + self.CS.cstm_btns.hasChanges = True + else: + self.CS.cstm_btns.btns[id].btn_status = btn_status * self.CS.cstm_btns.btns[id].btn_status + else: + self.CS.cstm_btns.btns[id].btn_status = btn_status diff --git a/selfdrive/car/tesla/ACC_module.py b/selfdrive/car/tesla/ACC_module.py index 63cc3f38831847..a1bc05dff8276e 100644 --- a/selfdrive/car/tesla/ACC_module.py +++ b/selfdrive/car/tesla/ACC_module.py @@ -177,7 +177,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_index("acc") == 1: # 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/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 249d8bfe7c4e7b..a29a0741710b2a 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -328,7 +328,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # once the speed is detected we have to use our own PID to determine # how much accel and break we have to do #################################################################### - if (CS.cstm_btns.get_button_label2("pedal") == "Follow"): + if (CS.cstm_btns.get_button_label2_index("pedal") == 1): self.v_pid, self.b_pid = self.calc_follow_speed(CS) # cruise speed can't be negative even is user is distracted self.v_pid = max(self.v_pid, 0.) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 16813ecbad791b..a99dc760f1ceae 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -167,7 +167,13 @@ def get_epas_parser(CP): class CarState(object): def __init__(self, CP): #labels for ALCA modes - self.alcaLabels = ["MadMax","Normal","Wifey"] + #self.alcaLabels = ["MadMax","Normal","Wifey"] + self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \ + ["acc","ACC",["Mod OP","Mod JJ"]], \ + ["steer","STR",[""]], \ + ["brake","BRK",[""]], \ + ["msg", "MSG",[""]], \ + ["sound", "SND", [""]]] if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS @@ -304,64 +310,29 @@ def __init__(self, CP): def init_ui_buttons(self): - btns = [] - btns.append(UIButton("alca","ALC",0,self.alcaLabels[1],0)) if self.pedal_hardware_present: - btns.append(UIButton("pedal","PDL",0,"Longit",1)) - else: - btns.append(UIButton("acc","ACC",0,"Mod OP",1)) - btns.append(UIButton("steer","STR",0,"",2)) - btns.append(UIButton("brake","BRK",1,"",3)) - btns.append(UIButton("msg","MSG",1,"",4)) - btns.append(UIButton("sound","SND",1,"",5)) - return btns + self.btns_init[1] = ["pedal","PDL",["Lng MPC","Follow"]] def config_ui_buttons(self,pedalPresent): #self.CP.enableGasInterceptor = pedalPresent + hasChanges = False if pedalPresent: btn = self.cstm_btns.get_button("acc") if btn: - btn.btn_name = "pedal" - btn.btn_label = "PDL" - btn.btn_label2 = "Lng MPC" - btn.btn_status = 1 + self.btns_init[1] = ["pedal","PDL",["Lng MPC","Follow"]] + hasChanges = True else: #we don't have pedal interceptor btn = self.cstm_btns.get_button("pedal") if btn: - btn.btn_name = "acc" - btn.btn_label = "ACC" - btn.btn_label2 = "Mod OP" - btn.btn_status = 1 - self.update_ui_buttons(1,1) - - def update_ui_buttons(self,id,btn_status): - if self.cstm_btns.btns[id].btn_status > 0: - if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - alcaMode = 0 - for i in range(0,len(self.alcaLabels)): - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: - alcaMode = (i + 1 ) % len(self.alcaLabels) - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] - self.cstm_btns.hasChanges = True - elif (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="acc": - #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" - else: - self.cstm_btns.btns[id].btn_label2 = "Mod OP" - self.cstm_btns.hasChanges = True - elif (id == 1) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="pedal": - #don't change status, just model - if (self.cstm_btns.btns[id].btn_label2 == "Follow"): - self.cstm_btns.btns[id].btn_label2 = "Lng MPC" - else: - self.cstm_btns.btns[id].btn_label2 = "Follow" - self.cstm_btns.hasChanges = True - else: - self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status - else: - self.cstm_btns.btns[id].btn_status = btn_status + self.btns_init[1] = ["acc","ACC",["Mod OP","Mod JJ"]] + hasChanges = True + if hasChanges: + btn.btn_name = self.btns_init[i][0] + btn.btn_label = self.btns_init[i][1] + btn.btn_label2 = self.btns_init[i][2][0] + btn.btn_status = 1 + self.cstm_btns.update_ui_buttons(1,1) def update(self, cp, epas_cp): From e4f2f7290cd542803d416c78d7577d9b8d05df80 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 11:11:58 -0400 Subject: [PATCH 12/20] moved most of code in UIBT module from each car specific carstate --- selfdrive/car/honda/carstate.py | 35 +++++++--------------------- selfdrive/car/modules/UIBT_module.py | 7 +++++- selfdrive/car/tesla/carstate.py | 4 ++-- selfdrive/car/toyota/carstate.py | 34 ++++++--------------------- 4 files changed, 23 insertions(+), 57 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 809d4e53f92faa..508441a259aa77 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -135,8 +135,14 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): - #labels for ALCA modes - self.alcaLabels = ["MadMax","Normal","Wifey"] + #labels for buttons + self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \ + ["","",[""]], \ + ["","",[""]], \ + ["sound","SND",[""]], \ + ["", "",[""]], \ + ["", "", [""]]] + #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -225,31 +231,6 @@ def __init__(self, CP): K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 - #BB init ui buttons - def init_ui_buttons(self): - btns = [] - btns.append(UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 0)) - btns.append(UIButton("", "", 0, "", 1)) - btns.append(UIButton("", "", 0, "", 2)) - btns.append(UIButton("sound", "SND", 1, "", 3)) - btns.append(UIButton("", "", 0, "", 4)) - btns.append(UIButton("", "", 0, "", 5)) - return btns - - #BB update ui buttons - def update_ui_buttons(self,id,btn_status): - if self.cstm_btns.btns[id].btn_status > 0: - if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - alcaMode = 0 - for i in range(0,len(self.alcaLabels)): - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: - alcaMode = (i + 1 ) % len(self.alcaLabels) - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] - self.cstm_btns.hasChanges = True - else: - self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status - else: - self.cstm_btns.btns[id].btn_status = btn_status def update(self, cp): diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index ea41fa5d800a12..b03147d9a17875 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -110,7 +110,12 @@ def __init__(self, carstate,car,folder): def init_ui_buttons(self): self.btns = [] - self.CS.init_ui_buttons() + try: + self.CS.init_ui_buttons() + print "Buttons iniatlized with custom CS code" + except AttributeError: + # no init method + print "Buttons iniatlized with just base code" for i in range(0,len(self.CS.btns_init)): self.btns.append(UIButton(self.CS.btns_init[i][0],self.CS.btns_init[i][1],1,self.CS.btns_init[i][2][0],i)) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index a99dc760f1ceae..1cbcc3e930d096 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -166,8 +166,7 @@ def get_epas_parser(CP): class CarState(object): def __init__(self, CP): - #labels for ALCA modes - #self.alcaLabels = ["MadMax","Normal","Wifey"] + #labels for buttons self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \ ["acc","ACC",["Mod OP","Mod JJ"]], \ ["steer","STR",[""]], \ @@ -312,6 +311,7 @@ def __init__(self, CP): def init_ui_buttons(self): if self.pedal_hardware_present: self.btns_init[1] = ["pedal","PDL",["Lng MPC","Follow"]] + return 0 def config_ui_buttons(self,pedalPresent): #self.CP.enableGasInterceptor = pedalPresent diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index b813d4ad66fbc1..db13b551c419e3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -70,8 +70,13 @@ def get_can_parser(CP): class CarState(object): def __init__(self, CP): - #labels for ALCA modes - self.alcaLabels = ["MadMax","Normal","Wifey"] + #labels for buttons + self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \ + ["","",[""]], \ + ["","",[""]], \ + ["sound","SND",[""]], \ + ["", "",[""]], \ + ["", "", [""]]] #if (CP.carFingerprint == CAR.MODELS): # ALCA PARAMS # max REAL delta angle for correction vs actuator @@ -149,31 +154,6 @@ def __init__(self, CP): K=np.matrix([[0.12287673], [0.29666309]])) self.v_ego = 0.0 - #BB init ui buttons - def init_ui_buttons(self): - btns = [] - btns.append(UIButton("alca", "ALC", 0, self.alcaLabels[self.alcaMode], 0)) - btns.append(UIButton("", "", 0, "", 1)) - btns.append(UIButton("", "", 0, "", 2)) - btns.append(UIButton("sound", "SND", 1, "", 3)) - btns.append(UIButton("", "", 0, "", 4)) - btns.append(UIButton("", "", 0, "", 5)) - return btns - - #BB update ui buttons - def update_ui_buttons(self,id,btn_status): - if self.cstm_btns.btns[id].btn_status > 0: - if (id == 0) and (btn_status == 0) and self.cstm_btns.btns[id].btn_name=="alca": - alcaMode = 0 - for i in range(0,len(self.alcaLabels)): - if self.cstm_btns.btns[id].btn_label2 == self.alcaLabels[i]: - alcaMode = (i + 1 ) % len(self.alcaLabels) - self.cstm_btns.btns[id].btn_label2 = self.alcaLabels[alcaMode] - self.cstm_btns.hasChanges = True - else: - self.cstm_btns.btns[id].btn_status = btn_status * self.cstm_btns.btns[id].btn_status - else: - self.cstm_btns.btns[id].btn_status = btn_status def update(self, cp): # copy can_valid From 8225ac66d06732f789238bc134b63c285f9f72fd Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 11:17:16 -0400 Subject: [PATCH 13/20] adding check to see if label2 is a valid option --- selfdrive/car/modules/UIBT_module.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index b03147d9a17875..825e9eb138b9d5 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -37,13 +37,17 @@ def read_buttons_labels_from_file(self): name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) if (self.btns[j].btn_name != name.rstrip("\0")): file_matches = False - #we have all the da;ta and it matches + #we have all the data and it matches if file_matches: for i in range(0, len(indata), btn_msg_len): j = int(i/btn_msg_len) name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) self.btns[j].btn_label = label.rstrip("\0") - self.btns[j].btn_label2 = label2.rstrip("\0") + #check if label is actually a valid option + if label2.rstrip("\0") in self.CS.btns_init[j][2]: + self.btns[j].btn_label2 = label2.rstrip("\0") + else: + self.btns[j].btn_label2 = self.CS.btns_init[j][2][0] return file_matches else: #we don't have all the data, ignore From 83d5c298b4d5c70f9bb7ad7c5701a44cd0aa7968 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 12:01:36 -0400 Subject: [PATCH 14/20] changes to PCC for PID optimization --- selfdrive/car/tesla/PCC_module.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index a29a0741710b2a..ec6937efaf602f 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -366,7 +366,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): self.v_acc_sol = reset_speed self.a_acc_sol = reset_accel self.pid.reset() - self.v_pid = v_ego_pid + self.v_pid = reset_speed self.v_acc = self.v_cruise self.a_acc = self.a_cruise @@ -385,10 +385,11 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # it's all about testing now. aTarget = self.a_acc_sol vTarget = self.v_acc_sol + self.v_acc_future = self.v_pid vTargetFuture = self.v_acc_future prevent_overshoot = not CS.CP.stoppingControl and CS.v_ego < 1.5 and vTargetFuture < 0.7 + self.v_pid = v_target output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= a_target, freeze_integrator=prevent_overshoot) - self.v_pid = CS.v_ego if prevent_overshoot: output_gb = min(output_gb, 0.0) ############################################################## From 5f2ccdb3f7ab7ecc283f8c215ca91810c7f7a103 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 12:25:10 -0400 Subject: [PATCH 15/20] bug fix in UIBT --- selfdrive/car/modules/UIBT_module.py | 6 +++--- selfdrive/car/tesla/carstate.py | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index 825e9eb138b9d5..9b5293c247a1de 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -92,7 +92,7 @@ def __init__(self, carstate,car,folder): self.hasChanges = True self.last_in_read_time = datetime.min if os.path.exists(self.buttons_labels_path): - self.btns = self.init_ui_buttons() + self.init_ui_buttons() #there is a file, load it if self.read_buttons_labels_from_file(): self.read_buttons_out_file() @@ -103,7 +103,7 @@ def __init__(self, carstate,car,folder): self.write_buttons_out_file() else: #there is no file, create it - self.btns = self.init_ui_buttons() + self.init_ui_buttons() self.hasChanges = True self.write_buttons_labels_to_file() self.write_buttons_out_file() @@ -176,7 +176,7 @@ def get_button_label2_index(self, btn_name): btn = self.get_button(btn_name) if btn: b_index = -1 - bpos = self.CS.cstm_btns.index(btn) + bpos = self.btns.index(btn) for i in range(0,len(self.CS.btns_init[bpos][2])): if btn.btn_label2 == self.CS.btns_init[bpos][2][i]: b_index = i diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 1cbcc3e930d096..a728eccd4d0b26 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -328,6 +328,7 @@ def config_ui_buttons(self,pedalPresent): self.btns_init[1] = ["acc","ACC",["Mod OP","Mod JJ"]] hasChanges = True if hasChanges: + i = 1 btn.btn_name = self.btns_init[i][0] btn.btn_label = self.btns_init[i][1] btn.btn_label2 = self.btns_init[i][2][0] From 17780a22a4537db8308253673df40a47afaa7ac8 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 12:43:04 -0400 Subject: [PATCH 16/20] fixing code for multistate buttons --- selfdrive/car/modules/UIBT_module.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/car/modules/UIBT_module.py b/selfdrive/car/modules/UIBT_module.py index 9b5293c247a1de..b4d562956cc5df 100644 --- a/selfdrive/car/modules/UIBT_module.py +++ b/selfdrive/car/modules/UIBT_module.py @@ -37,8 +37,10 @@ def read_buttons_labels_from_file(self): name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) if (self.btns[j].btn_name != name.rstrip("\0")): file_matches = False + print "Btn file does not match" #we have all the data and it matches if file_matches: + print "Btn file matches" for i in range(0, len(indata), btn_msg_len): j = int(i/btn_msg_len) name,label,label2 = struct.unpack(btn_msg_struct, indata[i:i+btn_msg_len]) @@ -46,6 +48,7 @@ def read_buttons_labels_from_file(self): #check if label is actually a valid option if label2.rstrip("\0") in self.CS.btns_init[j][2]: self.btns[j].btn_label2 = label2.rstrip("\0") + print "Set label2 from file" else: self.btns[j].btn_label2 = self.CS.btns_init[j][2][0] return file_matches From da3e400c6f7c87b55476843bdaf188e282e46505 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 13:05:40 -0400 Subject: [PATCH 17/20] fixes PID for Tesla Follow --- selfdrive/car/tesla/PCC_module.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index ec6937efaf602f..00aba3c0412046 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -343,7 +343,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): accel_limits = map(float, calc_cruise_accel_limits(CS.v_ego, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] - accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) + accel_limits = limit_accel_in_turns(CS.v_ego, CS.angle_steers, accel_limits, CS.CP) v_cruise_setpoint = self.v_pid self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, v_cruise_setpoint, @@ -354,7 +354,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: - a_ego = min(CS.aEgo, 0.0) + a_ego = min(CS.a_ego, 0.0) reset_speed = CS.v_ego reset_accel = a_ego self.v_acc = reset_speed @@ -388,8 +388,8 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): self.v_acc_future = self.v_pid vTargetFuture = self.v_acc_future prevent_overshoot = not CS.CP.stoppingControl and CS.v_ego < 1.5 and vTargetFuture < 0.7 - self.v_pid = v_target - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= a_target, freeze_integrator=prevent_overshoot) + self.v_pid = vTarget + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= aTarget, freeze_integrator=prevent_overshoot) if prevent_overshoot: output_gb = min(output_gb, 0.0) ############################################################## From 2072ecc5193e8dded083faae68dec1a2aca51323 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 13:11:23 -0400 Subject: [PATCH 18/20] force pedal hardware = True in this branch --- selfdrive/car/tesla/carstate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index a728eccd4d0b26..78cf38932f65c8 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -309,8 +309,8 @@ def __init__(self, CP): def init_ui_buttons(self): - if self.pedal_hardware_present: - self.btns_init[1] = ["pedal","PDL",["Lng MPC","Follow"]] + #if self.pedal_hardware_present: + self.btns_init[1] = ["pedal","PDL",["Lng MPC","Follow"]] return 0 def config_ui_buttons(self,pedalPresent): From 4b4f0c441ae95ba8dfc1cb4b0127daf2040d16d9 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 16:02:11 -0400 Subject: [PATCH 19/20] pedal changes with new longcontrol for PCC Follow --- selfdrive/car/tesla/PCC_module.py | 103 ++++++++---------- selfdrive/car/tesla/interface.py | 14 +-- selfdrive/car/tesla/longcontrol_tesla.py | 131 +++++++++++++++++++++++ 3 files changed, 181 insertions(+), 67 deletions(-) create mode 100644 selfdrive/car/tesla/longcontrol_tesla.py diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index 00aba3c0412046..f2856c95333bd1 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -1,5 +1,8 @@ from selfdrive.car.tesla import teslacan -from selfdrive.controls.lib.pid import PIController +#from selfdrive.controls.lib.pid import PIController +from selfdrive.car.tesla.longcontrol_tesla import LongControl, STARTING_TARGET_SPEED +from selfdrive.car.tesla.interface import tesla_compute_gb +from selfdrive.car.tesla import teslacan from common.numpy_fast import clip, interp from selfdrive.services import service_list from selfdrive.car.tesla.values import AH,CruiseState, CruiseButtons, CAR @@ -68,7 +71,7 @@ A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) _DT = 0.01 # 100Hz -_DT_MPC = 0.2 # 5Hz +_DT_MPC = 0.01 # 100Hz in our case def calc_cruise_accel_limits(v_ego, following): a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) @@ -149,7 +152,6 @@ def __init__(self,carcontroller): self.PedalForZeroTorque = 18. #starting number, works on my S85 self.lastTorqueForPedalForZeroTorque = TORQUE_LEVEL_DECEL self.follow_time = FOLLOW_UP_TIME # in seconds - self.pid = None self.v_pid = 0. self.a_pid = 0. self.b_pid = 0. @@ -174,19 +176,17 @@ def __init__(self,carcontroller): self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 + #Long Control + self.LoC = None def reset(self, v_pid): - self.pid.reset() - self.v_pid = v_pid + if self.LoC: + self.LoC.reset(v_pid) def update_stat(self,CS, enabled, sendcan): - if self.pid == None: - CP = CS.CP - self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), - (CP.longitudinalKiBP, CP.longitudinalKiV), - rate=100.0, - sat_limit=0.8) - self.reset(0.) + if self.LoC == None: + self.LoC = LongControl(CS.CP, tesla_compute_gb) + can_sends = [] #BBTODO: a better way to engage the pedal early and reset its CAN @@ -240,7 +240,7 @@ def update_stat(self,CS, enabled, sendcan): if ready and double_pull: # A double pull enables ACC. updating the max ACC speed if necessary. self.enable_pedal_cruise = True - self.pid.reset() + self.LoC.reset(CS.v_ego) # Increase ACC speed to match current, if applicable. self.pedal_speed_kph = max(CS.v_ego * CV.MS_TO_KPH, self.pedal_speed_kph) else: @@ -332,10 +332,7 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): self.v_pid, self.b_pid = self.calc_follow_speed(CS) # cruise speed can't be negative even is user is distracted self.v_pid = max(self.v_pid, 0.) - v_cruise_setpoint = self.v_pid - - self.pid.pos_limit = accel_max - self.pid.neg_limit = - brake_max + self.b_pid *= MPC_BRAKE_MULTIPLIER v_ego_pid = max(CS.v_ego, MIN_CAN_SPEED) @@ -344,54 +341,40 @@ def update_pdl(self,enabled,CS,frame,actuators,pcm_speed): # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.v_ego, CS.angle_steers, accel_limits, CS.CP) - v_cruise_setpoint = self.v_pid self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start, - v_cruise_setpoint, + self.v_pid, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) + self.v_acc = self.v_cruise + self.a_acc = self.a_cruise + + self.v_acc_future = self.v_pid + vTargetFuture = self.v_acc_future + + # Interpolation of trajectory + dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps + self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) + self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 + + + # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... + # it's all about testing now. + aTarget = self.a_acc_sol + vTarget = self.v_acc_sol + + t_go, t_brake = self.LoC.update(self.enable_pedal_cruise, CS.v_ego, CS.brake_pressed != 0, CS.standstill, False, + self.v_pid, vTarget, vTargetFuture, aTarget, + CS.CP, self.lead_1) + output_gb = t_go - t_brake else: - a_ego = min(CS.a_ego, 0.0) - reset_speed = CS.v_ego - reset_accel = a_ego - self.v_acc = reset_speed - self.a_acc = reset_accel - self.v_acc_start = reset_speed - self.a_acc_start = reset_accel - self.v_cruise = reset_speed - self.a_cruise = reset_accel - self.v_acc_sol = reset_speed - self.a_acc_sol = reset_accel - self.pid.reset() - self.v_pid = reset_speed - - self.v_acc = self.v_cruise - self.a_acc = self.a_cruise - - # Interpolation of trajectory - dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) - self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 - - deadzone = interp(v_ego_pid, CS.CP.longPidDeadzoneBP, CS.CP.longPidDeadzoneV) - - #BBAD adding overridet to pid to see if we can engage sooner - override = self.enable_pedal_cruise and CS.v_ego * CV.MS_TO_KPH > self.pedal_speed_kph and CS.user_pedal_pressed - - # we will try to feed forward the pedal position.... we might want to feed the last output_gb.... - # it's all about testing now. - aTarget = self.a_acc_sol - vTarget = self.v_acc_sol - self.v_acc_future = self.v_pid - vTargetFuture = self.v_acc_future - prevent_overshoot = not CS.CP.stoppingControl and CS.v_ego < 1.5 and vTargetFuture < 0.7 - self.v_pid = vTarget - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, override=override, deadzone=deadzone, feedforward= aTarget, freeze_integrator=prevent_overshoot) - if prevent_overshoot: - output_gb = min(output_gb, 0.0) + self.LoC.reset(CS.v_ego) + output_gb = 0. + + ############################################################## # this mode (Lng MPC) uses the longitudinal MPC built in OP # @@ -492,12 +475,12 @@ def calc_follow_speed(self, CS): if lead_dist == 0 and new_speed > self.pedal_speed_kph: msg = "Slow to max" new_speed -= SPEED_DOWN - new_brake =0.5 + new_brake = 1. # 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 CS.angle_steers < 5.0: - if new_speed < self.pedal_speed_kph + SPEED_UP: + if new_speed <= self.pedal_speed_kph + SPEED_UP: msg = "Accel to max" new_speed += SPEED_UP # if we have a populated lead_distance @@ -600,7 +583,7 @@ def calc_follow_speed(self, CS): new_brake = 0.2 if msg: print msg - new_speed = clip(new_speed, 0, self.pedal_speed_kph) + #new_speed = clip(new_speed, 0, self.pedal_speed_kph) new_speed = clip(new_speed,MIN_PCC_V,MAX_PCC_V) self.last_speed = new_speed return new_speed * CV.KPH_TO_MS, new_brake diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 3f40b7036800fb..e578c4474ef301 100644 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -19,13 +19,13 @@ K_MULT = .8 K_MULTi = 280. def tesla_compute_gb(accel, speed): - creep_brake = 0.0 - creep_speed = 2.3 - creep_brake_value = 0.15 - if speed < creep_speed: - creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value - return float(accel) / 4.8 - creep_brake - #return float(accel) + #creep_brake = 0.0 + #creep_speed = 2.3 + #creep_brake_value = 0.15 + #if speed < creep_speed: + # creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + #return float(accel) / 4.8 - creep_brake + return float(accel) class CarInterface(object): diff --git a/selfdrive/car/tesla/longcontrol_tesla.py b/selfdrive/car/tesla/longcontrol_tesla.py new file mode 100644 index 00000000000000..633269670983bf --- /dev/null +++ b/selfdrive/car/tesla/longcontrol_tesla.py @@ -0,0 +1,131 @@ +from common.numpy_fast import clip, interp +from selfdrive.controls.lib.pid import PIController + +STOPPING_EGO_SPEED = 0.5 +MIN_CAN_SPEED = 0.3 #TODO: parametrize this in car interface +STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 +STARTING_TARGET_SPEED = 0.5 +BRAKE_THRESHOLD_TO_PID = 0.2 + + +class LongCtrlState: + #*** this function handles the long control state transitions + # long_control_state labels (see capnp enum): + off = 'off' # Off + pid = 'pid' # moving and tracking targets, with PID control running + stopping = 'stopping' # stopping and changing controls to almost open loop as PID does not fit well at such a low speed + starting = 'starting' # starting and releasing brakes in open loop before giving back to PID + + + +def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, + output_gb, brake_pressed, cruise_standstill): + + stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ + (v_ego < STOPPING_EGO_SPEED and \ + ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or + brake_pressed)) + + starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill + + if not active: + long_control_state = LongCtrlState.off + + else: + if long_control_state == LongCtrlState.off: + if active: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.pid: + if stopping_condition: + long_control_state = LongCtrlState.stopping + + elif long_control_state == LongCtrlState.stopping: + if starting_condition: + long_control_state = LongCtrlState.starting + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif output_gb >= -BRAKE_THRESHOLD_TO_PID: + long_control_state = LongCtrlState.pid + + return long_control_state + + +stopping_brake_rate = 0.2 # brake_travel/s while trying to stop +starting_brake_rate = 0.8 # brake_travel/s while releasing on restart +brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary + +_MAX_SPEED_ERROR_BP = [0., 30.] # speed breakpoints +_MAX_SPEED_ERROR_V = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp + + +class LongControl(object): + def __init__(self, CP, compute_gb): + self.long_control_state = LongCtrlState.off # initialized to off + self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV), + (CP.longitudinalKiBP, CP.longitudinalKiV), + rate=100.0, + sat_limit=0.8, + convert=compute_gb) + self.v_pid = 0.0 + self.last_output_gb = 0.0 + + def reset(self, v_pid): + self.pid.reset() + self.v_pid = v_pid + + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1): + # actuation limits + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) + + output_gb = self.last_output_gb + rate = 100.0 + self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego, + v_target_future, self.v_pid, output_gb, + brake_pressed, cruise_standstill) + + v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + + # *** long control behavior based on state + if self.long_control_state == LongCtrlState.off: + self.v_pid = v_ego_pid # do nothing + output_gb = 0. + self.pid.reset() + + # tracking objects and driving + elif self.long_control_state == LongCtrlState.pid: + prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7 + + self.v_pid = v_target + self.pid.pos_limit = gas_max + self.pid.neg_limit = - brake_max + deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV) + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) + if prevent_overshoot: + output_gb = min(output_gb, 0.0) + + # intention is to stop, switch to a different brake control until we stop + elif self.long_control_state == LongCtrlState.stopping: + # TODO: use the standstill bit from CAN to detect movements, usually more accurate than looking at v_ego + if not standstill or output_gb > -brake_stopping_target: + output_gb -= stopping_brake_rate / rate + output_gb = clip(output_gb, -brake_max, gas_max) + + self.v_pid = v_ego + self.pid.reset() + + # intention is to move again, release brake fast before handling control to PID + elif self.long_control_state == LongCtrlState.starting: + if output_gb < -0.2: + output_gb += starting_brake_rate / rate + self.v_pid = v_ego + self.pid.reset() + + self.last_output_gb = output_gb + final_gas = clip(output_gb, 0., gas_max) + final_brake = -clip(output_gb, -brake_max, 0.) + + return final_gas, final_brake \ No newline at end of file From ccf6497a284f2234a94bcfafaf071d8215188b80 Mon Sep 17 00:00:00 2001 From: BogGyver Date: Mon, 22 Oct 2018 16:35:27 -0400 Subject: [PATCH 20/20] bug fix on LoC creation --- selfdrive/car/tesla/PCC_module.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tesla/PCC_module.py b/selfdrive/car/tesla/PCC_module.py index f2856c95333bd1..232210e9b207fa 100644 --- a/selfdrive/car/tesla/PCC_module.py +++ b/selfdrive/car/tesla/PCC_module.py @@ -1,7 +1,7 @@ from selfdrive.car.tesla import teslacan #from selfdrive.controls.lib.pid import PIController from selfdrive.car.tesla.longcontrol_tesla import LongControl, STARTING_TARGET_SPEED -from selfdrive.car.tesla.interface import tesla_compute_gb +#from selfdrive.car.tesla.interface import tesla_compute_gb from selfdrive.car.tesla import teslacan from common.numpy_fast import clip, interp from selfdrive.services import service_list @@ -73,6 +73,9 @@ _DT = 0.01 # 100Hz _DT_MPC = 0.01 # 100Hz in our case +def tesla_compute_gb(accel, speed): + return float(accel) / 3.0 + def calc_cruise_accel_limits(v_ego, following): a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) @@ -105,6 +108,8 @@ class PCCState(object): 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)) @@ -178,7 +183,8 @@ def __init__(self,carcontroller): self.a_cruise = 0.0 #Long Control self.LoC = None - + + def reset(self, v_pid): if self.LoC: self.LoC.reset(v_pid)