From 99031cf0eb1f27d5e5d78a35d60a3857bba587ac Mon Sep 17 00:00:00 2001 From: kegman Date: Sun, 16 Dec 2018 10:41:38 -0500 Subject: [PATCH] Merge with kegman-gernbyFFsteer-latest (#69) * Honda Pilot PID Tuning * Adjustable Follow Distance w/HUD (#12) * TR * TR * TR * TR * TR * TR * TR * TR * remove readdistancelines - causes boot error * copying cereal * arne's cereal * HUD distance lines display * capnpp add * revert * revert * Set ACC HUD to KPH * Test - set HUD_ACC distance line to 2 * revert cruise_kph LKAS error * TR * TR * TR * TR * TR * TR * TR * TR * remove readdistancelines - causes boot error * copying cereal * HUD distance lines display * capnpp add * revert * revert * Set ACC HUD to KPH * Test - set HUD_ACC distance line to 2 * revert cruise_kph LKAS error * Remove custom btn var line could be crashing the code when button pressed * HUD distance lines * HUD distance lines * passing CS.CP.readdistancelines * Hardcode hud object dist_lines var * Initializes CS.readdistancelines before passing to hud obj * use integer read_distance_lines instead * Add HUD_DISTANCE_3 for horizontal HUD bars on dash * Default to 2 bars follow distance (1.8s) * Code Cleanup for Distance Adjustment Cleanup Cleanup Cleanup Cleanup Cleanup Cleanup Cleanup Cleanup Cleanup * Auto-distance at surface street speeds Override short distance setting when slow Override distance when slow Override distance if slow Override distance when slow Override distance when slow Increase follow distance when below 60 kph Lead car and rel. velocity to distance override Override distance bars revert Override and restore dist based on speed and accel Step up distance when < 40 km/h and restore Revert When using 1 bar distance: Slow the car down earlier and more aggressively (#18) * Change one bar braking profile to 3 bars * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * fake lead car vars * fake lead vars * Auto Distance Bar Working - tuning values * Logging to log.txt * Override hysteresis for one bar follow * Only set override when decelerating * syntax error fix for override flag * Switch to 1.8s for softer braking * Revert "Switch to 1.8s for softer braking" This reverts commit d53623062ef814509db925fe4e624c3ae8aff1f1. * Disable OTA update for safety * Physical LKAS button disables OP steer and shows dashed lines on HUD (#19) * Change one bar braking profile to 3 bars * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * Update planner.py * fake lead car vars * fake lead vars * Auto Distance Bar Working - tuning values * Logging to log.txt * Override hysteresis for one bar follow * Only set override when decelerating * syntax error fix for override flag * Switch to 1.8s for softer braking * Disable LKAS * Apply Steer = 0 when user presses LKAS button * Add Lkas button * Add LK mode to HUDD data object * Add Dashed Lanes to HUD * Adjust HDD object * return LKAS var in CarState * Return LK var to CS * Syntax error fix * Update carcontroller.py * testing hud object hardcoded hud_lanes and lkMode * Restore accidentally deleted steer command * Update carcontroller.py * Update carcontroller.py * Don't show solid lanes if OP steering disabled * Dashed lines fix during engagement eliminate steering alert when lkas disabled lkas button logic cleanup Reverse the dashed-lane indicator No steering alert if LKAS disabled revert Alert - LKAS errors during steer disable Disable audible alert when steering disabled Delete CS.aEgo < -0.5 override condition * Remove chime disable when steering overriden One bar profile change to 1.8s if < 65 kph Simplified chime instead of beep disable for LK override * Adjust BRAKE_STOPPING_TARGET to 0.25 Keeps the brake pump from going crazy at a stop * Add relative velocity to auto distance for 1 bar setting (#26) * CAMERA_OFFSET = 0.08 * Revert Cost_long to comma value of 0.1 * Do not override to 1.8s when accel from stop * Update logic for override distance bars * Override distance when < 65kph and v_rel not > 5 * Use rel acceleration to reset to 1 bar * Use vRel * Update planner.py * v_lead - vEgo > 1 * vLead - vEgo = -1 * vLead-vEgo < -1.5 * Cleanup Arne's code for distance * vLead - vEgo < 1.25 * Back to vLead - vEgo < -1 * Disable OP steering and audio alarm when blinkers on (#28) * LKAS disable when blinkers are on * Update carcontroller.py * Update carcontroller.py * fix syntax error * Critical alarm if cruise disengages while braking (#32) * Update pathplanner.py * Loud alert in case cruise disables when braking * re-add if statement on speed for loud alert * Update interface.py * increase stopping distance to car by 1.5m * Increase stopping distance to 2m * Revert CAMERA_OFFSET to 0.06 (comma default) * Lane width reduction to 2.85m * Reduce tailgating for one bar distance at surface street speeds & code cleanup (#43) * Gernby's Lane width filter fix (#41) * Fix lane width filtering * Fixed lane width filtering * CAMERA_OFFSET = 0.08 * Minimum distance for one bar distance setting * Don't tailgate at slow speeds with 1 bar distance * x_lead < 5 only * x_lead < 5 syntax fix * self.v_rel < -1 or (x_lead < 10 and self.v_rel >= -1 and self.v_rel < 1) * Tail gating auto distance change * x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.2 * self.lastTR > 0 re-adding reinitialization * Revert "Gernby's Lane width filter fix (#41)" This reverts commit d081cfd8a05d2eb32e0148d477c806e65662dea7. * Increase min distance to 10m for auto distance * Auto distance code cleanup * Add lead car fast approaching autodistance * Auto distance Code cleanup * Don't re-initialize MPC if already initialized * self.lastTR != instead of > * Initialize TR = 1.8 * include if statements vs just assigning boolean * switch to 1 and 0 for TRUE AND FALSE * Revert "switch to 1 and 0 for TRUE AND FALSE" This reverts commit 58dcf718cdc3fca636229bf29942669670cb4487. * Revert "include if statements vs just assigning boolean" This reverts commit da7b5fa5088d62e78ba57d9e4c01e89160e77b9e. * Revert "Initialize TR = 1.8" This reverts commit f9c2571faeb0acaab818bcff146a2a397c0f07ff. * Initialize TR=1.8 inside override loop * Cleanup TR assignment for override loop * Code Cleanup * Syntax error * Syntax error * if statements for autodistance vars * tr-last assignment indent * CS.readdistancelines assignment to self.tr_last * remove else MPC init * Flatten out elif ladder for autodistance * Removed shrinking_fast stopping for now Braking is too sudden with 3 bar distance when radar detects car. Shrinking gap takes care of this with 2 bar distance. * Code Cleanup * Revert CAMERA_OFFSET to Comma default of 0.06 * Merge Gernby's Resonant Steering (#51) * Learn_angle_steer min max increased to +/-7 degs * Increase min one bar distance to 12m for autodist * Merge Gernby's Feed-Forward-Accel-Rate Branch (#49) * Fixed feed-forward to consider steer_offset * fixed a small oops on types * Testing strategies for zero-crossing * Moved angle stat collection to latcontrol.py * First version that runs * added outbound interface for feed-forward table * First working * Added more metrics * Adjusted parameter timing * performance tweaks * Cleanup * untested tweaks * minor tweaks * many untested changes * going the other way with some things... * Best yet * cleaned up personalized "stuff" * more cleanup for general use * untested: minor adjustment to further reduce noise * Fixed defect in desired angle interpolation * some cleanup * reverted change to Ki * Reverted changes to manager.py * Added steering rate consideration to latcontrol * cleaned up for PR * Fixed merge * Testing approach when desired angle close to actual * trying rate-based feed-forward * added self-tuning parms for rate and angle FF * fixed trim logic, and persisted to flash * working amazingly well * decreased time lapse for angle-based FF adjust * many tweaks, self-tuning is a bitch * simulteneous dual mode feedforward working very well * added angular accelleration limit * added super awesome angular accel limit * non-specific save * switching directions again * oops * ugly code with amazing results * awesome, but untested after some cleanup * more cleanup * cleanup of the cleanup? Need to test * works amazingly well ... big O face * cleanup * I wish git was batter for cleanup * removed Steer Offset because Pilot no likey * Increase tailgating 1 bar resume to 0.5 m/s * Gernby's resonant steering v2 * fixed an oops for non-bosch hondas * CAMERA_OFFSET = 0.08 * More conservative tailgating distance and resume * More conservative auto-distance for 1 bar setting * Revert CAMERA_OFFSET to 0.06 * Revert auto-distance for 1 bar distance setting * Revert "Merge Gernby's Resonant Steering" (#53) * Revert "Revert auto-distance for 1 bar distance setting" This reverts commit b7e5fb51e534489d67fac8c4c88e5377983b5487. * Revert "Revert CAMERA_OFFSET to 0.06" This reverts commit 05cf21d2712d762107073c46186050ba994feae5. * Revert "Merge Gernby's Resonant Steering (#51)" This reverts commit d35aa19c67c1074b1e5ecd3f590e761342f684e5. * better crowned road centering: learn_angle +/-7 * Implement csouers 67-70% charging to preserve batt * slightly harder braking when tailgating * x_lead < 5 or (x_lead < 17.5 and self.v_rel < 0.5) * Proper alerts on Eon and disable flashing HUD alert when blinkers on or LKAS off (#63) * Disable steer alert when LKAS off * Display steering unavailable on Eon when LKAS off * Revert chime disable in favor of new method * x_lead < 6 or (x_lead < 17.5 and self.v_rel < 0.5) * logic correction on HUD alert * blinkers on disables HUD alarm * Add Manual Steering alert for LKAS Off mode * Call manual steering alert when LKAS off * Call blinkers alert when blinkers on * Add blinkers alert * Adjust Blinkers alert logic to include LKAS mode * Fix blinker logic * Add new steering required alerts to capnp * Trace LKAS / LDW error * Disable hud alert when LKAS off or blinkers on * Disable HUD alerts if LKAS off or blinkers on * Disable other ACC alert if LKAS off or blinkers on * Revert testing of steering required alert override * Disable steer saturate alert if blinkers on or LKAS off * Disable steer sat alert if LKAS off or blinkers on * Revert steerSaturated logic * disable steer limit alert when LKAS off or blinkers on * Fix Syntax error * Revert steerLimit alert disable * Disable steerSat alert when blinkers on or LKAS off * Revert stopping distance to default for pedal lurch * Re-intro 2m stopping distance * x_lead = 6 * Merge Gernby's latest Feed forward resonant (#68) * Fixed feed-forward to consider steer_offset * fixed a small oops on types * Testing strategies for zero-crossing * Moved angle stat collection to latcontrol.py * First version that runs * added outbound interface for feed-forward table * First working * Added more metrics * Adjusted parameter timing * performance tweaks * Cleanup * untested tweaks * minor tweaks * many untested changes * going the other way with some things... * Best yet * cleaned up personalized "stuff" * more cleanup for general use * untested: minor adjustment to further reduce noise * Fixed defect in desired angle interpolation * some cleanup * reverted change to Ki * Reverted changes to manager.py * Added steering rate consideration to latcontrol * cleaned up for PR * Fixed merge * Testing approach when desired angle close to actual * trying rate-based feed-forward * added self-tuning parms for rate and angle FF * fixed trim logic, and persisted to flash * working amazingly well * decreased time lapse for angle-based FF adjust * many tweaks, self-tuning is a bitch * simulteneous dual mode feedforward working very well * added angular accelleration limit * added super awesome angular accel limit * non-specific save * switching directions again * oops * ugly code with amazing results * awesome, but untested after some cleanup * more cleanup * cleanup of the cleanup? Need to test * works amazingly well ... big O face * cleanup * I wish git was batter for cleanup * hopefully proper merge of merged fixes of merge * fixed an oops for non-bosch hondas * added steer rate to future state calculation * added actual acceleration to future projected state * fixed projected angle error for PIF * untested ... * added comments * completely UNtested * untested merge * battling shitty git * git is sucking giant monkey balls * Trying to figure out what git did... * still trying to un-git myself * Finally got it fixed, works awesome! * Hopefully not corrupted by git * hopefully fixed white space corruption * working again, hopefully resolved git issue * Revert "Implement csouers 67-70% charging to preserve batt" This reverts commit e3249bea61db68a3d7f577aab0ec9acca0b0b974. --- 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, 233 insertions(+), 43 deletions(-) diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 4a82d9a022f36f..83e2e02bcf4ddf 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -166,6 +166,9 @@ 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) { @@ -244,8 +247,17 @@ 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()); @@ -254,10 +266,6 @@ 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]; @@ -265,7 +273,7 @@ class CANParser { p = read_u64_le(dat); } else { p = read_u64_be(dat); - } + } DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p); @@ -273,8 +281,23 @@ 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) { @@ -317,6 +340,8 @@ class CANParser { UpdateCans(sec, cans); } + if (can_forward_period_ns > 0) ForwardCANData(sec); + UpdateValid(sec); zmq_msg_close(&msg); @@ -351,6 +376,11 @@ 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 db42e0af4ce28f..927e3363ff044e 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), @@ -242,7 +243,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/car/honda/interface.py b/selfdrive/car/honda/interface.py index 54f0876ccde12d..148af9687f445f 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.8], [0.24]] + ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.6], [0.18]] 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.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.] @@ -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.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.] @@ -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.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/controls/controlsd.py b/selfdrive/controls/controlsd.py index 85e1218153c856..cdc870feb2ebbe 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, + 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/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 81a371f783ca6c..43e69ffebbbdba 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,47 @@ 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 @@ -43,16 +95,10 @@ 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, 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 @@ -60,30 +106,49 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs self.last_mpc_ts = PL.last_md_ts self.angle_steers_des_prev = self.angle_steers_des_mpc - curvature_factor = VM.curvature_factor(v_ego) + # 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 - 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)) + # 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 - # account for actuation delay - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay) + # 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) 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, - PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width) + 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) # 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 + + # 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.mpc_updated = True # Check for infeasable MPC solution @@ -97,24 +162,97 @@ 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.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: - # 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 + # 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) + 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) + # 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 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) + + # 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)) 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 712c3031edb589..1858b8a98e3c94 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,3 +1,5 @@ +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 @@ -16,16 +18,31 @@ def __init__(self): self.lane_width_certainty = 1.0 self.lane_width = 2.85 - def update(self, v_ego, md): + def update(self, v_ego, md, LaC=None): 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 - # only offset left and right lane lines; offsetting p_poly does not make sense - l_poly[3] += CAMERA_OFFSET - r_poly[3] += CAMERA_OFFSET + 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 + 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 fbbbb7391afe91..e1a3c6d727ffa6 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) + self.PP.update(CS.vEgo, md, LaC) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan