diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e7eb1771c..4f336a359 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -183,9 +183,9 @@ class CarState(object): self.a_ego = float(v_ego_x[1]) self.standstill = not v_wheel > 0.001 - if self.CP.carFingerprint in TSS2_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH: + if self.CP.carFingerprint in TSS2_CAR: self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - elif self.CP.carFingerprint in NO_DSU_CAR: + elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH: # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start. # need to apply an offset as soon as the steering angle measurements are both received self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset diff --git a/selfdrive/controls/lib/curvature_learner.py b/selfdrive/controls/lib/curvature_learner.py index 507aed48c..22f71b65f 100644 --- a/selfdrive/controls/lib/curvature_learner.py +++ b/selfdrive/controls/lib/curvature_learner.py @@ -10,7 +10,7 @@ import os # The learned curvature offsets will save and load automatically # If you still need help, check out how I have it implemented in the devel_curvaturefactorlearner branch # by Zorrobyte -# version 2 +# version 4 class CurvatureLearner: def __init__(self, debug=False): @@ -19,17 +19,15 @@ class CurvatureLearner: self.frame = 0 self.debug = debug try: - self.learned_offsets = pickle.load(open("/data/curvaturev2.p", "rb")) - except (OSError, IOError) as e: + self.learned_offsets = pickle.load(open("/data/curvaturev4.p", "rb")) + except (OSError, IOError): self.learned_offsets = { "center": 0., - "leftinner": 0., - "rightinner": 0., - "leftouter": 0., - "rightouter": 0. + "inner": 0., + "outer": 0. } - pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb")) - os.chmod("/data/curvaturev2.p", 0o777) + pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb")) + os.chmod("/data/curvaturev4.p", 0o777) def update(self, angle_steers=0., d_poly=None, v_ego=0.): if angle_steers > 0.1: @@ -37,27 +35,27 @@ class CurvatureLearner: self.learned_offsets["center"] -= d_poly[3] / self.learning_rate self.offset = self.learned_offsets["center"] elif 2. < abs(angle_steers) < 5.: - self.learned_offsets["leftinner"] -= d_poly[3] / self.learning_rate - self.offset = self.learned_offsets["leftinner"] + self.learned_offsets["inner"] -= d_poly[3] / self.learning_rate + self.offset = self.learned_offsets["inner"] elif abs(angle_steers) > 5.: - self.learned_offsets["leftouter"] -= d_poly[3] / self.learning_rate - self.offset = self.learned_offsets["leftouter"] + self.learned_offsets["outer"] -= d_poly[3] / self.learning_rate + self.offset = self.learned_offsets["outer"] elif angle_steers < -0.1: if abs(angle_steers) < 2.: self.learned_offsets["center"] += d_poly[3] / self.learning_rate self.offset = self.learned_offsets["center"] elif 2. < abs(angle_steers) < 5.: - self.learned_offsets["rightinner"] += d_poly[3] / self.learning_rate - self.offset = self.learned_offsets["rightinner"] + self.learned_offsets["inner"] += d_poly[3] / self.learning_rate + self.offset = self.learned_offsets["inner"] elif abs(angle_steers) > 5.: - self.learned_offsets["rightouter"] += d_poly[3] / self.learning_rate - self.offset = self.learned_offsets["rightouter"] + self.learned_offsets["outer"] += d_poly[3] / self.learning_rate + self.offset = self.learned_offsets["outer"] self.offset = clip(self.offset, -0.3, 0.3) self.frame += 1 if self.frame == 12000: # every 2 mins - pickle.dump(self.learned_offsets, open("/data/curvaturev2.p", "wb")) + pickle.dump(self.learned_offsets, open("/data/curvaturev4.p", "wb")) self.frame = 0 if self.debug: with open('/data/curvdebug.csv', 'a') as csv_file: diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 8bd715d5a..10c22a92e 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -30,7 +30,7 @@ class PathPlanner(object): self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 - #self.curvature_offset = CurvatureLearner(debug=False) + self.curvature_offset = CurvatureLearner(debug=False) def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -60,11 +60,11 @@ class PathPlanner(object): # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) - curvature_factor = VM.curvature_factor(v_ego) - #if active: - # curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego) - #else: - # curvfac = 0. + #curvature_factor = VM.curvature_factor(v_ego) + if active: + curvfac = self.curvature_offset.update(angle_steers - angle_offset, self.LP.d_poly, v_ego) + else: + curvfac = 0. #curvature_factor = VM.curvature_factor(v_ego) + curvfac