From 01712350f1e3e82fbc92c0974d7b87b89823f9a7 Mon Sep 17 00:00:00 2001 From: kegman Date: Sun, 16 Dec 2018 10:43:34 -0500 Subject: [PATCH] Revert "Merge with kegman-gernbyFFsteer-latest (#69)" This reverts commit 99031cf0eb1f27d5e5d78a35d60a3857bba587ac. --- selfdrive/can/parser.cc | 44 +----- selfdrive/car/honda/carstate.py | 7 +- selfdrive/car/honda/interface.py | 8 +- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol.py | 188 ++++---------------------- selfdrive/controls/lib/pathplanner.py | 25 +--- selfdrive/controls/lib/planner.py | 2 +- 7 files changed, 43 insertions(+), 233 deletions(-) diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 83e2e02bcf4ddf..4a82d9a022f36f 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -166,9 +166,6 @@ class CANParser { subscriber = zmq_socket(context, ZMQ_SUB); zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0); - forwarder = zmq_socket(context, ZMQ_PUB); - zmq_bind(forwarder, "tcp://*:8592"); - std::string tcp_addr_str; if (sendcan) { @@ -247,17 +244,8 @@ class CANParser { // parse the messages for (int i = 0; i < msg_count; i++) { auto cmsg = cans[i]; - - if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen - uint8_t dat[8] = {0}; - memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); - - if (can_forward_period_ns > 0) raw_can_values[cmsg.getSrc()][cmsg.getAddress()] = read_u64_be(dat); - - //if (((cmsg.getAddress() == 0xe4 or cmsg.getAddress() == 0x33d) and cmsg.getSrc() == bus) or \ - // (cmsg.getSrc() != bus and cmsg.getAddress() != 0x33d and cmsg.getAddress() != 0xe4 and \ - // (cmsg.getAddress() < 0x240 or cmsg.getAddress() > 0x24A))) { if (cmsg.getSrc() != bus) { + // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); continue; } auto state_it = message_states.find(cmsg.getAddress()); @@ -266,6 +254,10 @@ class CANParser { continue; } + if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen + uint8_t dat[8] = {0}; + memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); + // Assumes all signals in the message are of the same type (little or big endian) // TODO: allow signals within the same message to have different endianess auto& sig = message_states[cmsg.getAddress()].parse_sigs[0]; @@ -273,7 +265,7 @@ class CANParser { p = read_u64_le(dat); } else { p = read_u64_be(dat); - } + } DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p); @@ -281,23 +273,8 @@ class CANParser { } } - void ForwardCANData(uint64_t sec) { - if (sec > next_can_forward_ns) { - next_can_forward_ns += can_forward_period_ns; - // next_can_forward_ns starts at 0, so it needs to be reset. Also handle delays. - if (sec > next_can_forward_ns) next_can_forward_ns = sec + can_forward_period_ns; - std::string canOut = ""; - for (auto src : raw_can_values) { - for (auto pid : src.second) { - canOut = canOut + std::to_string(src.first) + " " + std::to_string(pid.first) + " " + std::to_string(pid.second) + "|"; - } - } - zmq_send(forwarder, canOut.data(), canOut.size(), 0); - } - } - void UpdateValid(uint64_t sec) { - can_valid = true; + can_valid = true; for (const auto& kv : message_states) { const auto& state = kv.second; if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { @@ -340,8 +317,6 @@ class CANParser { UpdateCans(sec, cans); } - if (can_forward_period_ns > 0) ForwardCANData(sec); - UpdateValid(sec); zmq_msg_close(&msg); @@ -376,11 +351,6 @@ class CANParser { void *context = NULL; void *subscriber = NULL; - void *forwarder = NULL; - uint64_t can_forward_period_ns = 100000000; - uint64_t next_can_forward_ns = 0; - std::unordered_map> raw_can_values; - const DBC *dbc = NULL; std::unordered_map message_states; }; diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 927e3363ff044e..db42e0af4ce28f 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -36,7 +36,6 @@ 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), @@ -243,11 +242,7 @@ 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'] - 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 = 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/car/honda/interface.py b/selfdrive/car/honda/interface.py index 148af9687f445f..54f0876ccde12d 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -196,7 +196,7 @@ 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.33], [0.10]] if is_fw_modified else [[0.6], [0.18]] + ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.8], [0.24]] if is_fw_modified: ret.steerKf = 0.00003 ret.longitudinalKpBP = [0., 5., 35.] @@ -211,7 +211,7 @@ def get_params(candidate, fingerprint): ret.centerToFront = centerToFront_civic ret.steerRatio = 14.63 # 10.93 is spec end-to-end tire_stiffness_factor = 1. - ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -256,7 +256,7 @@ 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.6], [0.18]] + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] @@ -283,7 +283,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.6], [0.18]] + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index cdc870feb2ebbe..85e1218153c856 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -274,7 +274,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, CS.steeringRate, + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, 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/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 43e69ffebbbdba..81a371f783ca6c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,8 +1,5 @@ -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 @@ -24,14 +21,6 @@ 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): @@ -40,47 +29,6 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) - self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) - self.projection_factor = 5.0 * _DT # Mutiplier for reactive component (PI) - self.accel_limit = 2.0 # Desired acceleration limit to prevent "whip steer" (resistive component) - self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward - self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward - self.ratioDelayExp = 2.0 # Exponential coefficient for variable steering rate (delay) - self.ratioDelayScale = 0.0 # Multiplier for variable steering rate (delay) - self.prev_angle_rate = 0 - self.feed_forward = 0.0 - self.angle_rate_desired = 0.0 - self.last_mpc_ts = 0.0 - self.angle_steers_des = 0.0 - self.angle_steers_des_mpc = 0.0 - self.angle_steers_des_prev = 0.0 - self.angle_steers_des_time = 0.0 - self.avg_angle_steers = 0.0 - self.last_y = 0.0 - self.new_y = 0.0 - self.angle_sample_count = 0.0 - self.projected_angle_steers = 0.0 - self.lateral_error = 0.0 - - # variables for dashboarding - self.context = zmq.Context() - self.steerpub = self.context.socket(zmq.PUB) - self.steerpub.bind("tcp://*:8594") - self.influxString = 'steerData3,testName=none,active=%s,ff_type=%s ff_type_a=%s,ff_type_r=%s,steer_status=%s,steer_torque_motor=%s,' \ - 'steering_control_active=%s,steer_parameter1=%s,steer_parameter2=%s,steer_parameter3=%s,steer_parameter4=%s,steer_parameter5=%s,' \ - 'steer_parameter6=%s,steer_stock_torque=%s,steer_stock_torque_request=%s,x=%s,y=%s,yb=%s,y0=%s,y1=%s,y2=%s,y3=%s,y4=%s,psi=%s,delta=%s,t=%s,' \ - 'curvature_factor=%s,slip_factor=%s,resonant_period=%s,accel_limit=%s,restricted_steer_rate=%s,ff_angle_factor=%s,ff_rate_factor=%s,' \ - 'pCost=%s,lCost=%s,rCost=%s,hCost=%s,srCost=%s,torque_motor=%s,driver_torque=%s,angle_rate_count=%s,angle_rate_desired=%s,' \ - 'avg_angle_rate=%s,future_angle_steers=%s,angle_rate=%s,steer_zero_crossing=%s,center_angle=%s,angle_steers=%s,angle_steers_des=%s,' \ - 'angle_offset=%s,self.angle_steers_des_mpc=%s,steerRatio=%s,steerKf=%s,steerKpV[0]=%s,steerKiV[0]=%s,steerRateCost=%s,l_prob=%s,' \ - 'r_prob=%s,c_prob=%s,p_prob=%s,l_poly[0]=%s,l_poly[1]=%s,l_poly[2]=%s,l_poly[3]=%s,r_poly[0]=%s,r_poly[1]=%s,r_poly[2]=%s,r_poly[3]=%s,' \ - 'p_poly[0]=%s,p_poly[1]=%s,p_poly[2]=%s,p_poly[3]=%s,c_poly[0]=%s,c_poly[1]=%s,c_poly[2]=%s,c_poly[3]=%s,d_poly[0]=%s,d_poly[1]=%s,' \ - 'd_poly[2]=%s,lane_width=%s,lane_width_estimate=%s,lane_width_certainty=%s,v_ego=%s,p=%s,i=%s,f=%s %s\n~' - self.steerdata = self.influxString - self.frames = 0 - self.curvature_factor = 0.0 - self.slip_factor = 0.0 - self.isActive = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -95,10 +43,16 @@ def setup_mpc(self, steer_rate_cost): self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 + self.last_mpc_ts = 0.0 + self.angle_steers_des = 0.0 + self.angle_steers_des_mpc = 0.0 + self.angle_steers_des_prev = 0.0 + self.angle_steers_des_time = 0.0 + def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL): + def update(self, active, v_ego, angle_steers, 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 @@ -106,49 +60,30 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.last_mpc_ts = PL.last_md_ts self.angle_steers_des_prev = self.angle_steers_des_mpc - # Use the model's solve time instead of cur_time - self.angle_steers_des_time = float(self.last_mpc_ts / 1000000000.0) - self.curvature_factor = VM.curvature_factor(v_ego) - - # This is currently disabled, but it is used to compensate for variable steering rate - ratioDelayFactor = 1. + self.ratioDelayScale * abs(angle_steers / 100.) ** self.ratioDelayExp - - # Determine a proper delay time that includes the model's processing time, which is variable - plan_age = _DT_MPC + cur_time - float(self.last_mpc_ts / 1000000000.0) - total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age + curvature_factor = VM.curvature_factor(v_ego) - # Use steering rate from the last 2 samples to estimate acceleration for a more realistic future steering rate - accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate + 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)) - # Project the future steering angle for the actuator delay only (not model delay) - self.projected_angle_steers = ratioDelayFactor * CP.steerActuatorDelay * accelerated_angle_rate + angle_steers - - 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 and the age of the plan - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, self.projected_angle_steers, self.curvature_factor, CP.steerRatio, total_delay) + # 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, - self.l_poly, self.r_poly, self.p_poly, - PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, self.curvature_factor, v_ego_mpc, PL.PP.lane_width) + l_poly, r_poly, 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) - - # Use last 2 desired angles to determine the model's desired steer rate - self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC + self.angle_steers_des_time = cur_time self.mpc_updated = True # Check for infeasable MPC solution @@ -162,97 +97,24 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") - elif self.frames > 20: - self.steerpub.send(self.steerdata) - self.frames = 0 - self.steerdata = self.influxString - if v_ego < 0.3 or not active: output_steer = 0.0 - self.feed_forward = 0.0 self.pid.reset() - ff_type = "r" - projected_angle_steers_des = 0.0 - projected_angle_steers = 0.0 - restricted_steer_rate = 0.0 else: - # Interpolate desired angle between MPC updates - self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time) - self.avg_angle_steers = (4.0 * self.avg_angle_steers + angle_steers) / 5.0 - - # 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(angle_rate) - self.accel_limit, float(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 project angle steers using current steer rate - projected_angle_steers = float(angle_steers) + self.projection_factor * float(angle_rate) - + # 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 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: - # Decide which feed forward mode should be used (angle or rate). Use more dominant mode, and only if conditions are met - # Spread feed forward out over a period of time to make it more 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(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \ - and (float(restricted_steer_rate) < 0) == (float(self.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(self.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(self.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 # feedforward desired angle + steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 - - # 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) - - - # All but the last 3 lines after here are for real-time dashboarding - 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 - steer_motor = 0.0 - self.frames += 1 - steer_parameter1 = 0.0 - steer_parameter2 = 0.0 - steer_parameter3 = 0.0 - steer_parameter4 = 0.0 - steer_parameter5 = 0.0 - steer_parameter6 = 0.0 - steering_control_active = 0.0 - steer_torque_motor = 0.0 - driver_torque = 0.0 - steer_status = 0.0 - steer_stock_torque = 0.0 - steer_stock_torque_request = 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,%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, steer_status, steer_torque_motor, steering_control_active, steer_parameter1, steer_parameter2, steer_parameter3, steer_parameter4, steer_parameter5, steer_parameter6, steer_stock_torque, steer_stock_torque_request, self.cur_state[0].x, self.cur_state[0].y, self.lateral_error, self.mpc_solution[0].y[0], self.mpc_solution[0].y[1], self.mpc_solution[0].y[2], self.mpc_solution[0].y[3], self.mpc_solution[0].y[4], self.cur_state[0].psi, self.cur_state[0].delta, self.cur_state[0].t, self.curvature_factor, self.slip_factor ,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, projected_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)) + 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) self.sat_flag = self.pid.saturated - self.prev_angle_rate = angle_rate return output_steer, float(self.angle_steers_des) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 1858b8a98e3c94..712c3031edb589 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,5 +1,3 @@ -import numpy as np -import math from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv @@ -18,31 +16,16 @@ def __init__(self): self.lane_width_certainty = 1.0 self.lane_width = 2.85 - def update(self, v_ego, md, LaC=None): + def update(self, v_ego, md): if md is not None: p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line - lateral_error = 0.0 - try: - if LaC is not None and LaC.angle_steers_des_mpc != 0.0: - angle_error = LaC.angle_steers_des_mpc - (0.05 * LaC.avg_angle_steers + 0.1 * LaC.projected_angle_steers) / 0.15 - if LaC is None or angle_error == 0: - print("2") - lateral_error = 0.0 - else: - LaC.lateral_error = -1.0 * np.clip(v_ego * 0.15 * math.tan(math.radians(angle_error)), -0.2, 0.2) - lateral_error = LaC.lateral_error + # only offset left and right lane lines; offsetting p_poly does not make sense + l_poly[3] += CAMERA_OFFSET + r_poly[3] += CAMERA_OFFSET - # only offset left and right lane lines; offsetting p_poly does not make sense - l_poly[3] += CAMERA_OFFSET + lateral_error - r_poly[3] += CAMERA_OFFSET + lateral_error - except: - print(4) - pass - - #print(lateral_error) p_prob = 1. # model does not tell this probability yet, so set to 1 for now l_prob = md.model.leftLane.prob # left line prob r_prob = md.model.rightLane.prob # right line prob diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e1a3c6d727ffa6..fbbbb7391afe91 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -426,7 +426,7 @@ def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): self.last_model = cur_time self.model_dead = False - self.PP.update(CS.vEgo, md, LaC) + self.PP.update(CS.vEgo, md) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan