diff --git a/cereal/log.capnp b/cereal/log.capnp index b059218feba89c..dc2080bc5874df 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1814,6 +1814,10 @@ struct KalmanOdometry { rotStd @3 :List(Float32); # std rad/s in device frame } +struct SmiskolData { + mpcTR @0 :Float32; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -1889,5 +1893,6 @@ struct Event { carEvents @68: List(Car.CarEvent); carParams @69: Car.CarParams; frontFrame @70: FrameData; + smiskolData @71 :SmiskolData; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index 884bfe9a5bf991..b09eb2b9fa7754 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -75,6 +75,7 @@ thumbnail: [8069, true, 0.2, 1] carEvents: [8070, true, 1., 1] carParams: [8071, true, 0.02, 1] frontFrame: [8072, true, 10.] +smiskolData: [8073, true, 20.] testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] diff --git a/common/op_params.py b/common/op_params.py index 4f2dab81db155b..e56107cb7ce01f 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -37,7 +37,8 @@ def __init__(self): 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'}, 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' 'If False, lane change will occur IMMEDIATELY after signaling')}, - 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} + 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}, + 'use_static_steering_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the automatically learned steering ratio. If True, it will use the value in interface.py instead'}} self.params = {} self.params_file = "/data/op_params.json" diff --git a/run_docker_tests.sh b/run_docker_tests.sh index 512c5161f32469..b575934283d566 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -7,8 +7,8 @@ RUN="docker run --shm-size 1G --rm tmppilot /bin/sh -c" docker build -t tmppilot -f Dockerfile.openpilot . $RUN "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py" -$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' -$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))' +#$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")' +#$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))' $RUN "$SETUP python -m unittest discover common" $RUN "$SETUP python -m unittest discover opendbc/can" $RUN "$SETUP python -m unittest discover selfdrive/boardd" diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 94b73917f55d93..73f3857d133c31 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -121,14 +121,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 - ret.steerRatio = 18.27 + ret.steerRatio = 17.8 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] ret.lateralTuning.pid.kf = 0.00003 * 0.639 # full torque for 20 deg at 80mph means 0.00007818594 - if ret.enableGasInterceptor: - ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] - # ret.longitudinalTuning.kiV = [0.135, 0.09] +# if ret.enableGasInterceptor: +# ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] +# ret.longitudinalTuning.kiV = [0.135, 0.09] elif candidate == CAR.LEXUS_RXH: stop_and_go = True diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6379dd768908f6..b28578ac78f396 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -239,7 +239,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -286,7 +286,13 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} + passable_loc = {} + if not travis: + passable_loc['lead_one'] = sm['radarState'].leadOne + passable_loc['mpc_TR'] = sm['smiskolData'].mpcTR + passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']} + passable_loc['has_lead'] = plan.hasLead + passable_loc['gas_pressed'] = CS.gasPressed actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc) # Steering PID loop and lateral MPC @@ -479,7 +485,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation']) + 'model', 'gpsLocation', 'radarState', 'liveTracks', 'smiskolData'], ignore_alive=['gpsLocation']) if can_sock is None: @@ -597,13 +603,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - if not travis: - passable_state_control = {'lead_one': sm['radarState'].leadOne} - else: - passable_state_control = {'lead_one': None} actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 7b32a1a7e7eb28..71a68a3fe6898f 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -31,12 +31,16 @@ def __init__(self, mpc_id): self.new_lead = False self.last_cloudlog_t = 0.0 + self.pm = None self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 self.customTR = self.op_params.get('following_distance', None) + def set_pm(self, pm): + self.pm = pm + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -79,6 +83,11 @@ def get_TR(self, CS): if not travis: self.change_cost(TR) + if self.mpc_id == 1: + dat = messaging.new_message() + dat.init('smiskolData') + dat.smiskolData.mpcTR = TR + self.pm.send('smiskolData', dat) return TR def change_cost(self, TR): @@ -115,10 +124,12 @@ def lead_accel_over_time(self): return a_lead # if above doesn't execute, we'll return a_lead from radar def dynamic_follow(self, CS): - # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities - # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469] y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881] + # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs + # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + # y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs sng_TR = 1.8 # stop and go parameters sng_speed = 15.0 * CV.MPH_TO_MS @@ -131,23 +142,20 @@ def dynamic_follow(self, CS): TR = interp(self.car_data['v_ego'], x, y) # Dynamic follow modifications (the secret sauce) - x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909] # relative velocity values - y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28] # modification values + x = [-20.0, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0.0, 1.3411, 1.89, 2.6909] # relative velocity values + y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.06, -0.144, -0.224] # modification values TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y) - x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411] # lead acceleration values - y = [0.237, 0.12, 0.027, 0, -0.105, -0.195] # modification values - TR_mod += interp(self.lead_accel_over_time(), x, y) # todo: test if these modifications are too much + x = [-4.4704, -1.77, -0.3145, 0.0, 0.1495, 0.5104, 0.7037, 0.9357] # lead acceleration values + y = [0.237, 0.12, 0.027, 0, -0.006, -0.036, -0.042, -0.045] # modification values + TR_mod += interp(self.lead_accel_over_time(), x, y) TR += TR_mod if CS.leftBlinker or CS.rightBlinker: - old_TR = float(TR) x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .65, .55] # reduce TR when changing lanes TR *= interp(self.car_data['v_ego'], x, y) - with open('/data/blinker_debug', 'a') as f: - f.write('{}\n'.format([CS.leftBlinker, CS.rightBlinker, self.car_data['v_ego'], old_TR, TR])) # TR *= self.get_traffic_level() # modify TR based on last minute of traffic data # todo: look at getting this to work, a model could be used @@ -181,6 +189,8 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Setup current mpc state self.cur_state[0].x_ego = 0.0 + with open("/data/set_speed", "a") as f: + f.write("{}".format([v_ego, CS.cruiseState.speed])) if lead is not None and lead.status: x_lead = lead.dRel diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b5ce9e1d2d96e2..5b03b5aae1cce1 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -2,7 +2,10 @@ from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController from common.travis_checker import travis -from selfdrive.car.toyota.values import CAR +from selfdrive.car.toyota.values import CAR as CAR_TOYOTA +from selfdrive.config import Conversions as CV +from common.op_params import opParams +import numpy as np LongCtrlState = log.ControlsState.LongControlState @@ -67,11 +70,18 @@ def __init__(self, CP, compute_gb, candidate): convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 + + self.op_params = opParams() + self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True) + self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS + self.candidate = candidate + self.toyota_candidates = [attr for attr in dir(CAR_TOYOTA) if not attr.startswith("__")] + + self.gas_pressed = False self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False} + self.track_data = [] + self.mpc_TR = 1.8 self.v_ego = 0.0 - self.gas_pressed = False - self.candidate = candidate - self.toyota_candidates = [attr for attr in dir(CAR) if not attr.startswith("__")] def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -80,12 +90,9 @@ def reset(self, v_pid): def dynamic_gas(self, CP): x, y = [], [] - if CP.enableGasInterceptor: # if pedal, todo: make different profiles for different vehicles - if self.candidate in [CAR.COROLLA]: - with open('/data/long_corolla', 'a') as f: - f.write('we have liftoff\n') - x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] - y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) + if CP.enableGasInterceptor and (self.candidate in self.toyota_candidates): # todo: make different profiles for different makes + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] # vels + y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # max gas elif self.candidate in self.toyota_candidates: x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] @@ -95,52 +102,99 @@ def dynamic_gas(self, CP): gas = interp(self.v_ego, x, y) - with open('/data/lead_data', 'a') as f: - f.write(str(self.lead_data) + '\n') - if self.lead_data['status']: # if lead if self.v_ego <= 8.9408: # if under 20 mph - # TR = 1.8 # desired TR, might need to switch this to hardcoded distance values - # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR - x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481] # relative velocity mod y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0] gas_mod = -interp(self.lead_data['v_rel'], x, y) - # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684] # lead accel mod # todo: this - # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)] - # gas_mod += interp(a_lead, x, y) + x = [0.44704, 1.78816] # lead accel mod + y = [0.0, gas_mod * .6] + gas_mod -= interp(self.lead_data['a_lead'], x, y) # as lead accelerates, we can reduce the reduction of the above mod (the max this will ouput is the original gas value, it never increases it) # x = [TR * 0.5, TR, TR * 1.5] # as lead gets further from car, lessen gas mod # todo: this # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5] # gas_mod += (interp(current_TR, x, y)) - new_gas = gas + gas_mod # (interp(current_TR, x, y)) + new_gas = gas + gas_mod x = [1.78816, 6.0, 8.9408] # slowly ramp mods down as we approach 20 mph y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas] gas = interp(self.v_ego, x, y) else: - x = [-0.89408, 0, 2.0] # need to tune this - y = [-.17, -.08, .01] - gas += interp(self.lead_data['v_rel'], x, y) + current_TR = self.lead_data['x_lead'] / self.v_ego + x = [-1.78816, -0.89408, 0, 1.78816, 2.68224] # relative velocity mod + y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.175, gas * 0.225] + gas_mod = interp(self.lead_data['v_rel'], x, y) + + x = [self.mpc_TR - 0.22, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4] + y = [-gas_mod * 0.36, 0.0, gas_mod * 0.15, gas_mod * 0.45] + gas_mod -= interp(current_TR, x, y) + + gas += gas_mod return clip(gas, 0.0, 1.0) + def handle_live_tracks(self, live_tracks): + if live_tracks['updated']: + self.track_data = [] + for track in live_tracks['tracks']: + self.track_data.append({'v_lead': self.v_ego + track.vRel, 'y_rel': track.yRel}) + def handle_passable(self, passable): self.gas_pressed = passable['gas_pressed'] self.lead_data['v_rel'] = passable['lead_one'].vRel self.lead_data['a_lead'] = passable['lead_one'].aLeadK self.lead_data['x_lead'] = passable['lead_one'].dRel self.lead_data['status'] = passable['has_lead'] # this fixes radarstate always reporting a lead, thanks to arne + self.mpc_TR = passable['mpc_TR'] + self.handle_live_tracks(passable['live_tracks']) + + def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target): + v_cruise *= CV.KPH_TO_MS # convert to m/s + min_tracks = 3 + vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]] + margins = [0.4, 0.55, 0.6] + track_speed_margin = interp(self.v_ego, vels, margins) + MPC_TIME_STEP = 1 / 20. + track_tolerance_v = 0.022352 + track_tolerance_y = 1.8288 + if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed: + tracks = [] + for track in self.track_data: + valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= track_tolerance_v else False for trk in tracks]) # radar sometimes reports multiple points for one vehicle, especially semis + # valid_y = all([True if abs(trk['y_rel'] - track['y_rel']) >= track_tolerance_y else False for trk in tracks]) + if valid: # or valid_y: + tracks.append(track) + tracks = [trk['v_lead'] for trk in tracks if (self.v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise] # .125, 0.025, 0.02500009536743164, 0.02500009536743164 + if len(tracks) >= min_tracks: + average_track_speed = np.mean(tracks) + if average_track_speed < v_target and average_track_speed < v_target_future: + # so basically, if there's at least 3 tracks, the speeds of the tracks must be within n% of set speed, if our speed is at least set_speed mph, + # if the average speeds of tracks is less than v_target and v_target_future, then get a weight for how many tracks exist, with more tracks, the more we + # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values + # this should work...? + x = [3, 6, 19] + y = [0.275, .375, 0.5] + track_speed_weight = interp(len(tracks), x, y) + if self.lead_data['status']: # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...) + track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y)) + v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight) + if v_target_slow < v_target and v_target_slow < v_target_future: # just a sanity check, don't want to run into any leads if we somehow predict faster velocity + a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0) # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that? + a_target = a_target_slow + v_target = v_target_slow + v_target_future = v_target_slow + + return v_target, v_target_future, a_target def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.v_ego = v_ego - # Actuation limits if not travis: self.handle_passable(passable) gas_max = self.dynamic_gas(CP) + v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target) else: gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 74f68454fcc4a0..aa50c990d85f0c 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -10,6 +10,7 @@ from cereal import log from common.op_params import opParams from selfdrive.controls.lane_hugging import LaneHugging +from common.travis_checker import travis LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection @@ -49,6 +50,7 @@ def __init__(self, CP): self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost + self.steer_ratio = CP.steerRatio self.setup_mpc() self.solution_invalid_cnt = 0 @@ -59,6 +61,7 @@ def __init__(self, CP): self.op_params = opParams() self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True) self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0) + self.use_static_steering_ratio = self.op_params.get('use_static_steering_ratio', default=False) self.lane_hugging = LaneHugging() def setup_mpc(self): @@ -77,6 +80,11 @@ def setup_mpc(self): self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 + def getSteerRatio(self, sR): + if not travis and not self.use_static_steering_ratio: + return self.steer_ratio + return sR + def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle @@ -166,7 +174,8 @@ def update(self, sm, pm, CP, VM): # account for actuation delay angle_offset = self.lane_hugging.modify_offset(float(sm['liveParameters'].angleOffset), lane_change_direction, self.lane_change_state) - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) + sR = self.getSteerRatio(VM.sR) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e2e07409a03b3c..94c34238034c17 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -24,7 +24,7 @@ # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] -_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits @@ -65,8 +65,11 @@ class Planner(): def __init__(self, CP): self.CP = CP + pm = messaging.PubMaster(['smiskolData']) self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) + if not travis: + self.mpc1.set_pm(pm) self.v_acc_start = 0.0 self.a_acc_start = 0.0