Skip to content

Commit

Permalink
added self-tuning parms for rate and angle FF
Browse files Browse the repository at this point in the history
  • Loading branch information
@geohot
geohot committed Nov 25, 2018
1 parent cec70b7 commit 33e44f6
Showing 1 changed file with 54 additions and 13 deletions.
67 changes: 54 additions & 13 deletions selfdrive/controls/lib/latcontrol.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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()
Expand Down Expand Up @@ -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
Expand All @@ -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)

0 comments on commit 33e44f6

Please sign in to comment.