diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index da51ea29979586..b64fb590fea49c 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -36,6 +36,7 @@ def get_can_signals(CP): ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), ("STEER_ANGLE", "STEERING_SENSORS", 0), ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), + ("STEER_ANGLE_OFFSET", "STEERING_SENSORS", 0), ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), @@ -239,7 +240,11 @@ def update(self, cp, cp_cam): self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + if self.CP.carFingerprint in HONDA_BOSCH: + self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET'] + else: + self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] #self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5a4b7ef4dda41a..9ef6826a1f32c2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -276,7 +276,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, CP, PL.lead_1) # 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, CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL) # Send a "steering required alert" if saturation count has reached the limit diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9d78c966743569..c3db502fcfe4bb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -58,8 +58,8 @@ def rate_limit(new_value, last_value, dw_step, up_step): def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight # while being in the middle of the lane - min_offset = -5. # deg - max_offset = 5. # deg + min_offset = -7. # deg + max_offset = 7. # deg alpha = 1. / 36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz min_learn_speed = 1. diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 81a371f783ca6c..e69c1eaf2d0838 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,5 +1,8 @@ +import zmq import math import numpy as np +import time +import json from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT from selfdrive.controls.lib.lateral_mpc import libmpc_py @@ -21,6 +24,14 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ 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): @@ -29,6 +40,16 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) + self.context = zmq.Context() + self.steerpub = self.context.socket(zmq.PUB) + self.steerpub.bind("tcp://*:8594") + self.steerdata = "" + self.smooth_factor = CP.steerActuatorDelay / _DT + self.feed_forward = 0.0 + self.angle_rate_desired = 0.0 + self.ff_angle_factor = 1.0 + self.ff_rate_factor = self.ff_angle_factor * 3.0 + self.accel_limit = 5.0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -48,11 +69,21 @@ def setup_mpc(self, steer_rate_cost): self.angle_steers_des_mpc = 0.0 self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 + self.context = zmq.Context() + self.steerpub = self.context.socket(zmq.PUB) + self.steerpub.bind("tcp://*:8594") + self.steerdata = "" + self.smooth_factor = 11.0 * _DT_MPC / _DT + self.feed_forward = 0.0 + self.angle_rate_desired = 0.0 + self.ff_angle_factor = 0.25 + self.ff_rate_factor = 2.5 + self.accel_limit = 0.3 def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL): + def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL): cur_time = sec_since_boot() self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run @@ -62,28 +93,31 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs curvature_factor = VM.curvature_factor(v_ego) - l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) - r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) - p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) + self.l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) + self.r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly)) + self.p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.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) 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.l_poly, self.r_poly, self.p_poly, PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) # reset to current steer angle if not active or overriding if active: + self.isActive = 1 delta_desired = self.mpc_solution[0].delta[1] else: + self.isActive = 0 delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) - self.angle_steers_des_time = cur_time + self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0) + self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC self.mpc_updated = True # Check for infeasable MPC solution @@ -97,24 +131,63 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") + elif self.steerdata != "" and len(self.steerdata) > 40000: + self.steerpub.send(self.steerdata) + self.steerdata = "" + if v_ego < 0.3 or not active: output_steer = 0.0 + self.feed_forward = 0.0 self.pid.reset() 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 = self.angle_steers_des_mpc + self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time) + restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit) + future_angle_steers_des = float(angle_steers) + self.smooth_factor * _DT * restricted_steer_rate + future_angle_steers = float(angle_steers) + self.smooth_factor * _DT * float(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) + if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(future_angle_steers_des) - float(angle_offset)) - 0.5 \ + and (abs(float(restricted_steer_rate)) > abs(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \ + and (float(restricted_steer_rate) < 0) == (float(future_angle_steers_des) - float(angle_offset) - 0.5 < 0): + ff_type = "r" + 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(future_angle_steers_des - float(angle_offset)) > 0.5: + ff_type = "a" + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(future_angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor + else: + ff_type = "r" + self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor + else: + self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle 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) + feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) + + self.pCost = 0.0 + self.lCost = 0.0 + self.rCost = 0.0 + self.hCost = 0.0 + self.srCost = 0.0 + self.last_ff_a = 0.0 + self.last_ff_r = 0.0 + self.center_angle = 0.0 + self.steer_zero_crossing = 0.0 + self.steer_rate_cost = 0.0 + self.avg_angle_rate = 0.0 + self.angle_rate_count = 0.0 + driver_torque = 0.0 + steer_motor = 0.0 + + self.steerdata += ("%d,%s,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.smooth_factor, self.accel_limit, float(restricted_steer_rate) ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, steer_motor, float(driver_torque), \ + self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ + self.angle_steers_des_mpc, CP.steerRatio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \ + PL.PP.r_prob, PL.PP.c_prob, PL.PP.p_prob, self.l_poly[0], self.l_poly[1], self.l_poly[2], self.l_poly[3], self.r_poly[0], self.r_poly[1], self.r_poly[2], self.r_poly[3], \ + self.p_poly[0], self.p_poly[1], self.p_poly[2], self.p_poly[3], PL.PP.c_poly[0], PL.PP.c_poly[1], PL.PP.c_poly[2], PL.PP.c_poly[3], PL.PP.d_poly[0], PL.PP.d_poly[1], \ + PL.PP.d_poly[2], PL.PP.lane_width, PL.PP.lane_width_estimate, PL.PP.lane_width_certainty, v_ego, self.pid.p, self.pid.i, self.pid.f, int(time.time() * 100) * 10000000)) self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 712c3031edb589..90e37676e79e2e 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,7 +1,7 @@ from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -CAMERA_OFFSET = 0.06 # m from center car to camera +CAMERA_OFFSET = 0.08 # m from center car to camera class PathPlanner(object): def __init__(self): diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 6cee1a2d95b9bc..d6cec5a400a09f 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -230,13 +230,13 @@ def update(self, CS, lead, v_cruise_setpoint): # Defining some variables to make the logic more human readable for auto distance override below # Is the car tailgating the lead car? - if x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.25: + if x_lead < 15 and self.v_rel >= -1 and self.v_rel < 0.25: self.tailgating = 1 else: self.tailgating = 0 # Is the car running surface street speeds? - if CS.vEgo < 18.06: + if CS.vEgo < 19.44: self.street_speed = 1 else: self.street_speed = 0 @@ -251,9 +251,9 @@ def update(self, CS, lead, v_cruise_setpoint): if CS.readdistancelines == 1: # If one bar distance, auto set to 2 bar distance under current conditions to prevent rear ending lead car if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating): - TR=1.8 + TR=2.2 if self.lastTR != 0: - self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.libmpc.init(MPC_COST_LONG.TTC, 0.075, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.lastTR = 0 else: TR=0.9 # 10m at 40km/hr @@ -522,7 +522,7 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel): 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 - plan_send.plan.events = events + plan_send.plan.events = eventssetting plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts