def update(self, active, CS, CP, lat_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on lat_plan.rateSteers steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) if v_ego < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg pid_log.angleError = angle_steers_des - CS.steeringAngleDeg if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) deadzone = 0.0 check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, angle_steers_des, pid_log
def update(self, active, CS, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steering_angle = CS.steeringAngle # if CS.v_ego > 8: # self.new_scale = interp(abs(steering_angle), self.angle_differ_range, self.scale_range) steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset steering_angle -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = steering_angle - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( CS.steeringTorqueEps / torque_scale) + self.L.dot(e) if CS.vEgo < 0.3 or not active: lqr_log.active = False lqr_output = 0. self.reset() else: lqr_log.active = True # LQR u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale #new_scale # # Integrator if CS.steeringPressed: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: error = self.angle_steers_des - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if (error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg desired_angle = math.degrees( VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg desired_angle += instant_offset # Only add offset that originates from vehicle model errors lqr_log.steeringAngleDesiredDeg = desired_angle # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = steering_angle_no_offset - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( CS.steeringTorqueEps / torque_scale) + self.L.dot(e) if CS.vEgo < MIN_STEER_SPEED or not active: lqr_log.active = False lqr_output = 0. output_steer = 0. self.reset() else: lqr_log.active = True # LQR u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale # Integrator if CS.steeringPressed: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: error = desired_angle - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if (error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i output_steer = lqr_output + self.i_lqr output_steer = clip(output_steer, -steers_max, steers_max) lqr_log.steeringAngleDeg = angle_steers_k lqr_log.i = self.i_lqr lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = self._check_saturation( steers_max - abs(output_steer) < 1e-3, CS) return output_steer, desired_angle, lqr_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): self.tune.check() # Added for utune lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, v_ego) torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed torque_scale = min(torque_scale, 0.69) # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset angle_steers -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = angle_steers - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( eps_torque / torque_scale) + self.L.dot(e) if v_ego < 0.3 or not active: lqr_log.active = False lqr_output = 0. self.reset() else: lqr_log.active = True # LQR u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale # Integrator if steer_override: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: error = self.angle_steers_des - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if ((error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0))): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) check_saturation = (v_ego > 10) and not rate_limited and not steer_override saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, path_plan): # Update Kalman filter y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() indi_log.steerAngle = math.degrees(self.x[0]) indi_log.steerRate = math.degrees(self.x[1]) indi_log.steerAccel = math.degrees(self.x[2]) if CS.vEgo < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.delayed_output = 0.0 self.angle_steers_des = path_plan.angleSteers else: self.angle_steers_des = path_plan.angleSteers self.rate_steers_des = path_plan.rateSteers steers_des = math.radians(self.angle_steers_des) rate_des = math.radians(self.rate_steers_des) # Expected actuator value self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) accel_error = accel_sp - self.x[2] # Compute change in actuator g_inv = 1. / self.G delta_u = g_inv * accel_error # Enforce rate limit if self.enforce_rate_limit: steer_max = float(SteerLimitParams.STEER_MAX) new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) prev_output_steer_cmd = steer_max * self.output_steer new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams) self.output_steer = new_output_steer_cmd / steer_max else: self.output_steer = self.delayed_output + delta_u steers_max = get_steer_max(CP, CS.vEgo) self.output_steer = clip(self.output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) indi_log.accelSetPoint = float(accel_sp) indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.delayed_output) indi_log.delta = float(delta_u) indi_log.output = float(self.output_steer) check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) return float(self.output_steer), float(self.angle_steers_des), indi_log
def update(self, active, CS, CP, path_plan): self.live_tune(CP) pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(CS.steeringAngle) pid_log.steerRate = float(CS.steeringRate) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: if CS.vEgo < 7.0: # ristrict to 25km self.angle_steers_des = self.Mid_Smoother.get_data( path_plan.angleSteers, 0.3) elif CS.vEgo < 15.0: # ristrict to 54km self.angle_steers_des = self.Mid_Smoother.get_data( path_plan.angleSteers, 0.45) elif CS.vEgo < 20.0: # ristrict to 72km self.angle_steers_des = self.Mid_Smoother.get_data( path_plan.angleSteers, 0.6) elif CS.vEgo < 25.0: # ristrict to 90km self.angle_steers_des = self.Mid_Smoother.get_data( path_plan.angleSteers, 0.8) elif CS.vEgo < 35.0: # ristrict to 125km self.angle_steers_des = self.Mid_Smoother.get_data( path_plan.angleSteers, 0.9) else: self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = self.deadzone check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) angle_steers_des_no_offset = math.degrees( VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = angle_steers_des - CS.steeringAngleDeg if CS.vEgo < MIN_STEER_SPEED or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max # offset does not contribute to resistive torque steer_feedforward = self.get_steer_feedforward( angle_steers_des_no_offset, CS.vEgo) # torque for steer rate. ~0 angle, steer rate ~= steer command. steer_rate_actual = CS.steeringRateDeg steer_rate_desired = math.degrees( VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)) speed_mph = CS.vEgo * CV.MS_TO_MPH steer_rate_max = 0.0389837 * speed_mph**2 - 5.34858 * speed_mph + 223.831 steer_feedforward += ((steer_rate_desired - steer_rate_actual) / steer_rate_max) deadzone = 0.0 check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = self._check_saturation( steers_max - abs(output_steer) < 1e-3, CS) return output_steer, angle_steers_des, pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): # Update Kalman filter y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.Live100Data.LateralINDIState.new_message() indi_log.steerAngle = math.degrees(self.x[0]) indi_log.steerRate = math.degrees(self.x[1]) indi_log.steerAccel = math.degrees(self.x[2]) if v_ego < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.delayed_output = 0.0 else: self.angle_steers_des = path_plan.angleSteers self.rate_steers_des = path_plan.rateSteers steers_des = math.radians(self.angle_steers_des) rate_des = math.radians(self.rate_steers_des) # Expected actuator value self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) accel_error = accel_sp - self.x[2] # Compute change in actuator g_inv = 1. / self.G delta_u = g_inv * accel_error # Enforce rate limit if self.enfore_rate_limit: steer_max = float(SteerLimitParams.STEER_MAX) new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) prev_output_steer_cmd = steer_max * self.output_steer new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams) self.output_steer = new_output_steer_cmd / steer_max else: self.output_steer = self.delayed_output + delta_u steers_max = get_steer_max(CP, v_ego) self.output_steer = clip(self.output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) indi_log.accelSetPoint = float(accel_sp) indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.delayed_output) indi_log.delta = float(delta_u) indi_log.output = float(self.output_steer) self.sat_flag = False return float(self.output_steer), float(self.angle_steers_des), indi_log
def update(self, active, CS, CP, path_plan): self.live_tune(CP) pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(CS.steeringAngle) pid_log.steerRate = float(CS.steeringRate) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner self.angle_steer_new = interp(CS.vEgo, self.angleBP, self.angle_steer_rate) check_pingpong = abs(self.angle_steers_des - self.angle_steers_des_last) > 4.0 if check_pingpong: self.angle_steers_des = path_plan.angleSteers * self.angle_steer_new steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle self.angle_steers_des_last = self.angle_steers_des if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque #steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) _c1, _c2, _c3 = [ 0.35189607550172824, 7.506201251644202, 69.226826411091 ] steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3 deadzone = self.deadzone check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, lat_plan): if self.params.get_bool("OpkrLiveTune"): self.live_tune(CP) pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) angle_steers_des_no_offset = math.degrees( VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max # TODO: feedforward something based on lat_plan.rateSteers steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque if self.new_kf_tuned: _c1, _c2, _c3 = 0.35189607550172824, 7.506201251644202, 69.226826411091 steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3 else: steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = float( int(self.params.get("IgnoreZone", encoding="utf8")) * 0.1) if self.params.get("IgnoreZone", encoding="utf8") is not None else 0.0 check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, 0, pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset angle_steers -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = angle_steers - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( eps_torque / torque_scale) + self.L.dot(e) if v_ego < 0.3 or not active: lqr_log.active = False self.reset() else: lqr_log.active = True # LQR u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) # Integrator if steer_override: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k) lqr_output = torque_scale * u_lqr / self.scale self.i_lqr = clip( self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1 self.output_steer = lqr_output + self.i_lqr # Clip output steers_max = get_steer_max(CP, v_ego) self.output_steer = clip(self.output_steer, -steers_max, steers_max) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(CS.steeringAngle) pid_log.steerRate = float(CS.steeringRate) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque _c1, _c2, _c3 = [ 0.34365576041121065, 12.845373070976711, 51.63304088261174 ] steer_feedforward *= _c1 * CS.vEgo**2 + _c2 * CS.vEgo + _c3 deadzone = 0.0 check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) if v_ego < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: # TODO: ideally we should interp, but for tuning reasons we keep the mpc solution # constant for 0.05s. #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, lat_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) angle_steers_des_no_offset = math.degrees( VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo)) angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() else: steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max # TODO: feedforward something based on lat_plan.rateSteers steer_feedforward = angle_steers_des_no_offset # offset does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, 0, pid_log
def update(self, active, CS, CP, VM, params, lat_plan): self.speed = CS.vEgo self.RC = interp(self.speed, self._RC[0], self._RC[1]) self.G = interp(self.speed, self._G[0], self._G[1]) self.outer_loop_gain = interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1]) self.inner_loop_gain = interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) if self.params.get_bool("OpkrLiveTune"): self.live_tune(CP) # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() indi_log.steeringAngleDeg = math.degrees(self.x[0]) indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) if CS.vEgo < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.delayed_output = 0.0 else: steers_des = VM.get_steer_from_curvature(-lat_plan.curvature, CS.vEgo) steers_des += math.radians(params.angleOffsetDeg) rate_des = VM.get_steer_from_curvature(-lat_plan.curvatureRate, CS.vEgo) # Expected actuator value alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) self.delayed_output = self.delayed_output * alpha + self.output_steer * (1. - alpha) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) accel_error = accel_sp - self.x[2] # Compute change in actuator g_inv = 1. / self.G delta_u = g_inv * accel_error # If steering pressed, only allow wind down if CS.steeringPressed and (delta_u * self.output_steer > 0): delta_u = 0 # Enforce rate limit if self.enforce_rate_limit: steer_max = float(CarControllerParams.STEER_MAX) new_output_steer_cmd = steer_max * (self.delayed_output + delta_u) prev_output_steer_cmd = steer_max * self.output_steer new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) self.output_steer = new_output_steer_cmd / steer_max else: self.output_steer = self.delayed_output + delta_u steers_max = get_steer_max(CP, CS.vEgo) self.output_steer = clip(self.output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) indi_log.accelSetPoint = float(accel_sp) indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.delayed_output) indi_log.delta = float(delta_u) indi_log.output = float(self.output_steer) check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) return float(self.output_steer), 0, indi_log
def update(self, active, CS, CP, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(CS.steeringAngle) pid_log.steerRate = float(CS.steeringRate) if CS.vEgo < 0.3 or not active: output_steer = 0.0 pid_log.active = False if len(CP.lateralTuning.pid.kpV) > 1 and not self.dualpids: self.dualPIDinit(CP) self.pid.reset() self.lowpid.reset() self.increasing = False else: self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner steers_max = get_steer_max(CP, CS.vEgo) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max self.lowpid.pos_limit = steers_max self.lowpid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed #output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, # feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) output_steer = 0.0 if not self.dualpids: output_steer = self.pid.update( self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) else: if not self.increasing: raw_low_output_steer = self.lowpid.update( self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) output_steer = raw_low_output_steer * self.factor #print("Lo - " + str(output_steer)) if abs(output_steer) > (self.factor * 0.99): self.pidset(self.pid, output_steer, self.angle_steers_des, CS.steeringAngle, steer_feedforward) #self.pid.p = self.lowpid.p * self.factor #self.pid.i = self.lowpid.i * self.factor #self.pid.f = self.lowpid.f * self.factor #self.pid.sat_count = 0.0 #self.pid.saturated = False #self.pid.control = self.lowpid.control * self.factor self.increasing = True else: output_steer = self.pid.update( self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) #print("Hi - " + str(output_steer)) if abs(output_steer) < (self.factor * 0.1) and abs( self.pid.i) < (self.factor * 0.2): self.pidset(self.lowpid, (output_steer / self.factor), self.angle_steers_des, CS.steeringAngle, steer_feedforward) #self.lowpid.p = self.pid.p / self.factor #self.lowpid.i = 0 #self.lowpid.f = self.pid.f / self.factor #self.lowpid.sat_count = 0.0 #self.lowpid.saturated = False #self.lowpid.control = self.pid.control / self.factor self.increasing = False pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() indi_log.steeringAngleDeg = math.degrees(self.x[0]) indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll) steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) if CS.vEgo < MIN_STEER_SPEED or not active: indi_log.active = False self.steer_filter.x = 0.0 output_steer = 0 else: # Expected actuator value self.steer_filter.update_alpha(self.RC) self.steer_filter.update(last_actuators.steer) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) accel_error = accel_sp - self.x[2] # Compute change in actuator g_inv = 1. / self.G delta_u = g_inv * accel_error # If steering pressed, only allow wind down if CS.steeringPressed and (delta_u * last_actuators.steer > 0): delta_u = 0 output_steer = self.steer_filter.x + delta_u steers_max = get_steer_max(CP, CS.vEgo) output_steer = clip(output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) indi_log.accelSetPoint = float(accel_sp) indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) indi_log.output = float(output_steer) indi_log.saturated = self._check_saturation( steers_max - abs(output_steer) < 1e-3, CS) return float(output_steer), float(steers_des), indi_log
def update(self, active, CS, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed #neokii torque_scale = min(torque_scale, 0.65) steering_angle = CS.steeringAngle steeringTQ = CS.steeringTorqueEps v_ego_kph = CS.vEgo * CV.MS_TO_KPH self.ki, self.scale = self.atom_tune(v_ego_kph, CS.steeringAngle, CP) # ### 설정값 최적화 분석을 위한 랜덤화 임시 코드 #now = datetime.datetime.now() # current date and time #micro_S = int(now.microsecond) #if micro_S < 10000 : #1초에 한번만 랜덤변환 # self.ki = random.uniform(0.015, 0.025) #self.ki - (self.ki*0.5), self.ki + (self.ki*0.5) ) # self.scale = random.uniform(1750, 1950) #int(self.scale) - int(self.scale*0.055), int(self.scale) + int(self.scale*0.055) ) ) # self.dc_gain = random.uniform(0.0028, 0.0032) #self.dc_gain - (self.dc_gain*0.1), self.dc_gain + (self.dc_gain*0.1) ) # steers_max = random.uniform(1.0, 1.2) # ########################### log_ki = self.ki log_scale = self.scale log_dc_gain = self.dc_gain # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset steering_angle -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = steering_angle - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot( CS.steeringTorqueEps / torque_scale) + self.L.dot(e) error = self.angle_steers_des - angle_steers_k u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) if CS.vEgo < 0.3 or not active: lqr_log.active = False lqr_output = 0. self.reset() else: lqr_log.active = True # LQR #u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale # Integrator if CS.steeringPressed: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: #error = self.angle_steers_des - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if (error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) check_saturation = ( CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) if not CS.steeringPressed: str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format( v_ego_kph, steering_angle, self.angle_steers_des, angle_steers_k, steeringTQ, torque_scale, log_scale, log_ki, log_dc_gain, self.output_steer) self.trLQR.add(str2) str5 = 'LQR_Set:dc_gain={:06.4f}/scale={:06.1f}/ki={:05.3f}/OutputSteer={:5.3f}/Angle={:5.1f}|{:5.1f}'.format( self.scale, self.dc_gain, self.ki, self.output_steer, steering_angle, angle_steers_k) trace1.printf2(str5) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, blinkers_on, CP, VM, path_plan, live_params): if angle_steers_rate == 0.0 and self.calculate_rate: if angle_steers != self.prev_angle_steers: self.steer_counter_prev = self.steer_counter self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0 self.steer_counter = 0.0 elif self.steer_counter >= self.steer_counter_prev: self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0) self.steer_counter += 1.0 angle_steers_rate = self.rough_steers_rate else: # If non-zero angle_rate is provided, stop calculating angle rate self.calculate_rate = False pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) max_bias_change = 0.0002 / (abs(self.angle_bias) + 0.0001) self.angle_bias = float(clip(live_params.angleOffset - live_params.angleOffsetAverage, self.angle_bias - max_bias_change, self.angle_bias + max_bias_change)) self.live_tune(CP) if v_ego < 0.3 or not active: output_steer = 0.0 self.lane_changing = 0.0 self.previous_integral = 0.0 self.damp_angle_steers= 0.0 self.damp_rate_steers_des = 0.0 self.damp_angle_steers_des = 0.0 pid_log.active = False self.pid.reset() else: self.angle_steers_des = path_plan.angleSteers if not self.driver_assist_hold: self.damp_angle_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcAngles) - self.damp_angle_steers_des) / max(1.0, self.damp_mpc * 100.) self.damp_rate_steers_des += (interp(sec_since_boot() + self.damp_mpc + self.react_mpc, path_plan.mpcTimes, path_plan.mpcRates) - self.damp_rate_steers_des) / max(1.0, self.damp_mpc * 100.) self.damp_angle_steers += (angle_steers - self.angle_bias + self.damp_time * angle_steers_rate - self.damp_angle_steers) / max(1.0, self.damp_time * 100.) else: self.damp_angle_steers = angle_steers self.damp_angle_steers_des = self.damp_angle_steers + self.driver_assist_offset if steer_override and abs(self.damp_angle_steers) > abs(self.damp_angle_steers_des) and self.pid.saturated: self.damp_angle_steers_des = self.damp_angle_steers steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset) self.angle_ff_ratio = interp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1]) rate_feedforward = (1.0 - self.angle_ff_ratio) * self.rate_ff_gain * self.damp_rate_steers_des steer_feedforward = float(v_ego)**2 * (rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain) if len(self.poly_scale) > 0: if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers): self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[1]) - self.cur_poly_scale) else: self.cur_poly_scale += 0.05 * (interp(abs(self.damp_rate_steers_des), self.poly_scale[0], self.poly_scale[2]) - self.cur_poly_scale) else: self.cur_poly_scale = 1.0 if len(self.steer_p_scale) > 0: if abs(self.damp_angle_steers_des) > abs(self.damp_angle_steers): p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[1]) else: p_scale = interp(abs(angle_feedforward), self.steer_p_scale[0], self.steer_p_scale[2]) else: p_scale = 1.0 if CP.carName == "honda" and steer_override and not self.prev_override and not self.driver_assist_hold and self.pid.saturated and abs(angle_steers) < abs(self.damp_angle_steers_des) and not blinkers_on: self.driver_assist_hold = True self.driver_assist_offset = self.damp_angle_steers_des - self.damp_angle_steers else: self.driver_assist_hold = steer_override and self.driver_assist_hold self.path_error = float(v_ego) * float(self.get_projected_path_error(v_ego, angle_feedforward, angle_steers, live_params, path_plan, VM)) * self.poly_factor * self.cur_poly_scale * self.angle_ff_gain if self.driver_assist_hold and not steer_override and abs(angle_steers) > abs(self.damp_angle_steers_des): #self.angle_bias = 0.0 driver_opposing_i = False elif (steer_override and self.pid.saturated) or self.driver_assist_hold or self.lane_changing > 0.0 or blinkers_on: #self.angle_bias = 0.0 self.path_error = 0.0 if self.gernbySteer and not steer_override and v_ego > 10.0: if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0): self.adjust_angle_gain() else: self.previous_integral = self.pid.i driver_opposing_i = steer_override and self.pid.i * self.pid.p > 0 and not self.pid.saturated and not self.driver_assist_hold deadzone = 0.0 output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i, add_error=float(self.path_error), feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale) driver_opposing_op = steer_override and (angle_steers - self.prev_angle_steers) * output_steer < 0 self.update_lane_state(angle_steers, driver_opposing_op, blinkers_on, path_plan) output_steer *= self.lane_change_adjustment pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.p2 = float(self.pid.p2) pid_log.output = float(output_steer) pid_log.saturated = bool(self.pid.saturated) pid_log.angleFFRatio = self.angle_ff_ratio pid_log.angleBias = self.angle_bias self.prev_angle_steers = angle_steers self.prev_override = steer_override self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): self.live_tune(CP) pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) v_ego_kph = v_ego * CV.MS_TO_KPH if v_ego < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 500) else: if v_ego_kph < 10: self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 200) elif v_ego_kph < 20: self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 100) elif v_ego_kph < 30: self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 50) elif v_ego_kph < 40: self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 10) else: self.angle_steers_des = self.movAvg.get_data( path_plan.angleSteers, 5) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: # TODO: feedforward something based on path_plan.rateSteers steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = self.deadzone check_saturation = (v_ego > 10) and not rate_limited and not steer_override output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_advance, angle_steers_rate, steer_override, blinkers_on, CP, VM, path_plan, live_mpc, logMonoTime): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) if logMonoTime['pathPlan'] != self.last_plan_time: if self.path_age > 0.23: self.old_plan_count += 1 self.last_plan_time = logMonoTime['pathPlan'] path_age = sec_since_boot() - logMonoTime['pathPlan'] * 1e-9 self.avg_plan_age += 0.01 * (path_age - self.avg_plan_age) self.plan_index = max( 0, min(24, int(100 * (self.react_mpc + path_age)))) #self.lane_error = self.poly_factor * path_plan.cPoly[0] + self.polyReact*(path_plan.cPoly[len(path_plan.cPoly)-1]-path_plan.cPoly[3]) #if path_plan.cPoly[0] > 0: # self.lane_error += self.poly_factor #elif path_plan.cPoly[0] < 0: # self.lane_error -= self.poly_factor self.projected_lane_error = self.polyReact * 0.001 * sum( self.poly_range * self.poly_range * path_plan.cPoly) #-path_plan.cPoly[3]) else: self.plan_index += 1 self.min_index = min(self.min_index, self.plan_index) self.max_index = max(self.max_index, self.plan_index) if self.frame % 300 == 0: #print(path_plan.mpcAngles) #print(path_plan.mpcRates) print( "old plans: %d avg plan age: %0.3f min index: %d max_index: %d angles: %d poly_react: %d" % (self.old_plan_count, self.avg_plan_age, self.min_index, self.max_index, len(path_plan.cPoly), self.polyReact)) self.min_index = 100 self.max_index = 0 self.frame += 1 self.live_tune(CP) if (v_ego < 0.3 or not active): # or self.plan_index > len(path_plan.mpcAngles)): output_steer = 0.0 self.previous_integral = 0.0 self.damp_angle_steers = 0.0 self.damp_rate_steers_des = 0.0 self.damp_angle_steers_des = 0.0 self.angle_bias = 0.0 pid_log.active = False self.pid.reset() else: try: self.angle_steers_des = path_plan.mpcAngles[self.plan_index // 7] #self.angle_steers_des += ((path_plan.mpcAngles[self.plan_index//7] - path_plan.mpcAngles[(self.plan_index//7)-1])/7) * self.plan_index % 7 #self.damp_angle_steers_des = self.angle_steers_des #print(" %0.1f %0.1f %0.1f %0.1f %d %d" % (self.damp_rate_steers_des, self.angle_steers_des, self.damp_angle_steers_des, path_plan.mpcAngles[self.plan_index//7], self.plan_index,self.plan_index//7 )) #self.angle_steers_des = path_plan.mpcAngles[self.plan_index] self.damp_angle_steers_des += ( self.angle_steers_des - self.damp_angle_steers_des) / max( 1.0, self.damp_mpc * 100.) #self.damp_rate_steers_des += ((path_plan.mpcAngles[self.plan_index//7] - path_plan.mpcAngles[(self.plan_index//7)-1]) - self.damp_rate_steers_des) / max(1.0, self.damp_mpc * 100.) except: pass self.damp_angle_steers = angle_steers # += (angle_steers + angle_steers_rate * self.damp_time - self.damp_angle_steers) / max(1.0, self.damp_time * 100.) self.damp_angle_rate = angle_steers_rate #+= (angle_steers_rate - self.damp_angle_rate) / max(1.0, self.damp_time * 100.) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset) self.angle_ff_ratio = float( gernterp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1])) rate_feedforward = ( 1.0 - self.angle_ff_ratio ) * self.rate_ff_gain * self.damp_rate_steers_des steer_feedforward = float(v_ego)**2 * ( rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain) #self.lane_compensation += self.poly_factor * path_plan.cPoly[0] * 1 if self.lane_compensation * path_plan.cPoly[0] > 1 else 10 self.path_error_comp += ( v_ego * self.projected_lane_error * self.angle_ff_gain - self.path_error_comp) / self.poly_smoothing #self.path_error_comp += ((self.projected_lane_error + self.lane_compensation) - self.path_error_comp) / self.poly_smoothing if self.gernbySteer and not steer_override and v_ego > 10.0: if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0): self.adjust_angle_gain() else: self.previous_integral = self.pid.i p_scale = 1.0 deadzone = 0.0 driver_opposing_i = steer_override #output_steer = self.pid.update(self.angle_steers_des + self.path_error_comp, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i, # feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale) print("%0.3f %0.3f %0.3f " % (self.path_error_comp, self.angle_steers_des, self.damp_angle_steers)) output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=driver_opposing_i, add_error=float( self.path_error_comp), feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone, p_scale=p_scale) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) pid_log.output = float(output_steer) pid_log.p2 = float( self.pid.p2 ) #float(self.path_error_comp) * float(self.pid._k_p[1][0]) pid_log.saturated = bool(self.pid.saturated) pid_log.angleFFRatio = self.angle_ff_ratio pid_log.angleBias = self.angle_bias #if self.frame % 100 == 0: # print(path_plan) self.prev_angle_steers = angle_steers self.prev_override = steer_override self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des), pid_log
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan): pid_log = log.Live100Data.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) if v_ego < 0.3 or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() self.previous_integral = 0.0 else: self.angle_steers_des = interp(sec_since_boot(), path_plan.mpcTimes, path_plan.mpcAngles) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des # feedforward desired angle if CP.steerControlType == car.CarParams.SteerControlType.torque: angle_feedforward = steer_feedforward - path_plan.angleOffset self.angle_ff_ratio = interp(abs(angle_feedforward), self.angle_ff_bp[0], self.angle_ff_bp[1]) angle_feedforward *= self.angle_ff_ratio * self.angle_ff_gain rate_feedforward = (1.0 - self.angle_ff_ratio) * self.rate_ff_gain * path_plan.rateSteers steer_feedforward = v_ego**2 * (rate_feedforward + angle_feedforward) if v_ego > 10.0: if abs(angle_steers) > (self.angle_ff_bp[0][1] / 2.0): self.adjust_angle_gain() else: self.previous_integral = self.pid.i deadzone = 0.0 output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) # Reset sat_flat always, set it only if needed self.sat_flag = False # If PID is saturated, set time which it was saturated if self.pid.saturated and self.sat_time < 0.5: self.sat_time = sec_since_boot() # To save cycles, nest in sat_time check if self.sat_time > 0.5: # If its been saturated for 0.7 seconds then set flag if (sec_since_boot() - self.sat_time) > 0.7: self.sat_flag = True # If it is no longer saturated, clear the sat flag and timer if not self.pid.saturated: self.sat_time = 0.0 if CP.steerControlType == car.CarParams.SteerControlType.torque: return output_steer, path_plan.angleSteers, pid_log else: return self.angle_steers_des, path_plan.angleSteers
def update(self, active, CS, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, CS.vEgo) torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed #neokii #torque_scale = min(torque_scale, 0.65) steering_angle = CS.steeringAngle steeringTQ = CS.steeringTorqueEps v_ego_kph = CS.vEgo * CV.MS_TO_KPH self.ki, self.scale = self.atom_tune( v_ego_kph, CS.steeringAngle, CP ) self.tune.check() # neokii 추가 # Subtract offset. Zero angle should correspond to zero torque self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset steering_angle -= path_plan.angleOffset # Update Kalman filter angle_steers_k = float(self.C.dot(self.x_hat)) e = steering_angle - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) error = self.angle_steers_des - angle_steers_k u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) if CS.vEgo < 0.3 or not active: lqr_log.active = False lqr_output = 0. self.reset() else: lqr_log.active = True # LQR #u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat)) lqr_output = torque_scale * u_lqr / self.scale # Integrator if CS.steeringPressed: self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) else: #error = self.angle_steers_des - angle_steers_k i = self.i_lqr + self.ki * self.i_rate * error control = lqr_output + i if (error >= 0 and (control <= steers_max or i < 0.0)) or \ (error <= 0 and (control >= -steers_max or i > 0.0)): self.i_lqr = i self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) if CS.steeringPressed: whoissteering = "Driver" else: whoissteering = "Openpilot" # str2 = '/{} /{} /{} /{} /{} /{} /{} /{} /{} /{} /{}'.format( # v_ego_kph, steering_angle, self.angle_steers_des, angle_steers_k, steeringTQ, torque_scale, self.dc_gain, self.scale, self.ki, self.output_steer, whoissteering) # self.trLQR.add( str2 ) # str5 = 'LQR_Set:dcgain={:06.4f}/scale={:06.1f}/ki={:05.3f}/tq={:4.1f}/u={:5.1f}/i={:5.3f}/O={:5.3f}'.format( # self.dc_gain, self.scale, self.ki, steeringTQ, u_lqr, self.i_lqr, self.output_steer ) # trace1.printf2( str5 ) lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log
def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) indi_log = log.ControlsState.LateralINDIState.new_message() indi_log.steeringAngleDeg = math.degrees(self.x[0]) indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll) steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) if CS.vEgo < 0.3 or not active: indi_log.active = False self.output_steer = 0.0 self.steer_filter.x = 0.0 else: # Expected actuator value self.steer_filter.update_alpha(self.RC) self.steer_filter.update(self.output_steer) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des accel_sp = self.inner_loop_gain * (rate_sp - self.x[1]) accel_error = accel_sp - self.x[2] # Compute change in actuator g_inv = 1. / self.G delta_u = g_inv * accel_error # If steering pressed, only allow wind down if CS.steeringPressed and (delta_u * self.output_steer > 0): delta_u = 0 # Enforce rate limit if self.enforce_rate_limit: steer_max = float(CarControllerParams.STEER_MAX) new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u) prev_output_steer_cmd = steer_max * self.output_steer new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) self.output_steer = new_output_steer_cmd / steer_max else: self.output_steer = self.steer_filter.x + delta_u steers_max = get_steer_max(CP, CS.vEgo) self.output_steer = clip(self.output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) indi_log.accelSetPoint = float(accel_sp) indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) indi_log.output = float(self.output_steer) check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) return float(self.output_steer), float(steers_des), indi_log