From 509bee56441485903f8f686d89861e4eea2aae8e Mon Sep 17 00:00:00 2001 From: Greg Esmond Date: Thu, 4 Apr 2019 17:25:32 -0500 Subject: [PATCH] added rate-based feed forward --- cereal/log.capnp | 4 +-- dashboard.py | 13 ++++---- selfdrive/controls/controlsd.py | 1 + selfdrive/controls/lib/latcontrol.py | 48 +++++++++++++++++++++------ selfdrive/controls/lib/pathplanner.py | 8 +++-- 5 files changed, 53 insertions(+), 21 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 43e3d113a7ce59..22eb522506670b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -401,8 +401,8 @@ struct Live100Data { dampAngleSteers @52 :Float32; dampAngleSteersDes @53 :Float32; dampRateSteersDes @54 :Float32; - rateModeFF @56 :Int32; - angleModeFF @57 :Int32; + rateModeFF @56 :Float32; + angleModeFF @57 :Float32; curvature @37 :Float32; # path curvature from vehicle model hudLeadDEPRECATED @14 :Int32; cumLagMs @15 :Float32; diff --git a/dashboard.py b/dashboard.py index b2a5ff198eb91a..80a0a80d55a20b 100644 --- a/dashboard.py +++ b/dashboard.py @@ -36,12 +36,12 @@ def dashboard_thread(rate=300): context = zmq.Context() poller = zmq.Poller() ipaddress = "127.0.0.1" - carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller) + #carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller) #can = messaging.sub_sock(context, service_list['can'].port, addr=ipaddress, poller=poller) can = "disabled" vEgo = 0.0 - #pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller) - pathPlan = None + pathPlan = messaging.sub_sock(context, service_list['pathPlan'].port, addr=ipaddress, conflate=True, poller=poller) + #pathPlan = None live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=False, poller=poller) #liveParameters = messaging.sub_sock(context, service_list['liveParameters'].port, addr=ipaddress, conflate=True, poller=poller) liveParameters = None @@ -140,7 +140,7 @@ def dashboard_thread(rate=300): last_actual = l100.live100.angleSteers v_curv = l100.live100.curvature - influxLineString += ("opData,sources=capnp ff_rate=%d,ff_angle=%d,ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" % + influxLineString += ("opData,sources=capnp ff_rate=%1.3f,ff_angle=%1.3f,ang_err_noise=%1.1f,des_noise=%1.1f,ang_noise=%1.1f,angle_steers_des=%1.2f,angle_steers=%1.2f,dampened_angle_steers_des=%1.2f,dampened_angle_rate_des=%1.2f,dampened_angle_steers=%1.2f,v_ego=%1.2f,steer_override=%1.2f,v_ego=%1.4f,p=%1.2f,i=%1.4f,f=%1.4f,cumLagMs=%1.2f,vCruise=%1.2f %s\n" % (l100.live100.rateModeFF, l100.live100.angleModeFF, angle_error_noise, desired_angle_change_noise, actual_angle_change_noise, l100.live100.angleSteersDes, l100.live100.angleSteers, l100.live100.dampAngleSteersDes, l100.live100.dampAngleRateDes, l100.live100.dampAngleSteers, l100.live100.vEgo, l100.live100.steerOverride, l100.live100.vPid, l100.live100.upSteer, l100.live100.uiSteer, l100.live100.ufSteer, l100.live100.cumLagMs, l100.live100.vCruise, receiveTime)) frame_count += 1 @@ -297,15 +297,16 @@ def dashboard_thread(rate=300): if sample_str != "": sample_str += "," a = _pathPlan.pathPlan.mpcAngles + r = _pathPlan.pathPlan.mpcRates p = _pathPlan.pathPlan #sample_str = ("lane_width=%1.2f,lpoly2=%1.3f,rpoly2=%1.3f,cpoly2=%1.3f,dpoly2=%1.3f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpc7=%1.2f,mpc8=%1.2f,mpc9=%1.2f,mpc10=%1.2f,mpc11=%1.2f,mpc12=%1.2f,mpc13=%1.2f,mpc14=%1.2f,mpc15=%1.2f,mpc16=%1.2f,mpc17=%1.2f,mpc18=%1.2f" % # (_pathPlan.pathPlan.laneWidth, _pathPlan.pathPlan.lPoly[2], _pathPlan.pathPlan.rPoly[2], _pathPlan.pathPlan.cPoly[2], _pathPlan.pathPlan.dPoly[2],_pathPlan.pathPlan.lPoly[3], _pathPlan.pathPlan.rPoly[3], _pathPlan.pathPlan.cPoly[3], _pathPlan.pathPlan.dPoly[3], # _pathPlan.pathPlan.cProb, _pathPlan.pathPlan.lProb, _pathPlan.pathPlan.rProb, a[1], a[2], a[3], # a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14], a[15], a[16], a[17], a[18])) - sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f" % + sample_str = ("lane_width=%1.2f,lpoly0=%1.6f,rpoly0=%1.6f,cpoly0=%1.6f,dpoly0=%1.6f,lpoly1=%1.5f,rpoly1=%1.5f,cpoly1=%1.5f,dpoly1=%1.5f,lpoly2=%1.4f,rpoly2=%1.4f,cpoly2=%1.4f,dpoly2=%1.4f,lpoly3=%1.3f,rpoly3=%1.3f,cpoly3=%1.3f,dpoly3=%1.3f,cProb=%1.3f,lProb=%1.3f,rProb=%1.3f,mpc0=%1.2f,mpc1=%1.2f,mpc2=%1.2f,mpc3=%1.2f,mpc4=%1.2f,mpc5=%1.2f,mpc6=%1.2f,mpcR0=%1.2f,mpcR1=%1.2f,mpcR2=%1.2f,mpcR3=%1.2f,mpcR4=%1.2f,mpcR5=%1.2f,mpcR6=%1.2f" % (p.laneWidth, p.lPoly[0], p.rPoly[0], p.cPoly[0], p.dPoly[0], p.lPoly[1], p.rPoly[1], p.cPoly[1], p.dPoly[1], p.lPoly[2], p.rPoly[2], p.cPoly[2], p.dPoly[2],p.lPoly[3], p.rPoly[3], p.cPoly[3], p.dPoly[3], - p.cProb, p.lProb, p.rProb, a[1], a[2], a[3])) + p.cProb, p.lProb, p.rProb, a[0], a[1], a[2], a[3], a[4], a[5], a[6], r[0], r[1], r[2], r[3], r[4], r[5], r[6])) influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % receiveTime) sample_str = "" frame_count += 1 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index af22560983f545..de53fbc6dab6a9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -355,6 +355,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis "ufAccelCmd": float(LoC.pid.f), "angleSteersDes": float(path_plan.pathPlan.angleSteers), "dampAngleSteersDes": float(LaC.dampened_desired_angle), + "dampRateSteersDes": float(LaC.dampened_desired_rate), "rateModeFF": LaC.rate_mode, "angleModeFF": LaC.angle_mode, "upSteer": float(LaC.pid.p), diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 04586e9a437cf3..6aaa2b2392e30b 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -60,7 +60,7 @@ def __init__(self, CP): self.actual_smoothing = max(1.0, CP.steerDampTime / _DT) self.desired_smoothing = max(1.0, CP.steerMPCDampTime / _DT) self.ff_angle_factor = 1.0 - self.ff_rate_factor = 0.0 + self.ff_rate_factor = 1.0 self.dampened_desired_angle = 0.0 self.steer_counter = 1.0 self.steer_counter_prev = 0.0 @@ -69,8 +69,8 @@ def __init__(self, CP): self.calculate_rate = True self.lane_prob_reset = False self.feed_forward = 0.0 - self.rate_mode = 0 - self.angle_mode = 1 + self.rate_mode = 0.0 + self.angle_mode = 0.0 KpV = [interp(25.0, CP.steerKpBP, CP.steerKpV)] KiV = [interp(25.0, CP.steerKiBP, CP.steerKiV)] @@ -128,27 +128,37 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM self.pid.reset() self.dampened_angle_steers = float(angle_steers) self.dampened_desired_angle = float(angle_steers) + self.dampened_desired_rate = 0.0 + self.dampened_rate_ff = 0.0 + self.dampened_angle_ff = 0.0 else: if self.gernbySteer == False: self.dampened_angle_steers = float(angle_steers) self.dampened_desired_angle = float(path_plan.angleSteers) + self.dampened_desired_rate = float(path_plan.rateSteers) else: - projected_desired_angle = interp(sec_since_boot() + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles) + cur_time = sec_since_boot() + projected_desired_angle = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcAngles) self.dampened_desired_angle += ((projected_desired_angle - self.dampened_desired_angle) / self.desired_smoothing) + projected_desired_rate = interp(cur_time + self.total_desired_projection, path_plan.mpcTimes, path_plan.mpcRates) + self.dampened_desired_rate += ((projected_desired_rate - self.dampened_desired_rate) / self.desired_smoothing) - projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate) if not steer_override: - self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing) + projected_angle_steers = float(angle_steers) + self.total_actual_projection * float(angle_rate) + else: + projected_angle_steers = float(angle_steers) + self.dampened_angle_steers += ((projected_angle_steers - self.dampened_angle_steers) / self.actual_smoothing) - if path_plan.laneProb == 0.0 and self.lane_prob_reset == False: + '''if path_plan.laneProb == 0.0 and self.lane_prob_reset == False: if path_plan.lPoly[3] - path_plan.rPoly[3] > 3.9: print(self.dampened_desired_angle, path_plan.angleSteers) #self.dampened_desired_angle = path_plan.angleSteers self.lane_prob_reset = True elif path_plan.laneProb > 0.0: self.lane_prob_reset = False + ''' if CP.steerControlType == car.CarParams.SteerControlType.torque: @@ -157,9 +167,24 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM self.pid.neg_limit = -steers_max deadzone = 0.0 - angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 0.5) * v_ego**2 - rate_feedforward = self.ff_rate_factor * path_plan.rateSteers * v_ego**2 - rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward)) + angle_feedforward = apply_deadzone(self.ff_angle_factor * (self.dampened_desired_angle - path_plan.angleOffset), 1.0) * v_ego**2 + rate_feedforward = self.ff_rate_factor * self.dampened_desired_rate * v_ego**2 + + self.dampened_angle_ff += (angle_feedforward - self.dampened_angle_ff) / self.desired_smoothing + if angle_feedforward == 0 or (angle_feedforward < 0) == (rate_feedforward < 0): + self.dampened_rate_ff += (rate_feedforward - self.dampened_rate_ff) / self.desired_smoothing + else: + self.dampened_rate_ff += (0.0 - self.dampened_rate_ff) / self.desired_smoothing + + feed_forward = self.dampened_angle_ff + self.dampened_rate_ff + if abs(feed_forward) > 0: + self.angle_mode = max(0.0, min(1.0, self.dampened_angle_ff / feed_forward)) + self.rate_mode = 1.0 - self.angle_mode + else: + self.angle_mode = 0.0 + self.rate_mode = 0.0 + + '''rate_more_significant = (abs(rate_feedforward) > abs(angle_feedforward)) rate_same_direction = (rate_feedforward > 0) == (angle_feedforward > 0) if rate_more_significant and rate_same_direction: # and rate_away_from_zero: @@ -171,9 +196,10 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, CP, VM self.rate_mode = 0 self.angle_mode = 1 self.feed_forward += ((angle_feedforward - self.feed_forward) / self.desired_smoothing) + ''' output_steer = self.pid.update(self.dampened_desired_angle, self.dampened_angle_steers, check_saturation=(v_ego > 10), - override=steer_override, feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone) + override=steer_override, feedforward=feed_forward, speed=v_ego, deadzone=deadzone) self.sat_flag = self.pid.saturated self.prev_angle_steers = float(angle_steers) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index a26416473538cb..5bed52da5a063b 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -82,13 +82,17 @@ def update(self, CP, VM, CS, md, live100, live_parameters): if active: self.mpc_angles[0] = live100.live100.dampAngleSteersDes self.mpc_times[0] = live100.logMonoTime * 1e-9 + self.mpc_rates[0] = live100.live100.dampRateSteersDes for i in range(1,20): - self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias) - self.mpc_rates[i] = math.degrees(self.mpc_solution[0].rate[i] * VM.sR) self.mpc_times[i] = self.mpc_times[i-1] + _DT_MPC + self.mpc_angles[i] = float(math.degrees(self.mpc_solution[0].delta[i] * VM.sR) + angle_offset_bias) + self.mpc_rates[i] = (self.mpc_angles[i] - self.mpc_angles[i-1]) / (self.mpc_times[i] - self.mpc_times[i-1]) + #self.mpc_rates[i] = math.degrees(self.mpc_solution[0].rate[i] * VM.sR) delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) + rate_desired2 = math.degrees(self.mpc_solution[0].rate[1] * VM.sR) + print("%1.2f %1.2f %1.2f %1.2f %1.2f %1.2f " % (rate_desired, self.mpc_rates[1], rate_desired - self.mpc_rates[1], rate_desired2, self.mpc_rates[2], rate_desired2 - self.mpc_rates[2] )) else: #print(CP.steerRateCost, VM.sR) delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR