diff --git a/.gitignore b/.gitignore index ca50a23656ed23..c755190b9099f6 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,7 @@ .ipynb_checkpoints .idea .sconsign.dblite +.vscode model2.png a.out @@ -22,7 +23,10 @@ a.out *.vcd config.json clcache - +launch_chffrplus.sh +.vscode/*.json +selfdrive/visiond/visiond +.vscode/ board/obj/ selfdrive/boardd/boardd selfdrive/visiond/visiond @@ -30,6 +34,7 @@ selfdrive/logcatd/logcatd selfdrive/proclogd/proclogd selfdrive/ui/ui selfdrive/test/tests/plant/out +openpilot_tools/ /src/ one diff --git a/cereal/car.capnp b/cereal/car.capnp index 449e743cc803a3..156c42a2858e88 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -329,6 +329,7 @@ struct CarParams { centerToFront @9 :Float32; # [m] GC distance to front axle steerRatio @10 :Float32; # [] ratio between front wheels and steering wheel angles steerRatioRear @11 :Float32; # [] rear steering ratio wrt front steering (usually 0) + eonToFront @54 :Float32; # [m] distance from EON to front wheels # things we can derive rotationalInertia @12 :Float32; # [kg*m2] body rotational inertia @@ -343,7 +344,10 @@ struct CarParams { steerKpDEPRECATED @15 :Float32; steerKiDEPRECATED @16 :Float32; steerKf @25 :Float32; - + steerReactance @51 :Float32; + steerInductance @52 :Float32; + steerResistance @53 :Float32; + # Kp and Ki for the longitudinal control longitudinalKpBP @36 :List(Float32); longitudinalKpV @37 :List(Float32); diff --git a/cereal/log.capnp b/cereal/log.capnp index eb6ea7632d22d9..2b1e25026eecce 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -607,6 +607,8 @@ struct PathPlan { lProb @5 :Float32; rPoly @6 :List(Float32); rProb @7 :Float32; + mpcAngles @10 :List(Float64); + mpcTimes @11 :List(Float64); angleSteers @8 :Float32; valid @9 :Bool; diff --git a/common/realtime.py b/common/realtime.py index 7fe183fb23abca..6bc7b4a97d9d35 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -73,7 +73,10 @@ class Ratekeeper(object): def __init__(self, rate, print_delay_threshold=0.): """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" self._interval = 1. / rate - self._next_frame_time = sec_since_boot() + self._interval + cur_time = sec_since_boot() + sync_time = (7500 - (1000000 * cur_time) % 5000) / 1000000 + self._next_frame_time = cur_time + self._interval + sync_time + print(self._next_frame_time, cur_time) self._print_delay_threshold = print_delay_threshold self._frame = 0 self._remaining = 0 @@ -105,4 +108,3 @@ def monitor_time(self): self._frame += 1 self._remaining = remaining return lagged - diff --git a/panda/examples/can_logger.py b/panda/examples/can_logger.py index 05c28a26df1228..e5f1fc86597085 100755 --- a/panda/examples/can_logger.py +++ b/panda/examples/can_logger.py @@ -2,6 +2,7 @@ from __future__ import print_function import binascii import csv +import time import sys from panda import Panda @@ -24,18 +25,25 @@ def can_logger(): outputfile = open('output.csv', 'wb') csvwriter = csv.writer(outputfile) #Write Header - csvwriter.writerow(['Bus', 'MessageID', 'Message', 'MessageLength']) + csvwriter.writerow(['Time', 'MessageID', 'Bus', 'Message']) + #csvwriter.writerow(['Bus', 'MessageID', 'Message', 'MessageLength']) print("Writing csv file output.csv. Press Ctrl-C to exit...\n") bus0_msg_cnt = 0 bus1_msg_cnt = 0 bus2_msg_cnt = 0 - + startTime = 0.0 + cycle_count = 0 + p.can_clear(0) + p.can_clear(1) + p.can_clear(2) while True: - can_recv = p.can_recv() - + can_recv = p.can_recv() + cycle_count += 1 for address, _, dat, src in can_recv: - csvwriter.writerow([str(src), str(hex(address)), "0x" + binascii.hexlify(dat), len(dat)]) + if startTime == 0.0: + startTime = time.time() + csvwriter.writerow([str(time.time()-startTime), str(address), str(src), binascii.hexlify(dat)]) if src == 0: bus0_msg_cnt += 1 @@ -44,7 +52,7 @@ def can_logger(): elif src == 2: bus2_msg_cnt += 1 - print("Message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt), end='\r') + if (bus0_msg_cnt + bus1_msg_cnt + bus2_msg_cnt) % 1000 == 0: print("Message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt), end='\r') except KeyboardInterrupt: print("\nNow exiting. Final message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt)) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 74b1f4c5de9616..39ec6936a21db6 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -446,12 +446,16 @@ void *can_recv_thread(void *crap) { void *context = zmq_ctx_new(); void *publisher = zmq_socket(context, ZMQ_PUB); zmq_bind(publisher, "tcp://*:8006"); + int sleepTime; // run at ~200hz while (!do_exit) { can_recv(publisher); // 5ms - usleep(5*1000); + sleepTime = 5000 - ((nanos_since_boot() / 1000) % 5000); + usleep(sleepTime); + //usleep(5000); + //LOGW(" sleeptime = %d ", sleepTime); } return NULL; } diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 0e29ea62620814..5b5f159cbd1588 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -77,6 +77,10 @@ def get_params(candidate, fingerprint): ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 f = 1.2 tireStiffnessFront_civic *= f diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f07ddcd6436a96..7aa5bc6a2de8ac 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -64,6 +64,11 @@ def get_params(candidate, fingerprint): ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) ret.openpilotLongitudinalControl = ret.enableCamera + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + std_cargo = 136 if candidate == CAR.VOLT: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index c2a112b3ef10b9..ac2d5ada713a85 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -186,6 +186,10 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKf = 0.00006 # conservative feed-forward + ret.steerReactance = 1.0 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: stop_and_go = True @@ -196,7 +200,11 @@ def get_params(candidate, fingerprint): tire_stiffness_factor = 1. # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.6], [0.18]] + ret.steerReactance = 0.3 + ret.steerInductance = 0.9 + ret.steerResistance = 1.0 + ret.eonToFront = 0.0 if is_fw_modified: tire_stiffness_factor = 0.9 ret.steerKf = 0.00004 @@ -227,6 +235,10 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 15.96 # 11.82 is spec end-to-end tire_stiffness_factor = 0.8467 + ret.steerReactance = 1.75 + ret.steerInductance = 2.25 + ret.steerResistance = 0.5 + ret.eonToFront = 1.0 ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -240,7 +252,11 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end tire_stiffness_factor = 0.72 - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.steerReactance = 0.3 + ret.steerInductance = 0.9 + ret.steerResistance = 1.0 + ret.eonToFront = 0.0 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -253,7 +269,11 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.steerReactance = 0.6 + ret.steerInductance = 1.0 + ret.steerResistance = 0.5 + ret.eonToFront = 0.0 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -280,7 +300,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec tire_stiffness_factor = 0.444 # not optimized yet - ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 3b423ae04ffe8b..6035d6485f5d57 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -185,7 +185,9 @@ def update(self, cp, cp_cam): self.standstill = not self.v_wheel > 0.1 self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] - self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] + + # The defined value in the DBC is always positive, so the calculated value needs to be used + self.angle_steers_rate = 0.0 #cp.vl["SAS11"]['SAS_Speed'] self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] self.main_on = True self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index a854eebbc38794..b41f1390cd52b4 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -69,6 +69,11 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + ret.steerActuatorDelay = 0.1 # Default delay tire_stiffness_factor = 1. diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 3a5e05257c4a90..851007e5af2977 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -74,6 +74,10 @@ def get_params(candidate, fingerprint): ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.] ret.steerActuatorDelay = 0. + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 return ret diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index bb3b3f05217795..ac1d0151f0f311 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -142,7 +142,13 @@ def update(self, cp, cp_cam): self.standstill = not self.v_wheel > 0.001 self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] + + # Only use the reported steer rate from some Toyotas, since others are very noisy + if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.COROLLA): + self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] + else: + self.angle_steers_rate = 0.0 + can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values) self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b6cb0a0129844..e81bf1b1cb3625 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -76,6 +76,11 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + if candidate == CAR.PRIUS: stop_and_go = True ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file @@ -87,6 +92,7 @@ def get_params(candidate, fingerprint): ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis ret.steerActuatorDelay = 0.25 + ret.eonToFront = 0.0 elif candidate in [CAR.RAV4, CAR.RAV4H]: stop_and_go = True if (candidate in CAR.RAV4H) else False diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a91614b6e042a5..9e8ab23894e17f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -257,7 +257,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise 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) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, angle_offset, CS.steeringPressed, CP, VM, path_plan) # Send a "steering required alert" if saturation count has reached the limit diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 78ffa3db8f9988..9de62c56f38a6a 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,6 +1,9 @@ from selfdrive.controls.lib.pid import PIController from common.numpy_fast import interp +from common.realtime import sec_since_boot from cereal import car +import math +import numpy as np _DT = 0.01 # 100Hz _DT_MPC = 0.05 # 20Hz @@ -10,36 +13,128 @@ def get_steer_max(CP, v_ego): return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) +def apply_deadzone(angle, deadzone): + if angle > deadzone: + angle -= deadzone + elif angle < -deadzone: + angle += deadzone + else: + angle = 0. + return angle + + class LatControl(object): def __init__(self, CP): - self.pid = PIController((CP.steerKpBP, CP.steerKpV), - (CP.steerKiBP, CP.steerKiV), - k_f=CP.steerKf, pos_limit=1.0) + + if CP.steerResistance > 0 and CP.steerReactance >= 0 and CP.steerInductance > 0: + self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) + self.projection_factor = CP.steerReactance * CP.steerActuatorDelay / 2.0 # Mutiplier for reactive component (PI) + self.accel_limit = 2.0 / CP.steerResistance # Desired acceleration limit to prevent "whip steer" (resistive component) + self.ff_angle_factor = 1.0 # Kf multiplier for angle-based feed forward + self.ff_rate_factor = 10.0 # Kf multiplier for rate-based feed forward + # Eliminate break-points, since they aren't needed (and would cause problems for resonance) + KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV)] + KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _DT / self.projection_factor] + self.pid = PIController(([0.], KpV), + ([0.], KiV), + k_f=CP.steerKf, pos_limit=1.0) + else: + self.pid = PIController((CP.steerKpBP, CP.steerKpV), + (CP.steerKiBP, CP.steerKiV), + k_f=CP.steerKf, pos_limit=1.0) + self.smooth_factor = 1.0 + self.projection_factor = 0.0 + self.accel_limit = 0.0 + self.ff_angle_factor = 1.0 + self.ff_rate_factor = 0.0 self.last_cloudlog_t = 0.0 - self.angle_steers_des = 0. + self.prev_angle_rate = 0 + self.feed_forward = 0.0 + self.last_mpc_ts = 0.0 + self.angle_steers_des_time = 0.0 + self.angle_steers_des_mpc = 0.0 + self.steer_counter = 1.0 + self.steer_counter_prev = 0.0 + self.rough_steers_rate = 0.0 + self.prev_angle_steers = 0.0 + self.calculate_rate = True def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_rate, angle_offset, steer_override, CP, VM, path_plan): + + if angle_rate == 0.0 and self.calculate_rate: + if angle_steers != self.prev_angle_steers: + self.steer_counter_prev = self.steer_counter + self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0 + self.steer_counter = 0.0 + elif self.steer_counter >= self.steer_counter_prev: + self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0) + self.steer_counter += 1.0 + angle_rate = self.rough_steers_rate + + # Don't use accelerated rate unless it's from CAN + accelerated_angle_rate = angle_rate + else: + # If non-zero angle_rate is provided, use it instead + self.calculate_rate = False + # Use steering rate from the last 2 samples to estimate acceleration for a likely future steering rate + accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate + + + cur_time = sec_since_boot() + self.angle_steers_des_time = cur_time + if v_ego < 0.3 or not active: output_steer = 0.0 + self.feed_forward = 0.0 self.pid.reset() + self.angle_steers_des = angle_steers else: - # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution - # constant for 0.05s. - #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps - #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) - self.angle_steers_des = path_plan.angleSteers + # Interpolate desired angle between MPC updates + self.angle_steers_des = np.interp(cur_time, path_plan.mpcTimes, path_plan.mpcAngles) + self.angle_steers_des_time = cur_time + + # Determine the target steer rate for desired angle, but prevent the acceleration limit from being exceeded + # Restricting the steer rate creates the resistive component needed for resonance + restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(accelerated_angle_rate) - self.accel_limit, + float(accelerated_angle_rate) + self.accel_limit) + + # Determine projected desired angle that is within the acceleration limit (prevent the steering wheel from jerking) + projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate + + # Determine future angle steers using accellerated steer rate + projected_angle_steers = float(angle_steers) + self.projection_factor * float(accelerated_angle_rate) + steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - steer_feedforward = self.angle_steers_des # feedforward desired angle - if CP.steerControlType == car.CarParams.SteerControlType.torque: - steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, - feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + + if CP.steerControlType == car.CarParams.SteerControlType.torque: + # Decide which feed forward mode should be used (angle or rate). Use more dominant mode, but only if conditions are met + # Spread feed forward out over a period of time to make it inductive (for resonance) + if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \ + and (abs(float(restricted_steer_rate)) > abs(accelerated_angle_rate) or (float(restricted_steer_rate) < 0) != (accelerated_angle_rate < 0)) \ + and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0): + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / self.smooth_factor + elif abs(self.angle_steers_des - float(angle_offset)) > 0.5: + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 \ + * float(apply_deadzone(float(self.angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor + else: + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor + + # Use projected desired and actual angles instead of "current" values, in order to make PI more reactive (for resonance) + output_steer = self.pid.update(projected_angle_steers_des, projected_angle_steers, check_saturation=(v_ego > 10), + override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) self.sat_flag = self.pid.saturated - return output_steer, float(self.angle_steers_des) + self.prev_angle_rate = angle_rate + self.prev_angle_steers = angle_steers + + # return MPC angle in the unused output (for ALCA) + if CP.steerControlType == car.CarParams.SteerControlType.torque: + return output_steer, self.angle_steers_des + else: + return self.angle_steers_des_mpc, float(self.angle_steers_des) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 662608eeb73af7..ea4693725df213 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -10,10 +10,10 @@ from selfdrive.controls.lib.model_parser import ModelParser import selfdrive.messaging as messaging +_DT_MPC = 0.05 - -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay +def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay, long_camera_offset): + states[0].x = max(0.0, v_ego * delay + long_camera_offset) states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay return states @@ -30,6 +30,18 @@ def __init__(self, CP): self.setup_mpc(CP.steerRateCost) self.invalid_counter = 0 + self.prev_angle_rate = 0 + self.feed_forward = 0.0 + self.last_mpc_ts = 0.0 + self.angle_steers_des_time = 0.0 + self.angle_steers_des_mpc = 0.0 + self.steer_counter = 1.0 + self.steer_counter_prev = 0.0 + self.rough_steers_rate = 0.0 + self.prev_angle_steers = 0.0 + self.calculate_rate = True + self.accelerated_angle_rate = 0.0 + def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -41,6 +53,8 @@ def setup_mpc(self, steer_rate_cost): self.cur_state[0].y = 0.0 self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 + self.mpc_angles = [0.0, 0.0, 0.0] + self.mpc_times = [0.0, 0.0, 0.0] self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 @@ -50,45 +64,58 @@ def setup_mpc(self, steer_rate_cost): def update(self, CP, VM, CS, md, live100): v_ego = CS.carState.vEgo angle_steers = CS.carState.steeringAngle + angle_rate = CS.carState.steeringRate active = live100.live100.active angle_offset = live100.live100.angleOffset self.MP.update(v_ego, md) # Run MPC - self.angle_steers_des_prev = self.angle_steers_des_mpc + cur_time = sec_since_boot() + self.angle_steers_des_prev = np.interp(cur_time, self.mpc_times, self.mpc_angles) curvature_factor = VM.curvature_factor(v_ego) l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly)) r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly)) p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) + # Determine future angle steers using steer rate + projected_angle_steers = float(angle_steers) + CP.steerActuatorDelay * float(angle_rate) - 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, - l_poly, r_poly, p_poly, - self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) + # account for actuation delay + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, projected_angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay + _DT_MPC, CP.eonToFront) # reset to current steer angle if not active or overriding if active: - delta_desired = self.mpc_solution[0].delta[1] + self.cur_state[0].delta = math.radians(self.angle_steers_des_prev - angle_offset) / CP.steerRatio else: - delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio + self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / CP.steerRatio - self.cur_state[0].delta = delta_desired + 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, + l_poly, r_poly, p_poly, + self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) - self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) + cur_time = sec_since_boot() + self.angle_steers_des_prev = np.interp(cur_time, self.mpc_times, self.mpc_angles) # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) - t = sec_since_boot() - if mpc_nans: + if not mpc_nans: + self.mpc_angles = [self.angle_steers_des_prev, + float(math.degrees(self.mpc_solution[0].delta[1] * CP.steerRatio) + angle_offset), + float(math.degrees(self.mpc_solution[0].delta[2] * CP.steerRatio) + angle_offset)] + + self.mpc_times = [cur_time, + cur_time + _DT_MPC, + cur_time + _DT_MPC + _DT_MPC] + + self.angle_steers_des_mpc = self.mpc_angles[1] + else: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio - if t > self.last_cloudlog_t + 5.0: - self.last_cloudlog_t = t + if cur_time > self.last_cloudlog_t + 5.0: + self.last_cloudlog_t = cur_time cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge @@ -109,6 +136,8 @@ def update(self, CP, VM, CS, md, live100): plan_send.pathPlan.rPoly = map(float, r_poly) plan_send.pathPlan.rProb = float(self.MP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) + plan_send.pathPlan.mpcAngles = map(float, self.mpc_angles) + plan_send.pathPlan.mpcTimes = map(float, self.mpc_times) plan_send.pathPlan.valid = bool(plan_valid) self.plan.send(plan_send.to_bytes()) diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond new file mode 100755 index 00000000000000..7f517b570ab4b7 Binary files /dev/null and b/selfdrive/visiond/visiond differ