From d04f86800c592c81f35927a0cd7ff89bd557f718 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Sat, 17 Nov 2018 14:29:29 -0600 Subject: [PATCH 01/28] Testing approach when desired angle close to actual --- selfdrive/controls/lib/latcontrol.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index eca046026e84df..9a93dd4db51664 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -114,6 +114,10 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.avg_angle_rate = (self.avg_angle_rate * self.angle_rate_count + angle_rate) / (self.angle_rate_count + 1.) self.angle_rate_count += 1.0 future_angle_steers = (self.avg_angle_rate * _DT_MPC) + self.starting_angle_steers + + if abs(angle_rate) <= 1. or abs(angle_steers - self.angle_steers_des_mpc) <= 0.2: + future_angle_steers = angle_steers + steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max From cec70b72e066b07aa1feb09f55a43dce83ecbfc5 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Sat, 24 Nov 2018 12:15:56 -0600 Subject: [PATCH 02/28] trying rate-based feed-forward --- launch_chffrplus.sh | 10 +-- selfdrive/controls/lib/latcontrol.py | 111 +++++++++++++++++++++------ 2 files changed, 91 insertions(+), 30 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 41f8a607cf9ca5..b882395c795643 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -6,11 +6,11 @@ fi function launch { # apply update - if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then - git reset --hard @{u} && - git clean -xdf && - exec "${BASH_SOURCE[0]}" - fi + #if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + # git reset --hard @{u} && + # git clean -xdf && + # exec "${BASH_SOURCE[0]}" + #fi # no cpu rationing for now echo 0-3 > /dev/cpuset/background/cpus diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 9a93dd4db51664..21686c4bfdad10 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 @@ -37,6 +40,7 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) + self.ff_smoothing = _DT_MPC / _DT def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -50,49 +54,96 @@ 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.steer_rate_cost = steer_rate_cost self.last_mpc_ts = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 - self.starting_angle_steers = 0.0 - self.avg_angle_rate = 0.0 - self.angle_rate_count = 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.steerpub2 = self.context.socket(zmq.PUB) + self.steerpub2.bind("tcp://*:8596") + self.steerdata2 = "" + self.ratioExp = 2.6 + self.ratioScale = 0. + self.steer_steps = [0., 0., 0., 0., 0.] + self.probFactor = 0. + self.prev_output_steer = 0. + self.rough_angle_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] + self.steer_speed_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] + self.tiny_angle_array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + self.steer_torque_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] + self.steer_torque_count = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + self.tiny_torque_array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + self.tiny_torque_count = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] + self.center_angle = 0. + self.center_count = 0. + self.save_steering = False + self.steer_zero_crossing = 0.0 + self.steer_initialized = False + self.avg_angle_steers = 0.0 + self.feed_forward = 0.0 + self.angle_rate_desired = 0.0 + self.pCost = 0.0 + self.lCost = 0.0 + self.rCost = 0.0 + self.hCost = 0.0 + self.srCost = 0.0 + self.frame = 0 + self.angle_steer_des_step = 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): self.mpc_updated = False + + enable_enhancements = True + self.frame += 1 + + cur_Steer_Ratio = CP.steerRatio * 1. + # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts self.starting_angle_steers = angle_steers - self.avg_angle_rate = 0.0 - self.angle_rate_count = 0.0 + self.avg_angle_rate = 0. + self.angle_rate_count = 0 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) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, cur_Steer_Ratio, 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: - delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio + self.isActive = 0 + delta_desired = math.radians(angle_steers - angle_offset) / cur_Steer_Ratio 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_mpc = float(math.degrees(delta_desired * cur_Steer_Ratio) + angle_offset) + self.angle_rate_desired = (self.angle_steers_des_mpc - angle_steers) / _DT_MPC + + #reset desired angle + self.angle_steers_des = angle_steers self.mpc_updated = True # Check for infeasable MPC solution @@ -100,35 +151,45 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly t = sec_since_boot() if self.mpc_nans: 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 + self.cur_state[0].delta = math.radians(angle_steers) / cur_Steer_Ratio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") + if self.steerdata != "" and (self.frame % 50) == 3: + self.steerpub.send(self.steerdata) + self.steerdata = "" + if v_ego < 0.3 or not active: output_steer = 0.0 self.pid.reset() + self.feed_forward = 0. else: - # Project future steering angle using average steer rate since last MPC update - self.avg_angle_rate = (self.avg_angle_rate * self.angle_rate_count + angle_rate) / (self.angle_rate_count + 1.) - self.angle_rate_count += 1.0 - future_angle_steers = (self.avg_angle_rate * _DT_MPC) + self.starting_angle_steers - - if abs(angle_rate) <= 1. or abs(angle_steers - self.angle_steers_des_mpc) <= 0.2: - future_angle_steers = angle_steers + self.angle_steers_des += (self.angle_rate_desired * _DT) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max if CP.steerControlType == car.CarParams.SteerControlType.torque: - steer_feedforward = apply_deadzone(self.angle_steers_des_mpc - float(angle_offset), 0.5) - steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) + self.feed_forward = (((self.ff_smoothing - 1) * self.feed_forward) + (self.angle_rate_desired)) / self.ff_smoothing else: - steer_feedforward = self.angle_steers_des_mpc # feedforward desired angle + self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 - output_steer = self.pid.update(self.angle_steers_des_mpc, future_angle_steers, check_saturation=(v_ego > 10), override=steer_override, - feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=False, override=steer_override, + feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) + + if not steer_override and v_ego > 10. and abs(angle_steers) <= 10: + + future_angle_steers = 0 + + self.steerdata += ("%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,%d|" % (self.isActive, \ + self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ + self.angle_steers_des_mpc, cur_Steer_Ratio, 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_mpc) From 33e44f6dee7b184e317dec165ce46a57e92ac21d Mon Sep 17 00:00:00 2001 From: George Hotz Date: Sat, 24 Nov 2018 18:36:45 -0600 Subject: [PATCH 03/28] added self-tuning parms for rate and angle FF --- selfdrive/controls/lib/latcontrol.py | 67 ++++++++++++++++++++++------ 1 file changed, 54 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 21686c4bfdad10..ff031d883d37ad 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -40,7 +40,7 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) - self.ff_smoothing = _DT_MPC / _DT + self.ff_smoothing = 3. * _DT_MPC / _DT def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -86,6 +86,8 @@ def setup_mpc(self, steer_rate_cost): self.steer_zero_crossing = 0.0 self.steer_initialized = False self.avg_angle_steers = 0.0 + self.feed_forward_rate = 0.0 + self.feed_forward_angle = 0.0 self.feed_forward = 0.0 self.angle_rate_desired = 0.0 self.pCost = 0.0 @@ -95,6 +97,10 @@ def setup_mpc(self, steer_rate_cost): self.srCost = 0.0 self.frame = 0 self.angle_steer_des_step = 0.0 + self.last_ff_a = 0.0 + self.last_ff_r = 0.0 + self.ff_angle_factor = 1.0 + self.ff_rate_factor = 1.0 def reset(self): self.pid.reset() @@ -139,11 +145,13 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.cur_state[0].delta = delta_desired + if self.angle_steers_des == 0.0 and self.angle_steer_des_step == 0.0: + self.angle_steers_des = angle_steers self.angle_steers_des_mpc = float(math.degrees(delta_desired * cur_Steer_Ratio) + angle_offset) - self.angle_rate_desired = (self.angle_steers_des_mpc - angle_steers) / _DT_MPC + self.angle_steer_des_step = _DT * (self.angle_steers_des_mpc - self.angle_steers_des) / _DT_MPC + self.feed_forward_rate = self.ff_rate_factor * (self.angle_steers_des_mpc - angle_steers) / _DT_MPC + self.feed_forward_angle = self.ff_angle_factor * self.angle_steers_des_mpc - float(angle_offset) - #reset desired angle - self.angle_steers_des = angle_steers self.mpc_updated = True # Check for infeasable MPC solution @@ -163,33 +171,66 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly if v_ego < 0.3 or not active: output_steer = 0.0 + self.angle_steer_des_step = 0.0 + self.angle_steers_des = 0.0 + self.feed_forward_angle = 0.0 + self.feed_forward_rate = 0.0 + self.feed_forward = 0.0 + self.last_ff_a = 0.0 + self.last_ff_r = 0.0 self.pid.reset() - self.feed_forward = 0. else: - self.angle_steers_des += (self.angle_rate_desired * _DT) + self.angle_steers_des += self.angle_steer_des_step + future_angle_steers = angle_steers + (angle_rate * _DT) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max + ff_type = "" if CP.steerControlType == car.CarParams.SteerControlType.torque: - self.feed_forward = (((self.ff_smoothing - 1) * self.feed_forward) + (self.angle_rate_desired)) / self.ff_smoothing + if (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)) or ((self.feed_forward_rate < 0) != (self.prev_output_steer < 0)): + ff_type = "a" + self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.ff_smoothing + if self.last_ff_a == 0.0: + self.last_ff_r = 0.0 + self.last_ff_a = sec_since_boot() + 1.0 + else: + ff_type = "r" + self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_rate)) / self.ff_smoothing + if self.last_ff_r == 0.0: + self.last_ff_a = 0.0 + self.last_ff_r = sec_since_boot() + 1.0 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=False, override=steer_override, + if abs(angle_steers) > 3.0: + self.last_ff_a = 0.0 + self.last_ff_r = 0.0 + elif ff_type == "r" and self.last_ff_r > 0.0 and sec_since_boot() > self.last_ff_r: + if (self.pid.p > 0) == (self.feed_forward > 0): + self.ff_rate_factor *= 1.001 + else: + self.ff_rate_factor *= 0.999 + self.last_ff_r = sec_since_boot() + 1.0 + elif ff_type == "a" and self.last_ff_a > 0.0 and sec_since_boot() > self.last_ff_a: + if (self.pid.p > 0) == (self.feed_forward > 0): + self.ff_angle_factor *= 1.001 + else: + self.ff_angle_factor *= 0.999 + self.last_ff_a = sec_since_boot() + 1.0 + + output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - - future_angle_steers = 0 - - self.steerdata += ("%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,%d|" % (self.isActive, \ - self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ + self.steerdata += ("%d,%s,%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, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ self.angle_steers_des_mpc, cur_Steer_Ratio, 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_output_steer = output_steer return output_steer, float(self.angle_steers_des_mpc) From 3f9b21e5343f7749837be0d753f1c324fb64b76b Mon Sep 17 00:00:00 2001 From: George Hotz Date: Sun, 25 Nov 2018 11:27:48 -0600 Subject: [PATCH 04/28] fixed trim logic, and persisted to flash --- selfdrive/controls/lib/latcontrol.py | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index ff031d883d37ad..f3e9e2c54d29e7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -101,6 +101,8 @@ def setup_mpc(self, steer_rate_cost): self.last_ff_r = 0.0 self.ff_angle_factor = 1.0 self.ff_rate_factor = 1.0 + self.save_steering = False + self.steer_initialized = False def reset(self): self.pid.reset() @@ -179,6 +181,17 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.last_ff_a = 0.0 self.last_ff_r = 0.0 self.pid.reset() + + if self.save_steering: + file = open("/sdcard/steering/steer_trims.dat","w") + file.write(json.dumps([self.ff_angle_factor, self.ff_rate_factor])) + file.close() + self.save_steering = False + elif not self.steer_initialized: + self.steer_initialized = True + file = open("/sdcard/steering/steer_trims.dat","r") + self.ff_angle_factor, self.ff_rate_factor = json.loads(file.read()) + print(self.ff_angle_factor, self.ff_rate_factor) else: self.angle_steers_des += self.angle_steer_des_step future_angle_steers = angle_steers + (angle_rate * _DT) @@ -204,29 +217,31 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 - if abs(angle_steers) > 3.0: + if abs(angle_steers) > 4.0: self.last_ff_a = 0.0 self.last_ff_r = 0.0 - elif ff_type == "r" and self.last_ff_r > 0.0 and sec_since_boot() > self.last_ff_r: + elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: if (self.pid.p > 0) == (self.feed_forward > 0): self.ff_rate_factor *= 1.001 else: self.ff_rate_factor *= 0.999 self.last_ff_r = sec_since_boot() + 1.0 - elif ff_type == "a" and self.last_ff_a > 0.0 and sec_since_boot() > self.last_ff_a: + self.save_steering = True + elif ff_type == "a" and self.last_ff_a != 0.0 and sec_since_boot() > self.last_ff_a: if (self.pid.p > 0) == (self.feed_forward > 0): self.ff_angle_factor *= 1.001 else: self.ff_angle_factor *= 0.999 self.last_ff_a = sec_since_boot() + 1.0 + self.save_steering = True output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if not steer_override and v_ego > 10. and abs(angle_steers) <= 10: self.steerdata += ("%d,%s,%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, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ - self.angle_steers_des_mpc, cur_Steer_Ratio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, \ + self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, self.angle_steers_des_mpc, cur_Steer_Ratio, 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)) From 60f9e852c7248629774b2ff4b70c5153dbf15589 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Sun, 25 Nov 2018 12:24:13 -0600 Subject: [PATCH 05/28] working amazingly well --- selfdrive/controls/lib/latcontrol.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index f3e9e2c54d29e7..a2c8f3668709cd 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -206,7 +206,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.ff_smoothing if self.last_ff_a == 0.0: self.last_ff_r = 0.0 - self.last_ff_a = sec_since_boot() + 1.0 + self.last_ff_a = sec_since_boot() + 0.5 else: ff_type = "r" self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_rate)) / self.ff_smoothing @@ -217,29 +217,29 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 - if abs(angle_steers) > 4.0: + if abs(angle_steers) > 3.0: self.last_ff_a = 0.0 self.last_ff_r = 0.0 - elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: + elif ff_type == "a" and self.last_ff_a != 0.0 and sec_since_boot() > self.last_ff_a: if (self.pid.p > 0) == (self.feed_forward > 0): - self.ff_rate_factor *= 1.001 + self.ff_angle_factor *= 1.005 else: - self.ff_rate_factor *= 0.999 - self.last_ff_r = sec_since_boot() + 1.0 + self.ff_angle_factor *= 0.995 + self.last_ff_a = sec_since_boot() + 0.5 self.save_steering = True - elif ff_type == "a" and self.last_ff_a != 0.0 and sec_since_boot() > self.last_ff_a: + elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: if (self.pid.p > 0) == (self.feed_forward > 0): - self.ff_angle_factor *= 1.001 + self.ff_rate_factor *= 1.005 else: - self.ff_angle_factor *= 0.999 - self.last_ff_a = sec_since_boot() + 1.0 + self.ff_rate_factor *= 0.995 + self.last_ff_r = sec_since_boot() + 1.0 self.save_steering = True output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - self.steerdata += ("%d,%s,%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, \ + 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,%d|" % (self.isActive, \ ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, \ self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, self.angle_steers_des_mpc, cur_Steer_Ratio, 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], \ From 527788b6203a30a28fed5b8e7c8c7dd6776a3f86 Mon Sep 17 00:00:00 2001 From: George Hotz Date: Sun, 25 Nov 2018 13:35:56 -0600 Subject: [PATCH 06/28] decreased time lapse for angle-based FF adjust --- selfdrive/controls/lib/latcontrol.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index a2c8f3668709cd..53d13cd516dd25 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -206,7 +206,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.ff_smoothing if self.last_ff_a == 0.0: self.last_ff_r = 0.0 - self.last_ff_a = sec_since_boot() + 0.5 + self.last_ff_a = sec_since_boot() + 0.25 else: ff_type = "r" self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_rate)) / self.ff_smoothing @@ -225,7 +225,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.ff_angle_factor *= 1.005 else: self.ff_angle_factor *= 0.995 - self.last_ff_a = sec_since_boot() + 0.5 + self.last_ff_a = sec_since_boot() + 0.25 self.save_steering = True elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: if (self.pid.p > 0) == (self.feed_forward > 0): From c70e60e0fdbfae36bb5887bb1d66abf50cb10c6f Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Mon, 26 Nov 2018 19:54:51 -0600 Subject: [PATCH 07/28] many tweaks, self-tuning is a bitch --- selfdrive/car/honda/interface.py | 2 +- selfdrive/controls/lib/latcontrol.py | 93 +++++++++++++++++----------- 2 files changed, 58 insertions(+), 37 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3ab57b899e9344..ea2c43d797b238 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -223,7 +223,7 @@ 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.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.steerKpV, ret.steerKiV = [[0.6], [0.09]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 53d13cd516dd25..0e54f6ade20e15 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -40,7 +40,7 @@ def __init__(self, CP): k_f=CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) - self.ff_smoothing = 3. * _DT_MPC / _DT + self.MPC_smoothing = 1. * _DT_MPC / _DT def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -65,9 +65,6 @@ def setup_mpc(self, steer_rate_cost): self.steerpub = self.context.socket(zmq.PUB) self.steerpub.bind("tcp://*:8594") self.steerdata = "" - self.steerpub2 = self.context.socket(zmq.PUB) - self.steerpub2.bind("tcp://*:8596") - self.steerdata2 = "" self.ratioExp = 2.6 self.ratioScale = 0. self.steer_steps = [0., 0., 0., 0., 0.] @@ -97,10 +94,14 @@ def setup_mpc(self, steer_rate_cost): self.srCost = 0.0 self.frame = 0 self.angle_steer_des_step = 0.0 + self.starting_angle_steers = 0.0 self.last_ff_a = 0.0 self.last_ff_r = 0.0 + self.last_i = 0.0 self.ff_angle_factor = 1.0 self.ff_rate_factor = 1.0 + self.angle_rate_count = 0.0 + self.avg_angle_rate = 0.0 self.save_steering = False self.steer_initialized = False @@ -109,18 +110,27 @@ def reset(self): def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL): self.mpc_updated = False + cur_time = sec_since_boot() enable_enhancements = True self.frame += 1 cur_Steer_Ratio = CP.steerRatio * 1. + # Use previous starting angle and average rate to project the NEXT angle_steer + self.avg_angle_rate = ((self.angle_rate_count * self.avg_angle_rate) + float(angle_rate)) / (self.angle_rate_count + 1.0) + self.angle_rate_count += 1.0 + future_angle_steers = float(angle_steers) + _DT * float(angle_rate) + #future_angle_steers = self.starting_angle_steers + (self.avg_angle_rate * (cur_time - self.angle_steers_des_time) + _DT * float(angle_rate)) + # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts - self.starting_angle_steers = angle_steers - self.avg_angle_rate = 0. - self.angle_rate_count = 0 + + # reset actual angle parameters for next iteration, but don't _entirely_ reset avg_angle_rate + self.starting_angle_steers = float(angle_steers) + self.avg_angle_rate = float(angle_rate) + self.angle_rate_count = 1.0 curvature_factor = VM.curvature_factor(v_ego) @@ -147,13 +157,14 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.cur_state[0].delta = delta_desired - if self.angle_steers_des == 0.0 and self.angle_steer_des_step == 0.0: - self.angle_steers_des = angle_steers + self.angle_steers_des_prev = self.angle_steers_des_mpc self.angle_steers_des_mpc = float(math.degrees(delta_desired * cur_Steer_Ratio) + angle_offset) - self.angle_steer_des_step = _DT * (self.angle_steers_des_mpc - self.angle_steers_des) / _DT_MPC - self.feed_forward_rate = self.ff_rate_factor * (self.angle_steers_des_mpc - angle_steers) / _DT_MPC - self.feed_forward_angle = self.ff_angle_factor * self.angle_steers_des_mpc - float(angle_offset) - + self.angle_steers_des_time = cur_time + + self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC + self.feed_forward_rate = self.ff_rate_factor * self.angle_rate_desired + self.feed_forward_angle = self.ff_angle_factor * (self.angle_steers_des_mpc - float(angle_offset)) + self.mpc_updated = True # Check for infeasable MPC solution @@ -173,13 +184,16 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly if v_ego < 0.3 or not active: output_steer = 0.0 - self.angle_steer_des_step = 0.0 + self.angle_steers_des_prev = 0.0 self.angle_steers_des = 0.0 self.feed_forward_angle = 0.0 self.feed_forward_rate = 0.0 self.feed_forward = 0.0 self.last_ff_a = 0.0 self.last_ff_r = 0.0 + self.avg_angle_rate = 0.0 + self.angle_rate_count = 0.0 + self.starting_angle_steers = 0.0 self.pid.reset() if self.save_steering: @@ -193,54 +207,61 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.ff_angle_factor, self.ff_rate_factor = json.loads(file.read()) print(self.ff_angle_factor, self.ff_rate_factor) else: - self.angle_steers_des += self.angle_steer_des_step - future_angle_steers = angle_steers + (angle_rate * _DT) - + + self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max ff_type = "" if CP.steerControlType == car.CarParams.SteerControlType.torque: - if (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)) or ((self.feed_forward_rate < 0) != (self.prev_output_steer < 0)): + if abs(self.angle_rate_desired) > abs(self.avg_angle_rate) or (self.angle_rate_desired < 0 != self.avg_angle_rate < 0): + ff_type = "r" + self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing + if self.last_ff_r == 0.0: + self.last_ff_a = 0.0 + self.last_i = self.pid.i + self.last_ff_r = sec_since_boot() + 0.1 + elif (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)): ff_type = "a" - self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.ff_smoothing + self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.MPC_smoothing if self.last_ff_a == 0.0: self.last_ff_r = 0.0 - self.last_ff_a = sec_since_boot() + 0.25 + self.last_i = self.pid.i + self.last_ff_a = sec_since_boot() + 0.1 else: ff_type = "r" - self.feed_forward = (((self.ff_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_rate)) / self.ff_smoothing - if self.last_ff_r == 0.0: - self.last_ff_a = 0.0 - self.last_ff_r = sec_since_boot() + 1.0 + self.last_ff_r = 0.0 + self.last_ff_r = 0.0 + self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + 0.0) / self.MPC_smoothing else: self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 + print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) if abs(angle_steers) > 3.0: self.last_ff_a = 0.0 self.last_ff_r = 0.0 elif ff_type == "a" and self.last_ff_a != 0.0 and sec_since_boot() > self.last_ff_a: - if (self.pid.p > 0) == (self.feed_forward > 0): - self.ff_angle_factor *= 1.005 + if (self.pid.i > self.last_i) == (self.feed_forward > 0): + self.ff_angle_factor *= 1.001 else: - self.ff_angle_factor *= 0.995 - self.last_ff_a = sec_since_boot() + 0.25 + self.ff_angle_factor *= 0.999 + self.last_ff_a = sec_since_boot() + 0.1 self.save_steering = True elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: - if (self.pid.p > 0) == (self.feed_forward > 0): - self.ff_rate_factor *= 1.005 + if (self.pid.i > self.last_i) == (self.feed_forward > 0): + self.ff_rate_factor *= 1.001 else: - self.ff_rate_factor *= 0.995 - self.last_ff_r = sec_since_boot() + 1.0 + self.ff_rate_factor *= 0.999 + self.last_ff_r = sec_since_boot() + 0.1 self.save_steering = True - output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, + output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) - if not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, future_angle_steers, angle_rate, \ + if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: + 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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, \ self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, self.angle_steers_des_mpc, cur_Steer_Ratio, 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], \ From aa2bc7799494ea11884d327c8f9afd94b56ce038 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Tue, 27 Nov 2018 10:59:01 -0600 Subject: [PATCH 08/28] simulteneous dual mode feedforward working very well --- selfdrive/car/honda/interface.py | 2 +- selfdrive/controls/lib/latcontrol.py | 33 ++++++++++++++++------------ 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index ea2c43d797b238..3ab57b899e9344 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -223,7 +223,7 @@ 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.steerKpV, ret.steerKiV = [[0.6], [0.09]] + 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/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 0e54f6ade20e15..5be34cb4d5145d 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -66,9 +66,9 @@ def setup_mpc(self, steer_rate_cost): self.steerpub.bind("tcp://*:8594") self.steerdata = "" self.ratioExp = 2.6 - self.ratioScale = 0. + self.ratioScale = 15.0 self.steer_steps = [0., 0., 0., 0., 0.] - self.probFactor = 0. + self.probFactor = 0.0 self.prev_output_steer = 0. self.rough_angle_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] self.steer_speed_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] @@ -115,8 +115,13 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly enable_enhancements = True self.frame += 1 - cur_Steer_Ratio = CP.steerRatio * 1. - + if enable_enhancements: + ratioFactor = max(0.1, 1. - self.ratioScale * abs(angle_steers / 100.) ** self.ratioExp) + else: + ratioFactor = 1.0 + + cur_Steer_Ratio = CP.steerRatio * ratioFactor + # Use previous starting angle and average rate to project the NEXT angle_steer self.avg_angle_rate = ((self.angle_rate_count * self.avg_angle_rate) + float(angle_rate)) / (self.angle_rate_count + 1.0) self.angle_rate_count += 1.0 @@ -169,7 +174,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) - t = sec_since_boot() + t = cur_time if self.mpc_nans: 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) / cur_Steer_Ratio @@ -205,7 +210,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.steer_initialized = True file = open("/sdcard/steering/steer_trims.dat","r") self.ff_angle_factor, self.ff_rate_factor = json.loads(file.read()) - print(self.ff_angle_factor, self.ff_rate_factor) + #print(self.ff_angle_factor, self.ff_rate_factor) else: self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired @@ -220,40 +225,40 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly if self.last_ff_r == 0.0: self.last_ff_a = 0.0 self.last_i = self.pid.i - self.last_ff_r = sec_since_boot() + 0.1 + self.last_ff_r = cur_time + 0.1 elif (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)): ff_type = "a" self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.MPC_smoothing if self.last_ff_a == 0.0: self.last_ff_r = 0.0 self.last_i = self.pid.i - self.last_ff_a = sec_since_boot() + 0.1 + self.last_ff_a = cur_time + 0.1 else: ff_type = "r" - self.last_ff_r = 0.0 + self.last_ff_a = 0.0 self.last_ff_r = 0.0 self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + 0.0) / self.MPC_smoothing else: self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 - print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) + #print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) if abs(angle_steers) > 3.0: self.last_ff_a = 0.0 self.last_ff_r = 0.0 - elif ff_type == "a" and self.last_ff_a != 0.0 and sec_since_boot() > self.last_ff_a: + elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: if (self.pid.i > self.last_i) == (self.feed_forward > 0): self.ff_angle_factor *= 1.001 else: self.ff_angle_factor *= 0.999 - self.last_ff_a = sec_since_boot() + 0.1 + self.last_ff_a = cur_time + 0.1 self.save_steering = True - elif ff_type == "r" and self.last_ff_r != 0.0 and sec_since_boot() > self.last_ff_r: + elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: if (self.pid.i > self.last_i) == (self.feed_forward > 0): self.ff_rate_factor *= 1.001 else: self.ff_rate_factor *= 0.999 - self.last_ff_r = sec_since_boot() + 0.1 + self.last_ff_r = cur_time + 0.1 self.save_steering = True output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, From 72ed9ef5882e55f123c14bb1408bb9b2fdbf9961 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Tue, 27 Nov 2018 15:50:29 -0600 Subject: [PATCH 09/28] added angular accelleration limit --- selfdrive/controls/lib/latcontrol.py | 52 ++++++++++++++++------------ 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 5be34cb4d5145d..3549eb3620f4bb 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -170,6 +170,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward_rate = self.ff_rate_factor * self.angle_rate_desired self.feed_forward_angle = self.ff_angle_factor * (self.angle_steers_des_mpc - float(angle_offset)) + self.desired_steer_accelleration = (self.angle_rate_desired - float(angle_rate)) + self.mpc_updated = True # Check for infeasable MPC solution @@ -213,15 +215,21 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly #print(self.ff_angle_factor, self.ff_rate_factor) else: - self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired + if self.angle_rate_desired > float(angle_rate): + restricted_steer_rate = min(float(angle_rate) + 5.0, self.angle_rate_desired) + else: + restricted_steer_rate = max(float(angle_rate) - 5.0, self.angle_rate_desired) + #self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired + self.angle_steers_des = self.angle_steers_des + _DT * restricted_steer_rate steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max ff_type = "" if CP.steerControlType == car.CarParams.SteerControlType.torque: - if abs(self.angle_rate_desired) > abs(self.avg_angle_rate) or (self.angle_rate_desired < 0 != self.avg_angle_rate < 0): + #if abs(self.angle_rate_desired) > abs(self.avg_angle_rate) or (self.angle_rate_desired < 0 != self.avg_angle_rate < 0): + if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != self.avg_angle_rate < 0): ff_type = "r" - self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing + self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * ((self.ff_rate_factor * restricted_steer_rate) + self.feed_forward_angle))) / self.MPC_smoothing if self.last_ff_r == 0.0: self.last_ff_a = 0.0 self.last_i = self.pid.i @@ -243,30 +251,30 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly deadzone = 0.0 #print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) - if abs(angle_steers) > 3.0: - self.last_ff_a = 0.0 - self.last_ff_r = 0.0 - elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: - if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_angle_factor *= 1.001 - else: - self.ff_angle_factor *= 0.999 - self.last_ff_a = cur_time + 0.1 - self.save_steering = True - elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: - if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_rate_factor *= 1.001 - else: - self.ff_rate_factor *= 0.999 - self.last_ff_r = cur_time + 0.1 - self.save_steering = True + #if abs(angle_steers) > 3.0: + # self.last_ff_a = 0.0 + # self.last_ff_r = 0.0 + #elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: + # if (self.pid.i > self.last_i) == (self.feed_forward > 0): + # self.ff_angle_factor *= 1.001 + # else: + # self.ff_angle_factor *= 0.999 + # self.last_ff_a = cur_time + 0.1 + # self.save_steering = True + #elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: + # if (self.pid.i > self.last_i) == (self.feed_forward > 0): + # self.ff_rate_factor *= 1.001 + # else: + # self.ff_rate_factor *= 0.999 + # self.last_ff_r = cur_time + 0.1 + # self.save_steering = True output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, \ + 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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, \ self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, self.angle_steers_des_mpc, cur_Steer_Ratio, 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], \ From fcc3c3edabeb48752293312ab1a441573aeaa30f Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Tue, 27 Nov 2018 19:49:21 -0600 Subject: [PATCH 10/28] added super awesome angular accel limit --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol.py | 89 ++++++++++++++-------------- 2 files changed, 47 insertions(+), 44 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4b564d0d6d915f..5907a1fb576674 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -281,7 +281,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.steeringRate, CS.steeringTorque, 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 3549eb3620f4bb..41dbe08b4c416a 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -93,22 +93,22 @@ def setup_mpc(self, steer_rate_cost): self.hCost = 0.0 self.srCost = 0.0 self.frame = 0 - self.angle_steer_des_step = 0.0 - self.starting_angle_steers = 0.0 + #self.angle_steer_des_step = 0.0 + #self.starting_angle_steers = 0.0 self.last_ff_a = 0.0 self.last_ff_r = 0.0 - self.last_i = 0.0 + #self.last_i = 0.0 self.ff_angle_factor = 1.0 self.ff_rate_factor = 1.0 - self.angle_rate_count = 0.0 - self.avg_angle_rate = 0.0 - self.save_steering = False + #self.angle_rate_count = 0.0 + #self.avg_angle_rate = 0.0 + #self.save_steering = False self.steer_initialized = False 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, angle_rate, steer_override, driver_torque, d_poly, angle_offset, CP, VM, PL): self.mpc_updated = False cur_time = sec_since_boot() @@ -123,9 +123,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly cur_Steer_Ratio = CP.steerRatio * ratioFactor # Use previous starting angle and average rate to project the NEXT angle_steer - self.avg_angle_rate = ((self.angle_rate_count * self.avg_angle_rate) + float(angle_rate)) / (self.angle_rate_count + 1.0) - self.angle_rate_count += 1.0 - future_angle_steers = float(angle_steers) + _DT * float(angle_rate) + #self.avg_angle_rate = ((self.angle_rate_count * self.avg_angle_rate) + float(angle_rate)) / (self.angle_rate_count + 1.0) + #self.angle_rate_count += 1.0 #future_angle_steers = self.starting_angle_steers + (self.avg_angle_rate * (cur_time - self.angle_steers_des_time) + _DT * float(angle_rate)) # TODO: this creates issues in replay when rewinding time: mpc won't run @@ -133,7 +132,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.last_mpc_ts = PL.last_md_ts # reset actual angle parameters for next iteration, but don't _entirely_ reset avg_angle_rate - self.starting_angle_steers = float(angle_steers) + #self.starting_angle_steers = float(angle_steers) self.avg_angle_rate = float(angle_rate) self.angle_rate_count = 1.0 @@ -163,15 +162,14 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.cur_state[0].delta = delta_desired self.angle_steers_des_prev = self.angle_steers_des_mpc + self.angle_steers_des = self.angle_steers_des_prev self.angle_steers_des_mpc = float(math.degrees(delta_desired * cur_Steer_Ratio) + angle_offset) - self.angle_steers_des_time = cur_time + #self.angle_steers_des_time = cur_time self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC self.feed_forward_rate = self.ff_rate_factor * self.angle_rate_desired self.feed_forward_angle = self.ff_angle_factor * (self.angle_steers_des_mpc - float(angle_offset)) - self.desired_steer_accelleration = (self.angle_rate_desired - float(angle_rate)) - self.mpc_updated = True # Check for infeasable MPC solution @@ -198,9 +196,9 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = 0.0 self.last_ff_a = 0.0 self.last_ff_r = 0.0 - self.avg_angle_rate = 0.0 - self.angle_rate_count = 0.0 - self.starting_angle_steers = 0.0 + #self.avg_angle_rate = 0.0 + #self.angle_rate_count = 0.0 + #self.starting_angle_steers = 0.0 self.pid.reset() if self.save_steering: @@ -208,17 +206,22 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly file.write(json.dumps([self.ff_angle_factor, self.ff_rate_factor])) file.close() self.save_steering = False - elif not self.steer_initialized: + if not self.steer_initialized: self.steer_initialized = True file = open("/sdcard/steering/steer_trims.dat","r") self.ff_angle_factor, self.ff_rate_factor = json.loads(file.read()) #print(self.ff_angle_factor, self.ff_rate_factor) else: + future_angle_steers = float(angle_steers) + _DT * float(angle_rate) + if self.angle_rate_desired > float(angle_rate): - restricted_steer_rate = min(float(angle_rate) + 5.0, self.angle_rate_desired) + restricted_steer_rate = min(float(angle_rate) + 4.0, self.angle_rate_desired) else: - restricted_steer_rate = max(float(angle_rate) - 5.0, self.angle_rate_desired) + restricted_steer_rate = max(float(angle_rate) - 4.0, self.angle_rate_desired) + + self.feed_forward_rate = self.ff_rate_factor * restricted_steer_rate + #self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired self.angle_steers_des = self.angle_steers_des + _DT * restricted_steer_rate steers_max = get_steer_max(CP, v_ego) @@ -226,10 +229,9 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.pid.neg_limit = -steers_max ff_type = "" if CP.steerControlType == car.CarParams.SteerControlType.torque: - #if abs(self.angle_rate_desired) > abs(self.avg_angle_rate) or (self.angle_rate_desired < 0 != self.avg_angle_rate < 0): - if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != self.avg_angle_rate < 0): + if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != float(angle_rate) < 0): ff_type = "r" - self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * ((self.ff_rate_factor * restricted_steer_rate) + self.feed_forward_angle))) / self.MPC_smoothing + self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing if self.last_ff_r == 0.0: self.last_ff_a = 0.0 self.last_i = self.pid.i @@ -251,31 +253,32 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly deadzone = 0.0 #print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) - #if abs(angle_steers) > 3.0: - # self.last_ff_a = 0.0 - # self.last_ff_r = 0.0 - #elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: - # if (self.pid.i > self.last_i) == (self.feed_forward > 0): - # self.ff_angle_factor *= 1.001 - # else: - # self.ff_angle_factor *= 0.999 - # self.last_ff_a = cur_time + 0.1 - # self.save_steering = True - #elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: - # if (self.pid.i > self.last_i) == (self.feed_forward > 0): - # self.ff_rate_factor *= 1.001 - # else: - # self.ff_rate_factor *= 0.999 - # self.last_ff_r = cur_time + 0.1 - # self.save_steering = True + if self.angle_rate_desired != restricted_steer_rate: + self.last_ff_a = 0.0 + self.last_ff_r = 0.0 + elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: + if (self.pid.i > self.last_i) == (self.feed_forward > 0): + self.ff_angle_factor *= 1.001 + else: + self.ff_angle_factor *= 0.999 + self.last_ff_a = cur_time + 0.1 + self.save_steering = True + elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: + if (self.pid.i > self.last_i) == (self.feed_forward > 0): + self.ff_rate_factor *= 1.001 + else: + self.ff_rate_factor *= 0.999 + self.last_ff_r = cur_time + 0.1 + self.save_steering = True output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, \ - self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, self.angle_steers_des_mpc, cur_Steer_Ratio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \ + 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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, driver_torque, \ + self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ + self.angle_steers_des_mpc, cur_Steer_Ratio, 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)) From 1a839ba31937243dc43be4a54a2ce159eaf4f586 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Wed, 28 Nov 2018 09:17:36 -0600 Subject: [PATCH 11/28] non-specific save --- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/controls/lib/latcontrol.py | 16 ++++++++-------- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f46343c18fc7d6..c343dfa0c7bfbf 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -265,7 +265,7 @@ def update(self, cp, cp_cam): else: self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] - self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] + self.steer_torque_driver = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3ab57b899e9344..6265473fb6e43c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -420,7 +420,7 @@ def update(self, c): # gear shifter lever ret.gearShifter = self.CS.gear_shifter - ret.steeringTorque = self.CS.steer_torque_driver + ret.steeringTorque = float(self.CS.steer_torque_driver) ret.steeringPressed = self.CS.steer_override # cruise state diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 41dbe08b4c416a..0481af98f06231 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -216,9 +216,9 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver future_angle_steers = float(angle_steers) + _DT * float(angle_rate) if self.angle_rate_desired > float(angle_rate): - restricted_steer_rate = min(float(angle_rate) + 4.0, self.angle_rate_desired) + restricted_steer_rate = min(float(angle_rate) + 2.0, self.angle_rate_desired) else: - restricted_steer_rate = max(float(angle_rate) - 4.0, self.angle_rate_desired) + restricted_steer_rate = max(float(angle_rate) - 2.0, self.angle_rate_desired) self.feed_forward_rate = self.ff_rate_factor * restricted_steer_rate @@ -258,25 +258,25 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver self.last_ff_r = 0.0 elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_angle_factor *= 1.001 + self.ff_angle_factor *= 1.0000000001 else: - self.ff_angle_factor *= 0.999 + self.ff_angle_factor *= 0.99999999 self.last_ff_a = cur_time + 0.1 self.save_steering = True elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_rate_factor *= 1.001 + self.ff_rate_factor *= 1.00000001 else: - self.ff_rate_factor *= 0.999 + self.ff_rate_factor *= 0.99999999 self.last_ff_r = cur_time + 0.1 self.save_steering = True - + output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, driver_torque, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, float(driver_torque), \ self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, future_angle_steers, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ self.angle_steers_des_mpc, cur_Steer_Ratio, 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], \ From 4ead1660ceb1edeaff4240ed1c007953a65d91c7 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Wed, 28 Nov 2018 15:25:23 -0600 Subject: [PATCH 12/28] switching directions again --- cereal/car.capnp | 1 + selfdrive/car/honda/carstate.py | 5 ++++- selfdrive/car/honda/interface.py | 1 + selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol.py | 19 +++++++++++-------- 5 files changed, 18 insertions(+), 10 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 2f25aa0d1ac02e..ddfa944e60d36d 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -103,6 +103,7 @@ struct CarState { steeringAngle @7 :Float32; # deg steeringRate @15 :Float32; # deg/s steeringTorque @8 :Float32; # TODO: standardize units + steeringTorqueMotor @26 :Float32; steeringPressed @9 :Bool; # if the user is using the steering wheel # cruise state diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index c343dfa0c7bfbf..63691120fc34a0 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -36,7 +36,9 @@ 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), + ("STEER_TORQUE_MOTOR", "STEER_STATUS", 0), ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), ("GEAR", "GEARBOX", 0), @@ -236,7 +238,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'] - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] @@ -266,6 +268,7 @@ def update(self, cp, cp_cam): self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] self.steer_torque_driver = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) + self.steer_torque_motor = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']) self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6265473fb6e43c..4fc3d4d3cfb65c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -421,6 +421,7 @@ def update(self, c): ret.gearShifter = self.CS.gear_shifter ret.steeringTorque = float(self.CS.steer_torque_driver) + ret.steeringTorqueMotor = float(self.CS.steer_torque_motor) ret.steeringPressed = self.CS.steer_override # cruise state diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5907a1fb576674..43512454ade33a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -281,7 +281,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, CS.steeringTorque, + actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorque, CS.steeringTorqueMotor, 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 0481af98f06231..02401762b754d7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -108,7 +108,7 @@ def setup_mpc(self, steer_rate_cost): def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver_torque, d_poly, angle_offset, CP, VM, PL): + def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_motor, steer_override, d_poly, angle_offset, CP, VM, PL): self.mpc_updated = False cur_time = sec_since_boot() @@ -216,9 +216,9 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver future_angle_steers = float(angle_steers) + _DT * float(angle_rate) if self.angle_rate_desired > float(angle_rate): - restricted_steer_rate = min(float(angle_rate) + 2.0, self.angle_rate_desired) + restricted_steer_rate = min(float(angle_rate) + 10.5, self.angle_rate_desired) else: - restricted_steer_rate = max(float(angle_rate) - 2.0, self.angle_rate_desired) + restricted_steer_rate = max(float(angle_rate) - 10.5, self.angle_rate_desired) self.feed_forward_rate = self.ff_rate_factor * restricted_steer_rate @@ -231,14 +231,16 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver if CP.steerControlType == car.CarParams.SteerControlType.torque: if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != float(angle_rate) < 0): ff_type = "r" - self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing + self.feed_forward = v_ego**2 * (self.feed_forward_angle) + #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing if self.last_ff_r == 0.0: self.last_ff_a = 0.0 self.last_i = self.pid.i self.last_ff_r = cur_time + 0.1 elif (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)): ff_type = "a" - self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.MPC_smoothing + #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.MPC_smoothing + self.feed_forward = v_ego**2 * self.feed_forward_angle if self.last_ff_a == 0.0: self.last_ff_r = 0.0 self.last_i = self.pid.i @@ -247,7 +249,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver ff_type = "r" self.last_ff_a = 0.0 self.last_ff_r = 0.0 - self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + 0.0) / self.MPC_smoothing + #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + 0.0) / self.MPC_smoothing + self.feed_forward = 0.0 else: self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 @@ -275,8 +278,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, driver feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, restricted_steer_rate ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, float(driver_torque), \ + 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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ self.angle_steers_des_mpc, cur_Steer_Ratio, 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], \ From 9f89e4de69756960c96ad88ac402bf39f5bdc640 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Wed, 28 Nov 2018 15:26:37 -0600 Subject: [PATCH 13/28] oops --- selfdrive/controls/lib/latcontrol.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 02401762b754d7..18eab671d0dbd2 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -231,7 +231,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m if CP.steerControlType == car.CarParams.SteerControlType.torque: if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != float(angle_rate) < 0): ff_type = "r" - self.feed_forward = v_ego**2 * (self.feed_forward_angle) + self.feed_forward = v_ego**2 * (self.feed_forward_rate) #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing if self.last_ff_r == 0.0: self.last_ff_a = 0.0 From b6584848da65155e30686a2edb9e88d97d27a384 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 16:33:27 -0600 Subject: [PATCH 14/28] ugly code with amazing results --- selfdrive/controls/lib/latcontrol.py | 145 +++++++-------------------- 1 file changed, 38 insertions(+), 107 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 18eab671d0dbd2..d81bb093ea756c 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -98,43 +98,30 @@ def setup_mpc(self, steer_rate_cost): self.last_ff_a = 0.0 self.last_ff_r = 0.0 #self.last_i = 0.0 - self.ff_angle_factor = 1.0 - self.ff_rate_factor = 1.0 + self.accel_limited_angle_steers_des = 0.0 + self.ff_angle_factor = 0.5 + self.ff_rate_factor = 2.5 #self.angle_rate_count = 0.0 #self.avg_angle_rate = 0.0 #self.save_steering = False self.steer_initialized = False + self.angle_steer_accel_limit = 1.0 def reset(self): self.pid.reset() def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_motor, steer_override, d_poly, angle_offset, CP, VM, PL): - self.mpc_updated = False cur_time = sec_since_boot() - enable_enhancements = True + cur_steer_ratio = CP.steerRatio self.frame += 1 - - if enable_enhancements: - ratioFactor = max(0.1, 1. - self.ratioScale * abs(angle_steers / 100.) ** self.ratioExp) - else: - ratioFactor = 1.0 - - cur_Steer_Ratio = CP.steerRatio * ratioFactor - - # Use previous starting angle and average rate to project the NEXT angle_steer - #self.avg_angle_rate = ((self.angle_rate_count * self.avg_angle_rate) + float(angle_rate)) / (self.angle_rate_count + 1.0) - #self.angle_rate_count += 1.0 - #future_angle_steers = self.starting_angle_steers + (self.avg_angle_rate * (cur_time - self.angle_steers_des_time) + _DT * float(angle_rate)) - + self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts - - # reset actual angle parameters for next iteration, but don't _entirely_ reset avg_angle_rate - #self.starting_angle_steers = float(angle_steers) - self.avg_angle_rate = float(angle_rate) - self.angle_rate_count = 1.0 + self.starting_angle_steers = angle_steers + self.avg_angle_rate = 0.0 + self.angle_rate_count = 0.0 curvature_factor = VM.curvature_factor(v_ego) @@ -143,145 +130,89 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m 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, cur_Steer_Ratio, CP.steerActuatorDelay) + 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, 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) / cur_Steer_Ratio + delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio self.cur_state[0].delta = delta_desired self.angle_steers_des_prev = self.angle_steers_des_mpc - self.angle_steers_des = self.angle_steers_des_prev - self.angle_steers_des_mpc = float(math.degrees(delta_desired * cur_Steer_Ratio) + angle_offset) - #self.angle_steers_des_time = cur_time - + self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC - self.feed_forward_rate = self.ff_rate_factor * self.angle_rate_desired - self.feed_forward_angle = self.ff_angle_factor * (self.angle_steers_des_mpc - float(angle_offset)) - + self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0) self.mpc_updated = True - + # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) - t = cur_time + t = sec_since_boot() if self.mpc_nans: 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) / cur_Steer_Ratio + self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") - if self.steerdata != "" and (self.frame % 50) == 3: + elif self.steerdata != "" and (self.frame % 10) == 3: self.steerpub.send(self.steerdata) self.steerdata = "" if v_ego < 0.3 or not active: output_steer = 0.0 - self.angle_steers_des_prev = 0.0 - self.angle_steers_des = 0.0 - self.feed_forward_angle = 0.0 - self.feed_forward_rate = 0.0 self.feed_forward = 0.0 - self.last_ff_a = 0.0 - self.last_ff_r = 0.0 - #self.avg_angle_rate = 0.0 - #self.angle_rate_count = 0.0 - #self.starting_angle_steers = 0.0 + self.angle_steers_des_time = 0.0 self.pid.reset() - - if self.save_steering: - file = open("/sdcard/steering/steer_trims.dat","w") - file.write(json.dumps([self.ff_angle_factor, self.ff_rate_factor])) - file.close() - self.save_steering = False - if not self.steer_initialized: - self.steer_initialized = True - file = open("/sdcard/steering/steer_trims.dat","r") - self.ff_angle_factor, self.ff_rate_factor = json.loads(file.read()) - #print(self.ff_angle_factor, self.ff_rate_factor) else: - - future_angle_steers = float(angle_steers) + _DT * float(angle_rate) + self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (_DT + cur_time - self.angle_steers_des_time) + restricted_steer_rate = np.clip(self.angle_rate_desired , float(angle_rate) - self.angle_steer_accel_limit, float(angle_rate) + self.angle_steer_accel_limit) + self.accel_limited_angle_steers_des = float(angle_steers) + _DT * restricted_steer_rate - if self.angle_rate_desired > float(angle_rate): - restricted_steer_rate = min(float(angle_rate) + 10.5, self.angle_rate_desired) - else: - restricted_steer_rate = max(float(angle_rate) - 10.5, self.angle_rate_desired) + #restricted_steer_rate = (self.angle_steers_des - float(angle_steers)) / _DT + #self.accel_limited_angle_steers_des = self.angle_steers_des - self.feed_forward_rate = self.ff_rate_factor * restricted_steer_rate + future_angle_steers = float(angle_steers) + _DT * float(angle_rate) + #future_angle_steers = angle_steers - #self.angle_steers_des = self.angle_steers_des_prev + (_DT + cur_time - self.angle_steers_des_time) * self.angle_rate_desired - self.angle_steers_des = self.angle_steers_des + _DT * restricted_steer_rate steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max - ff_type = "" if CP.steerControlType == car.CarParams.SteerControlType.torque: - if abs(restricted_steer_rate) > abs(float(angle_rate)) or (restricted_steer_rate < 0 != float(angle_rate) < 0): + #self.last_ff_a = 0.0 + #self.last_ff_r = 0.0 + if abs(2.5 * float(restricted_steer_rate)) > abs(0.7 * float(self.accel_limited_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.accel_limited_angle_steers_des) - float(angle_offset) - 0.5 < 0): ff_type = "r" - self.feed_forward = v_ego**2 * (self.feed_forward_rate) - #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * (self.feed_forward_rate + self.feed_forward_angle))) / self.MPC_smoothing - if self.last_ff_r == 0.0: - self.last_ff_a = 0.0 - self.last_i = self.pid.i - self.last_ff_r = cur_time + 0.1 - elif (abs(self.feed_forward_angle) - 0.5 > abs(self.feed_forward_rate)): + self.feed_forward = ((4. * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / 5. + elif abs(self.accel_limited_angle_steers_des - float(angle_offset)) > 0.1: ff_type = "a" - #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + (v_ego**2 * self.feed_forward_angle)) / self.MPC_smoothing - self.feed_forward = v_ego**2 * self.feed_forward_angle - if self.last_ff_a == 0.0: - self.last_ff_r = 0.0 - self.last_i = self.pid.i - self.last_ff_a = cur_time + 0.1 + self.feed_forward = ((4. * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.accel_limited_angle_steers_des) - float(angle_offset), 0.5))) / 5. else: ff_type = "r" - self.last_ff_a = 0.0 - self.last_ff_r = 0.0 - #self.feed_forward = (((self.MPC_smoothing - 1.0) * self.feed_forward) + 0.0) / self.MPC_smoothing - self.feed_forward = 0.0 + self.feed_forward = ((4. * self.feed_forward) + 0.0) / 5. else: self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle deadzone = 0.0 - #print (self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate) - - if self.angle_rate_desired != restricted_steer_rate: - self.last_ff_a = 0.0 - self.last_ff_r = 0.0 - elif ff_type == "a" and self.last_ff_a != 0.0 and cur_time > self.last_ff_a: - if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_angle_factor *= 1.0000000001 - else: - self.ff_angle_factor *= 0.99999999 - self.last_ff_a = cur_time + 0.1 - self.save_steering = True - elif ff_type == "r" and self.last_ff_r != 0.0 and cur_time > self.last_ff_r: - if (self.pid.i > self.last_i) == (self.feed_forward > 0): - self.ff_rate_factor *= 1.00000001 - else: - self.ff_rate_factor *= 0.99999999 - self.last_ff_r = cur_time + 0.1 - self.save_steering = True - output_steer = self.pid.update(self.angle_steers_des, future_angle_steers, check_saturation=False, override=steer_override, + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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, angle_rate, self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ - self.angle_steers_des_mpc, cur_Steer_Ratio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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, cur_steer_ratio, 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)) From d4acc241114503b5c715bbc6b7ba889a5b20b24c Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 20:47:26 -0600 Subject: [PATCH 15/28] awesome, but untested after some cleanup --- selfdrive/controls/lib/latcontrol.py | 93 ++++++++-------------------- 1 file changed, 27 insertions(+), 66 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d81bb093ea756c..3f81de8e329cd6 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -40,7 +40,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.MPC_smoothing = 1. * _DT_MPC / _DT def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -54,10 +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.steer_rate_cost = steer_rate_cost - self.last_mpc_ts = 0.0 - self.angle_steers_des = 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 @@ -65,63 +62,24 @@ def setup_mpc(self, steer_rate_cost): self.steerpub = self.context.socket(zmq.PUB) self.steerpub.bind("tcp://*:8594") self.steerdata = "" - self.ratioExp = 2.6 - self.ratioScale = 15.0 - self.steer_steps = [0., 0., 0., 0., 0.] - self.probFactor = 0.0 - self.prev_output_steer = 0. - self.rough_angle_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] - self.steer_speed_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] - self.tiny_angle_array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] - self.steer_torque_array = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] - self.steer_torque_count = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] - self.tiny_torque_array = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] - self.tiny_torque_count = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] - self.center_angle = 0. - self.center_count = 0. - self.save_steering = False - self.steer_zero_crossing = 0.0 - self.steer_initialized = False - self.avg_angle_steers = 0.0 - self.feed_forward_rate = 0.0 - self.feed_forward_angle = 0.0 + self.smooth_factor = 11.0 * _DT_MPC / _DT self.feed_forward = 0.0 self.angle_rate_desired = 0.0 - self.pCost = 0.0 - self.lCost = 0.0 - self.rCost = 0.0 - self.hCost = 0.0 - self.srCost = 0.0 self.frame = 0 - #self.angle_steer_des_step = 0.0 - #self.starting_angle_steers = 0.0 - self.last_ff_a = 0.0 - self.last_ff_r = 0.0 - #self.last_i = 0.0 - self.accel_limited_angle_steers_des = 0.0 self.ff_angle_factor = 0.5 self.ff_rate_factor = 2.5 - #self.angle_rate_count = 0.0 - #self.avg_angle_rate = 0.0 - #self.save_steering = False - self.steer_initialized = False - self.angle_steer_accel_limit = 1.0 + self.angle_steer_accel_limit = 0.2 def reset(self): self.pid.reset() def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_motor, steer_override, d_poly, angle_offset, CP, VM, PL): cur_time = sec_since_boot() - - cur_steer_ratio = CP.steerRatio self.frame += 1 self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: self.last_mpc_ts = PL.last_md_ts - self.starting_angle_steers = angle_steers - self.avg_angle_rate = 0.0 - self.angle_rate_count = 0.0 curvature_factor = VM.curvature_factor(v_ego) @@ -152,7 +110,6 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0) self.mpc_updated = True - # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() @@ -171,36 +128,28 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m if v_ego < 0.3 or not active: output_steer = 0.0 self.feed_forward = 0.0 - self.angle_steers_des_time = 0.0 self.pid.reset() else: - self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (_DT + cur_time - self.angle_steers_des_time) - restricted_steer_rate = np.clip(self.angle_rate_desired , float(angle_rate) - self.angle_steer_accel_limit, float(angle_rate) + self.angle_steer_accel_limit) - self.accel_limited_angle_steers_des = float(angle_steers) + _DT * restricted_steer_rate - - #restricted_steer_rate = (self.angle_steers_des - float(angle_steers)) / _DT - #self.accel_limited_angle_steers_des = self.angle_steers_des - - future_angle_steers = float(angle_steers) + _DT * float(angle_rate) - #future_angle_steers = angle_steers + 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)) / _DT , float(angle_rate) - self.angle_steer_accel_limit, float(angle_rate) + self.angle_steer_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 if CP.steerControlType == car.CarParams.SteerControlType.torque: - #self.last_ff_a = 0.0 - #self.last_ff_r = 0.0 - if abs(2.5 * float(restricted_steer_rate)) > abs(0.7 * float(self.accel_limited_angle_steers_des) - float(angle_offset)) - 0.5 \ + 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(self.accel_limited_angle_steers_des) - float(angle_offset) - 0.5 < 0): + and (float(restricted_steer_rate) < 0) == (float(future_angle_steers_des) - float(angle_offset) - 0.5 < 0): ff_type = "r" - self.feed_forward = ((4. * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / 5. - elif abs(self.accel_limited_angle_steers_des - float(angle_offset)) > 0.1: + 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 = ((4. * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.accel_limited_angle_steers_des) - float(angle_offset), 0.5))) / 5. + 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 = ((4. * self.feed_forward) + 0.0) / 5. + 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 @@ -209,14 +158,26 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: + 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 + 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,%d|" % (self.isActive, \ ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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, cur_steer_ratio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \ + 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_output_steer = output_steer return output_steer, float(self.angle_steers_des_mpc) From dbe68357750ab508dbf3d7f7941bbec213832d34 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 21:01:27 -0600 Subject: [PATCH 16/28] more cleanup --- selfdrive/controls/lib/latcontrol.py | 56 ++++++++++++++-------------- 1 file changed, 27 insertions(+), 29 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 3f81de8e329cd6..b720582226b9f3 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -53,8 +53,9 @@ 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.last_mpc_ts = 0.0 - self.angle_steers_des = 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 @@ -65,7 +66,6 @@ def setup_mpc(self, steer_rate_cost): self.smooth_factor = 11.0 * _DT_MPC / _DT self.feed_forward = 0.0 self.angle_rate_desired = 0.0 - self.frame = 0 self.ff_angle_factor = 0.5 self.ff_rate_factor = 2.5 self.angle_steer_accel_limit = 0.2 @@ -75,11 +75,11 @@ def reset(self): def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_motor, steer_override, d_poly, angle_offset, CP, VM, PL): cur_time = sec_since_boot() - self.frame += 1 self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: 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) @@ -105,11 +105,11 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m self.cur_state[0].delta = delta_desired - self.angle_steers_des_prev = self.angle_steers_des_mpc self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) - self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC 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 self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() @@ -121,7 +121,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") - elif self.steerdata != "" and (self.frame % 10) == 3: + elif self.steerdata != "" and len(self.steerdata) > 40000: self.steerpub.send(self.steerdata) self.steerdata = "" @@ -153,31 +153,29 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m 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=self.feed_forward, speed=v_ego, deadzone=deadzone) - if True == True: # not steer_override and v_ego > 10. and abs(angle_steers) <= 10: - 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 - - 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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.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 + + 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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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_mpc) + return output_steer, float(self.angle_steers_des) From c186f5c6823586c597801d8d440f77c89825b935 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 21:08:31 -0600 Subject: [PATCH 17/28] cleanup of the cleanup? Need to test --- selfdrive/controls/lib/latcontrol.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index b720582226b9f3..60aaf06d0a66db 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -53,7 +53,7 @@ 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.last_mpc_ts = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 @@ -168,6 +168,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m 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,%d|" % (self.isActive, \ ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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), \ From 5ed48fdb1abe509e1e3357233a3affcc11fd665e Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 22:02:57 -0600 Subject: [PATCH 18/28] works amazingly well ... big O face --- selfdrive/car/honda/interface.py | 2 -- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol.py | 8 ++++---- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 4fc3d4d3cfb65c..e2f95143189466 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -420,8 +420,6 @@ def update(self, c): # gear shifter lever ret.gearShifter = self.CS.gear_shifter - ret.steeringTorque = float(self.CS.steer_torque_driver) - ret.steeringTorqueMotor = float(self.CS.steer_torque_motor) ret.steeringPressed = self.CS.steer_override # cruise state diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 43512454ade33a..4b564d0d6d915f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -281,7 +281,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, CS.steeringTorque, CS.steeringTorqueMotor, + 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 60aaf06d0a66db..04fd21677b9053 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -66,14 +66,14 @@ def setup_mpc(self, steer_rate_cost): self.smooth_factor = 11.0 * _DT_MPC / _DT self.feed_forward = 0.0 self.angle_rate_desired = 0.0 - self.ff_angle_factor = 0.5 + self.ff_angle_factor = 0.25 self.ff_rate_factor = 2.5 - self.angle_steer_accel_limit = 0.2 + self.accel_limit = 0.3 def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_motor, 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 @@ -131,7 +131,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, driver_torque, steer_m self.pid.reset() else: 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)) / _DT , float(angle_rate) - self.angle_steer_accel_limit, float(angle_rate) + self.angle_steer_accel_limit) + restricted_steer_rate = np.clip((self.angle_steers_des - float(angle_steers)) / _DT , 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) From d820de2423ae64cf26a0fdd6620674a2d3e7fb60 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 22:12:46 -0600 Subject: [PATCH 19/28] cleanup --- cereal/car.capnp | 1 - selfdrive/car/honda/carstate.py | 2 -- 2 files changed, 3 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index ddfa944e60d36d..2f25aa0d1ac02e 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -103,7 +103,6 @@ struct CarState { steeringAngle @7 :Float32; # deg steeringRate @15 :Float32; # deg/s steeringTorque @8 :Float32; # TODO: standardize units - steeringTorqueMotor @26 :Float32; steeringPressed @9 :Bool; # if the user is using the steering wheel # cruise state diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 63691120fc34a0..f732cadbf3ccf4 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -38,7 +38,6 @@ def get_can_signals(CP): ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), ("STEER_ANGLE_OFFSET", "STEERING_SENSORS", 0), ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), - ("STEER_TORQUE_MOTOR", "STEER_STATUS", 0), ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), ("GEAR", "GEARBOX", 0), @@ -268,7 +267,6 @@ def update(self, cp, cp_cam): self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] self.steer_torque_driver = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) - self.steer_torque_motor = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']) self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] From 1aa2b75260462bd220d73861a2c6e0f85bc02412 Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 29 Nov 2018 22:23:07 -0600 Subject: [PATCH 20/28] I wish git was batter for cleanup --- selfdrive/car/honda/carstate.py | 2 +- selfdrive/car/honda/interface.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f732cadbf3ccf4..ae25d9675e61e2 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -266,7 +266,7 @@ def update(self, cp, cp_cam): else: self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] - self.steer_torque_driver = float(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) + self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e2f95143189466..3ab57b899e9344 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -420,6 +420,7 @@ def update(self, c): # gear shifter lever ret.gearShifter = self.CS.gear_shifter + ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override # cruise state From 559593b415ec6a665d8148bd84f7eaa767cfed16 Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Sat, 1 Dec 2018 20:36:32 -0600 Subject: [PATCH 21/28] hopefully proper merge of merged fixes of merge --- selfdrive/car/honda/carstate.py | 6 +++++- selfdrive/controls/lib/latcontrol.py | 26 +++++++++++++------------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index ae25d9675e61e2..54c03cbd45b742 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -237,7 +237,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'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET'] + if CP.radarOffCan: + 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/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 04fd21677b9053..3b4a805797f218 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -40,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 @@ -59,16 +69,6 @@ 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() @@ -131,7 +131,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.pid.reset() else: 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)) / _DT , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit) + 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) @@ -171,8 +171,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly 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,%d|" % (self.isActive, \ - ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, 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.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], \ From 8837355c306b3f89d49c955d8fb2cb9aaef3a6e4 Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Sat, 1 Dec 2018 22:22:25 -0600 Subject: [PATCH 22/28] fixed an oops for non-bosch hondas --- selfdrive/car/honda/carstate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 54c03cbd45b742..45045762daed11 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -237,7 +237,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 CP.radarOffCan: + 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'] From 68c9a0a5fd848acfb16689c7869426cad49ac397 Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Sun, 2 Dec 2018 17:39:35 -0600 Subject: [PATCH 23/28] added steer rate to future state calculation --- selfdrive/controls/lib/latcontrol.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 3b4a805797f218..2cc47f65c30395 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -50,6 +50,8 @@ def __init__(self, CP): self.ff_angle_factor = 1.0 self.ff_rate_factor = self.ff_angle_factor * 3.0 self.accel_limit = 5.0 + self.curvature_factor = 0.0 + self.slip_factor = 0.0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -81,19 +83,19 @@ 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 - curvature_factor = VM.curvature_factor(v_ego) + self.curvature_factor = VM.curvature_factor(v_ego) 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) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, (CP.steerActuatorDelay * angle_rate + angle_steers), self.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, curvature_factor, v_ego_mpc, PL.PP.lane_width) + 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: @@ -168,11 +170,11 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.steer_rate_cost = 0.0 self.avg_angle_rate = 0.0 self.angle_rate_count = 0.0 - driver_torque = 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.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,%d|" % (self.isActive, \ + ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, 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, 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], \ From 2d988b9118b11615d944d3804943262832963bfd Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Mon, 3 Dec 2018 14:37:37 -0600 Subject: [PATCH 24/28] added actual acceleration to future projected state --- selfdrive/car/honda/interface.py | 2 +- selfdrive/controls/lib/latcontrol.py | 22 +++++++++++++++++----- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3ab57b899e9344..80f8eb5c42ffe0 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -370,7 +370,7 @@ def get_params(candidate, fingerprint): ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 - ret.steerRateCost = 0.5 + ret.steerRateCost = 0.35 return ret diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 2cc47f65c30395..42ed45ab064031 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -47,11 +47,15 @@ def __init__(self, CP): 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.ff_angle_factor = 0.5 + self.ff_rate_factor = 8.0 self.accel_limit = 5.0 self.curvature_factor = 0.0 self.slip_factor = 0.0 + self.ratioDelayExp = 2.0 + self.ratioDelayScale = 0. + self.prev_angle_rate = 0 + self.frames = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -76,7 +80,7 @@ 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): - cur_time = sec_since_boot() + cur_time = sec_since_boot() self.mpc_updated = False # TODO: this creates issues in replay when rewinding time: mpc won't run if self.last_mpc_ts < PL.last_md_ts: @@ -84,13 +88,18 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.angle_steers_des_prev = self.angle_steers_des_mpc self.curvature_factor = VM.curvature_factor(v_ego) + ratioDelayFactor = 1. + self.ratioDelayScale * abs(angle_steers / 100.) ** self.ratioDelayExp + plan_age = _DT_MPC + cur_time - float(PL.last_md_ts / 1000000000.0) + total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age + accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate + 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 - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, (CP.steerActuatorDelay * angle_rate + angle_steers), self.curvature_factor, CP.steerRatio, CP.steerActuatorDelay) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, 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, @@ -123,8 +132,9 @@ 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.steerdata != "" and len(self.steerdata) > 40000: + elif self.steerdata != "" and self.frames > 10: self.steerpub.send(self.steerdata) + self.frames = 0 self.steerdata = "" if v_ego < 0.3 or not active: @@ -172,6 +182,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.angle_rate_count = 0.0 driver_torque = 0.0 steer_motor = 0.0 + self.frames += 1 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,%d|" % (self.isActive, \ ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, 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), \ @@ -182,4 +193,5 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly 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) From 46bf0aa14acc16356b0943d4bb02d29bf178b574 Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Tue, 4 Dec 2018 12:23:55 -0600 Subject: [PATCH 25/28] fixed projected angle error for PIF --- selfdrive/can/parser.cc | 44 ++++++++++++++++++++++++++++++++++------- 1 file changed, 37 insertions(+), 7 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; }; From dbdc4226e136ca304a9480c4f444969b018d349b Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Tue, 4 Dec 2018 13:22:41 -0600 Subject: [PATCH 26/28] untested ... --- selfdrive/controls/lib/latcontrol.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 42ed45ab064031..47eb3686266781 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -44,11 +44,12 @@ def __init__(self, CP): self.steerpub = self.context.socket(zmq.PUB) self.steerpub.bind("tcp://*:8594") self.steerdata = "" - self.smooth_factor = CP.steerActuatorDelay / _DT + self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT + self.projection_factor = 5.0 * _DT self.feed_forward = 0.0 self.angle_rate_desired = 0.0 self.ff_angle_factor = 0.5 - self.ff_rate_factor = 8.0 + self.ff_rate_factor = 5.0 self.accel_limit = 5.0 self.curvature_factor = 0.0 self.slip_factor = 0.0 @@ -144,28 +145,28 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly else: 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) + projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_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 if CP.steerControlType == car.CarParams.SteerControlType.torque: - 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 \ + 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(future_angle_steers_des) - float(angle_offset) - 0.5 < 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(future_angle_steers_des - float(angle_offset)) > 0.5: + 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(future_angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor + 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_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, + 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.pCost = 0.0 @@ -186,7 +187,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly 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,%d|" % (self.isActive, \ ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, 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, future_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \ + 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], \ From a559a7ddbf51738ded33e82e29a40ad03d900c4e Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Tue, 4 Dec 2018 20:35:08 -0600 Subject: [PATCH 27/28] added comments --- selfdrive/controls/lib/latcontrol.py | 53 +++++++++++++++++++++------- 1 file changed, 41 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 47eb3686266781..c57bd8361917e9 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -40,23 +40,25 @@ 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 = 5.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 + + # variables for dashboarding self.context = zmq.Context() self.steerpub = self.context.socket(zmq.PUB) self.steerpub.bind("tcp://*:8594") self.steerdata = "" - self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT - self.projection_factor = 5.0 * _DT - self.feed_forward = 0.0 - self.angle_rate_desired = 0.0 - self.ff_angle_factor = 0.5 - self.ff_rate_factor = 5.0 - self.accel_limit = 5.0 + self.frames = 0 self.curvature_factor = 0.0 self.slip_factor = 0.0 - self.ratioDelayExp = 2.0 - self.ratioDelayScale = 0. - self.prev_angle_rate = 0 - self.frames = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -89,17 +91,25 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.angle_steers_des_prev = self.angle_steers_des_mpc 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(PL.last_md_ts / 1000000000.0) total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age + + # 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 + + # Project the future steering angle for the actuator delay only (not model delay) 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 + # account for actuation delay and the age of the plan self.cur_state = calc_states_after_delay(self.cur_state, v_ego, 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 @@ -118,7 +128,11 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset) + + # Use the model's solve time instead of cur_time self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0) + + # 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 @@ -143,15 +157,26 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.feed_forward = 0.0 self.pid.reset() 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) + + # 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 + 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): @@ -166,9 +191,13 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly else: self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle 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 From e222d48a0367551fe3bd9a275c8c204d935ec3eb Mon Sep 17 00:00:00 2001 From: "gernby2002@yahoo.com" Date: Wed, 5 Dec 2018 19:01:14 -0600 Subject: [PATCH 28/28] completely UNtested --- panda/board/safety/safety_honda.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index fbee6cfe861f53..c1b3abe2d22930 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -153,7 +153,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = to_fwd->RIR>>21; if (bus_num == 0) { return 2; - } else if (bus_num == 2 && addr != 0xE4 && addr != 0x194 && addr != 0x1FA && + } else if (bus_num == 2 && addr != 0xE4 && addr != 0xE5 && addr != 0x194 && addr != 0x1FA && addr != 0x30C && addr != 0x33D && addr != 0x39F) { return 0; } @@ -164,7 +164,7 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 1 || bus_num == 2) { int addr = to_fwd->RIR>>21; - return addr != 0xE4 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1; + return addr != 0xE4 && addr != 0xE5 && addr != 0x33D ? (uint8_t)(~bus_num & 0x3) : -1; } return -1; }