def controlsd_thread(gctx, rate=100): # start the loop set_realtime_priority(2) context = zmq.Context() params = Params() # pub live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) livempc = messaging.pub_sock(context, service_list['liveMpc'].port) passive = params.get("Passive") != "0" if not passive: sendcan = messaging.pub_sock(context, service_list['sendcan'].port) else: sendcan = None # sub thermal = messaging.sub_sock(context, service_list['thermal'].port) health = messaging.sub_sock(context, service_list['health'].port) cal = messaging.sub_sock(context, service_list['liveCalibration'].port) logcan = messaging.sub_sock(context, service_list['can'].port) CC = car.CarControl.new_message() CI, CP = get_car(logcan, sendcan, 1.0 if passive else None) if CI is None: if passive: return else: raise Exception("unsupported car") if passive: CP.safetyModel = car.CarParams.SafetyModels.noOutput fcw_enabled = params.get("IsFcwEnabled") == "1" PL = Planner(CP, fcw_enabled) LoC = LongControl(CP, CI.compute_gb) VM = VehicleModel(CP) LaC = LatControl(VM) AM = AlertManager() if not passive: AM.add("startup", False) # write CarParams params.put("CarParams", CP.to_bytes()) state = State.DISABLED soft_disable_timer = 0 v_cruise_kph = 255 overtemp = False free_space = False cal_status = Calibration.UNCALIBRATED rear_view_toggle = False rear_view_allowed = params.get("IsRearViewMirror") == "1" # 0.0 - 1.0 awareness_status = 1. rk = Ratekeeper(rate, print_delay_threshold=2./1000) # learned angle offset angle_offset = 1.5 # Default model bias calibration_params = params.get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) angle_offset = calibration_params["angle_offset"] except (ValueError, KeyError): pass prof = Profiler() while 1: prof.reset() # avoid memory leak # sample data and compute car events CS, events, cal_status, overtemp, free_space = data_sample(CI, CC, thermal, health, cal, cal_status, overtemp, free_space) prof.checkpoint("Sample") # define plan plan, plan_ts = calc_plan(CS, events, PL, LoC, v_cruise_kph, awareness_status) prof.checkpoint("Plan") if not passive: # update control state state, soft_disable_timer, v_cruise_kph = state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) prof.checkpoint("State transition") # compute actuators actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle = state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset, rear_view_allowed, rear_view_toggle) prof.checkpoint("State Control") # publish data CC = data_send(plan, plan_ts, CS, CI, CP, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, live100, livempc, AM, rear_view_allowed, rear_view_toggle, awareness_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") # *** run loop at fixed rate *** if rk.keep_time(): prof.display()
class LatControlPID(object): def __init__(self, CP): self.kegman = kegman_conf(CP) self.kegtime_prev = 0 self.profiler = Profiler(False, 'LaC') self.frame = 0 self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf) self.angle_steers_des = 0. self.polyReact = 1. self.poly_damp = 0 self.poly_factor = CP.lateralTuning.pid.polyFactor self.poly_scale = CP.lateralTuning.pid.polyScale self.path_error_comp = 0.0 self.damp_angle_steers = 0. self.damp_angle_rate = 0. self.damp_steer = 0.1 self.react_steer = 0.01 self.react_mpc = 0.0 self.damp_mpc = 0.25 self.angle_ff_ratio = 0.0 self.angle_ff_gain = 1.0 self.rate_ff_gain = CP.lateralTuning.pid.rateFFGain self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]] self.lateral_offset = 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.limited_damp_angle_steers_des = 0.0 self.old_plan_count = 0 self.last_plan_time = 0 self.lane_change_adjustment = 1.0 self.angle_index = 0. self.avg_plan_age = 0. self.min_index = 0 self.max_index = 0 self.prev_angle_steers = 0. self.c_prob = 0. self.deadzone = 0. self.starting_angle = 0. self.projected_lane_error = 0. self.prev_projected_lane_error = 0. self.path_index = None #np.arange((30.))*100.0/15.0 self.angle_rate_des = 0.0 # degrees/sec, rate dynamically limited by accel_limit self.fast_angles = [[]] self.center_angles = [] self.live_tune(CP) self.react_index = 0.0 self.next_params_put = 36000 self.zero_poly_crossed = 0 self.zero_steer_crossed = 0 try: params = Params() lateral_params = params.get("LateralGain") lateral_params = json.loads(lateral_params) self.angle_ff_gain = max(1.0, float(lateral_params['angle_ff_gain'])) except: self.angle_ff_gain = 1.0 def live_tune(self, CP): if self.frame % 300 == 0: (mode, ino, dev, nlink, uid, gid, size, atime, mtime, self.kegtime) = os.stat(os.path.expanduser('~/kegman.json')) if self.kegtime != self.kegtime_prev: try: time.sleep(0.0001) self.kegman = kegman_conf() except: print(" Kegman error") self.pid._k_i = ([0.], [float(self.kegman.conf['Ki'])]) self.pid._k_p = ([0.], [float(self.kegman.conf['Kp'])]) self.pid.k_f = (float(self.kegman.conf['Kf'])) self.damp_steer = (float(self.kegman.conf['dampSteer'])) self.react_steer = (float(self.kegman.conf['reactSteer'])) self.react_mpc = (float(self.kegman.conf['reactMPC'])) self.damp_mpc = (float(self.kegman.conf['dampMPC'])) self.deadzone = float(self.kegman.conf['deadzone']) self.polyReact = min(11, max(0, int(10 * float(self.kegman.conf['polyReact'])))) self.poly_damp = min(1, max(0, float(self.kegman.conf['polyDamp']))) self.poly_factor = max(0.0, float(self.kegman.conf['polyFactor']) * 0.001) self.require_blinker = bool(int(self.kegman.conf['requireBlinker'])) self.require_nudge = bool(int(self.kegman.conf['requireNudge'])) self.react_center = [max(0, float(self.kegman.conf['reactCenter0'])),max(0, float(self.kegman.conf['reactCenter1'])),max(0, float(self.kegman.conf['reactCenter2'])), 0] self.kegtime_prev = self.kegtime def update_lane_state(self, angle_steers, driver_opposing_lane, blinker_on, path_plan): if self.require_nudge: if self.lane_changing > 0.0: # and path_plan.cProb > 0: self.lane_changing += 0.01 # max(self.lane_changing + 0.01, 0.005 * abs(path_plan.lPoly[5] + path_plan.rPoly[5])) if self.lane_changing > 2.75 or (not blinker_on and self.lane_changing < 1.0 and abs(path_plan.cPoly[5]) < 100 and min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) < 1.5 and path_plan.cPoly[14] * path_plan.cPoly[0] > 0): self.lane_changing = 0.0 self.stage = "4" elif 2.25 <= self.lane_changing < 2.5 and path_plan.cPoly[14] * path_plan.cPoly[4] > 0: # abs(path_plan.lPoly[5] + path_plan.rPoly[5]) < abs(path_plan.cPoly[5]): self.lane_changing = 2.5 self.stage = "3" elif 2.0 <= self.lane_changing < 2.25 and path_plan.cPoly[14] * path_plan.cPoly[9] > 0: # (path_plan.lPoly[5] + path_plan.rPoly[5]) * path_plan.cPoly[0] < 0: self.lane_changing = 2.25 self.stage = "2" elif self.lane_changing < 2.0 and path_plan.cPoly[14] * path_plan.cPoly[0] < 0: #path_plan.laneWidth < 1.2 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]): self.lane_changing = 2.0 self.stage = "1" elif self.lane_changing < 1.0 and abs(path_plan.cPoly[14]) > abs(path_plan.cPoly[7]): #path_plan.laneWidth < 1.2 * abs(path_plan.lPoly[5] + path_plan.rPoly[5]): self.lane_changing = 0.98 self.stage = "0" #else: #self.lane_changing = max(self.lane_changing + 0.01, 0.005 * abs(path_plan.lPoly[5] + path_plan.rPoly[5])) #if blinker_on: # self.lane_change_adjustment = 0.0 #else: self.lane_change_adjustment = interp(self.lane_changing, [0.0, 1.0, 2.0, 2.25, 2.5, 2.75], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) #print("%0.2f lane_changing %0.2f adjustment %0.2f p_poly %0.2f avg_poly stage = %s blinker %d opposing %d width_1 %0.2f width_2 %0.2f center_1 %0.2f center_2 %0.2f" % (self.lane_changing, self.lane_change_adjustment, path_plan.cPoly[5], path_plan.lPoly[5] + path_plan.rPoly[5], self.stage, blinker_on, driver_opposing_lane, path_plan.laneWidth, 0.6 * abs(path_plan.lPoly[5] - path_plan.rPoly[5]), path_plan.cPoly[0], path_plan.lPoly[5] + path_plan.rPoly[5])) elif (blinker_on or not self.require_blinker) and driver_opposing_lane and path_plan.rProb > 0 and path_plan.lProb > 0 and (abs(path_plan.cPoly[14]) > 100 or min(abs(self.starting_angle - angle_steers), abs(self.angle_steers_des - angle_steers)) > 1.0): # and path_plan.cPoly[14] * path_plan.cPoly[0] > 0: print('starting lane change @ %0.2f' % self.lane_changing) self.lane_changing = 0.01 self.lane_change_adjustment = 1.0 else: if self.lane_changing != 0: print('terminating lane change @ %0.2f' % self.lane_changing) self.lane_changing = 0 self.stage = "0" self.starting_angle = angle_steers self.lane_change_adjustment = 1.0 elif blinker_on: self.lane_change_adjustment = 0.0 else: self.lane_change_adjustment = 1.0 def reset(self): self.pid.reset() def adjust_angle_gain(self): if (self.pid.f > 0) == (self.pid.i > 0) and abs(self.pid.i) >= abs(self.previous_integral): if not abs(self.pid.f + self.pid.i) > 1: self.angle_ff_gain *= 1.0001 elif self.angle_ff_gain > 1.0: self.angle_ff_gain *= 0.9999 self.previous_integral = self.pid.i def update(self, active, brake_pressed, v_ego, angle_steers, angle_steers_rate, steer_override, CP, path_plan, canTime, blinker_on): self.profiler.checkpoint('controlsd') pid_log = car.CarState.LateralPIDState.new_message() path_age = (time.time() * 1000 - path_plan.sysTime) * 1e-3 if (angle_steers - path_plan.angleOffset >= 0) == (self.prev_angle_steers < 0): self.zero_steer_crossed = time.time() self.prev_angle_steers = angle_steers - path_plan.angleOffset if path_plan.canTime != self.last_plan_time and len(path_plan.fastAngles) > 1: time.sleep(0.00001) if path_age > 0.23: self.old_plan_count += 1 if self.path_index is None: self.avg_plan_age = path_age self.path_index = np.arange((len(path_plan.fastAngles)))*100.0/15.0 self.last_plan_time = path_plan.canTime self.avg_plan_age += 0.01 * (path_age - self.avg_plan_age) self.c_prob = path_plan.cProb self.projected_lane_error = float(min(0.5, max(-0.5, self.c_prob * self.poly_factor * sum(np.array(path_plan.cPoly))))) self.center_angles.append(float(self.projected_lane_error)) if len(self.center_angles) > 15: self.center_angles.pop(0) if (self.projected_lane_error >= 0) == (self.prev_projected_lane_error < 0): self.zero_poly_crossed = time.time() self.prev_projected_lane_error = self.projected_lane_error if time.time() - min(self.zero_poly_crossed, self.zero_steer_crossed) < 4: self.projected_lane_error -= (float(self.c_prob * self.poly_damp * self.center_angles[0])) self.fast_angles = np.array(path_plan.fastAngles) self.profiler.checkpoint('path_plan') if path_plan.paramsValid: self.angle_index = max(0., 100. * (self.react_mpc + path_age)) self.min_index = min(self.min_index, self.angle_index) self.max_index = max(self.max_index, self.angle_index) if self.frame % 300 == 0 and self.frame > 0: print("old plans: %d avg plan age: %0.3f min index: %d max_index: %d center_steer: %0.2f" % (self.old_plan_count, self.avg_plan_age, self.min_index, self.max_index, self.path_error_comp)) self.min_index = 100 self.max_index = 0 self.profiler.checkpoint('update') self.frame += 1 self.live_tune(CP) self.profiler.checkpoint('live_tune') if v_ego < 0.3 or not path_plan.paramsValid: if self.frame > self.next_params_put and v_ego == 0 and brake_pressed: self.next_params_put = self.frame + 36000 put_nonblocking("LateralGain", json.dumps({'angle_ff_gain': self.angle_ff_gain})) self.profiler.checkpoint('params_put') output_steer = 0.0 self.stage = "0" self.lane_changing = 0.0 self.previous_integral = 0.0 self.previous_lane_error = 0.0 self.path_error_comp = 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() self.profiler.checkpoint('pid_reset') else: try: pid_log.active = True if False and blinker_on and steer_override: self.path_error_comp *= 0.9 self.damp_angle_steers = angle_steers self.angle_steers_des = angle_steers self.damp_angle_steers_des = angle_steers self.limited_damp_angle_steers_des = angle_steers self.angle_rate_des = 0 else: react_steer = self.react_steer + self.react_center[min(len(self.react_center)-1, int(abs(angle_steers - path_plan.angleOffset)))] self.damp_angle_steers += (angle_steers + angle_steers_rate * (self.damp_steer + float(react_steer)) - self.damp_angle_steers) / max(1.0, self.damp_steer * 100.) self.angle_steers_des = interp(self.angle_index, self.path_index, self.fast_angles[min(len(self.fast_angles)-1, int(self.polyReact))]) self.damp_angle_steers_des += (self.angle_steers_des - self.damp_angle_steers_des + self.projected_lane_error) / max(1.0, self.damp_mpc * 100.) if (self.damp_angle_steers - self.damp_angle_steers_des) * (angle_steers - self.damp_angle_steers_des) < 0: self.damp_angle_steers = self.damp_angle_steers_des angle_feedforward = float(self.damp_angle_steers_des - path_plan.angleOffset) + float(self.path_error_comp) 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.angle_rate_des steer_feedforward = float(v_ego)**2 * (rate_feedforward + angle_feedforward * self.angle_ff_ratio * self.angle_ff_gain) if 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 if path_plan.cProb == 0 or (angle_feedforward > 0) == (self.pid.p > 0) or (path_plan.cPoly[-1] > 0) == (self.pid.p > 0): p_scale = 1.0 else: p_scale = max(0.2, min(1.0, 1 / abs(angle_feedforward))) self.profiler.checkpoint('pre-pid') output_steer = self.pid.update(self.damp_angle_steers_des, self.damp_angle_steers, check_saturation=(v_ego > 10), override=steer_override, p_scale=p_scale, add_error=0, feedforward=steer_feedforward, speed=v_ego, deadzone=self.deadzone if abs(angle_feedforward) < 1 else 0.0) self.profiler.checkpoint('pid_update') except: output_steer = 0 print(" angle error!") pass #driver_opposing_op = steer_override and (angle_steers - self.prev_angle_steers) * output_steer < 0 #self.update_lane_state(angle_steers, driver_opposing_op, blinker_on, path_plan) #self.profiler.checkpoint('lane_change') output_factor = self.lane_change_adjustment if pid_log.active else 0 if self.lane_change_adjustment < 1 and self.lane_changing > 0: self.damp_angle_steers_des = self.angle_steers_des self.limit_damp_angle_steers_des = self.angle_steers_des self.prev_override = steer_override self.pid.f *= output_factor self.pid.i *= output_factor self.pid.p *= output_factor output_steer *= output_factor 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.projected_lane_error) pid_log.saturated = bool(self.pid.saturated) pid_log.angleFFRatio = self.angle_ff_ratio pid_log.steerAngle = float(self.damp_angle_steers) pid_log.steerAngleDes = float(self.damp_angle_steers_des) self.sat_flag = self.pid.saturated self.profiler.checkpoint('post_update') if self.frame % 5000 == 1000 and self.profiler.enabled: self.profiler.display() self.profiler.reset(True) return output_steer, float(self.angle_steers_des), pid_log
#os.remove(os.path.expanduser('~/calibration.json')) profiler.checkpoint('save_cal') # TODO: replace kegman_conf with params! if frame % 100 == 0: (mode, ino, dev, nlink, uid, gid, size, atime, mtime, kegtime) = os.stat(os.path.expanduser('~/kegman.json')) if kegtime != kegtime_prev: kegtime_prev = kegtime kegman = kegman_conf() advanceSteer = 1.0 + max(0, float(kegman.conf['advanceSteer'])) angle_factor = float(kegman.conf['angleFactor']) angle_speed = min(5, max(0, int(10 * float(kegman.conf['polyReact'])))) use_discrete_angle = True if float(kegman.conf['discreteAngle']) > 0 else False angle_limit = abs(float(kegman.conf['discreteAngle'])) use_minimize = True if kegman.conf['useMinimize'] == '1' else False first_model = max(0, min(len(models)-1, int(float(kegman.conf['firstModel'])))) last_model = max(first_model, min(len(models)-1, int(float(kegman.conf['lastModel'])))) model_factor = abs(float(kegman.conf['modelFactor'])) profiler.checkpoint('kegman') execution_time_avg += max(0.0001, time_factor) * ((time.time() - start_time) - execution_time_avg) time_factor *= 0.96 if frame % 1000 == 0 and profiler.enabled: profiler.display() profiler.reset(True) #except: # pass
def controlsd_thread(gctx=None): setproctitle('controlsd') params = Params() print(params) # Pub Sockets profiler = Profiler(True, 'controls') sendcan = messaging.pub_sock(service_list['sendcan'].port) controlsstate = messaging.pub_sock(service_list['controlsState'].port) carstate = None #messaging.pub_sock(service_list['carState'].port) carcontrol = messaging.pub_sock(service_list['carControl'].port) carevents = messaging.pub_sock(service_list['carEvents'].port) carparams = messaging.pub_sock(service_list['carParams'].port) sm = messaging.SubMaster(['pathPlan', 'health', 'gpsLocationExternal']) can_sock = messaging.sub_sock(service_list['can'].port) hw_type = messaging.recv_one(sm.sock['health']).health.hwType is_panda_black = hw_type == log.HealthData.HwType.blackPanda print("panda black: ", is_panda_black) wait_for_can(can_sock) CI, CP = get_car(can_sock, sendcan, is_panda_black) #logcan.close() # TODO: Use the logcan socket from above, but that will currenly break the tests #can_timeout = None #if os.environ.get('NO_CAN_TIMEOUT', False) else 100 #can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) # Write CarParams for radard and boardd safety mode params.put("CarParams", CP.to_bytes()) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() AM = AlertManager() startup_alert = get_startup_alert(True, True) AM.add(sm.frame, startup_alert, False) LaC = LatControlPID(CP) lateral = Lateral(CP) lkasMode = int(float(LaC.kegman.conf['lkasMode'])) #CI.CS.lkasMode = (lkasMode == 0) lac_log = None #car.CarState.lateralControlState.pidState.new_message() state = State.disabled soft_disable_timer = 0 v_cruise_kph = 255 events_prev = [] frame = 0 sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True while True: start_time = 0 # time.time() #sec_since_boot() # Sample data and compute car events CS, events = data_sample(CI, CC, can_sock, carstate, lac_log, lateral, sm, profiler) profiler.checkpoint('data_sample') state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \ state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM) profiler.checkpoint('state_transition') # Compute actuators (runs PID loops and lateral MPC) sm.update(0) profiler.checkpoint('sm_update') actuators, lac_log = state_control(sm.frame, lkasMode, sm['pathPlan'], CS, CP, state, events, AM, LaC, lac_log, profiler) profiler.checkpoint('state_control') # Publish data CC, events_prev = data_send(sm, CS, CI, CP, state, events, actuators, carstate, carcontrol, carevents, carparams, controlsstate, sendcan, AM, LaC, start_time, lac_log, events_prev, profiler) profiler.checkpoint('data_send') frame += 1 if frame % 10000 == 0 and profiler.enabled: profiler.display() profiler.reset(True)