def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cp = get_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.enableCamera) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda
def __init__(self, CP, sendcan=None): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.can_invalid_count = 0 self.cp = get_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) # sending if read only is False if sendcan is not None: self.sendcan = sendcan self.CC = CarController(CP.enableCamera) if self.CS.accord: # self.accord_msg = [] raise NotImplementedError if not self.CS.civic: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda
def __init__(self, CP, CarController): self.CP = CP self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 self.gas_pressed_prev = False self.brake_pressed_prev = False self.cp = get_can_parser(CP) self.cp_cam = get_cam_can_parser(CP) # *** init the major players *** self.CS = CarState(CP) self.VM = VehicleModel(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() else: self.compute_gb = compute_gb_honda
def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model=True): # dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only) cp2 = get_can_parser(fake_car_params()) sgs = cp2._sgs msgs = cp2._msgs cks_msgs = cp2.msgs_ck # ******** get messages sent to the car ******** can_msgs = [] for a in messaging.drain_sock(Plant.sendcan): can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2])) self.cp.update_can(can_msgs) # ******** get live100 messages for plotting *** live_msgs = [] for a in messaging.drain_sock(Plant.live100): live_msgs.append(a.live100) if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] else: brake = 0.0 if self.cp.vl[0x200]['GAS_COMMAND'] > 0: gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0 else: gas = 0.0 if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']: steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00 else: steer_torque = 0.0 distance_lead = self.distance_lead_prev + v_lead * self.ts # ******** run the car ******** speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake) standstill = (speed == 0) distance = self.distance_prev + speed * self.ts speed = self.speed_prev + self.ts * acceleration if speed <= 0: speed = 0 acceleration = 0 # ******** lateral ******** self.angle_steer -= (steer_torque / 10.0) * self.ts # *** radar model *** if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) v_rel = v_lead - speed else: d_rel = 200. v_rel = 0. a_rel = 0 lateral_pos_rel = 0. # print at 5hz if (self.rk.frame % (self.rate / 5)) == 0: print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % ( distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel) # ******** publish the car ******** vls = [ self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, 0, self.gear_choice, speed != 0, 0, 0, 0, 0, self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, self.user_gas, cruise_buttons, self.esp_disabled, 0, self.user_brake, self.steer_error, self.speed_sensor(speed), self.brake_error, self.brake_error, self.gear_shifter, self.main_on, self.acc_status, self.pedal_gas, self.cruise_setting, # left_blinker, right_blinker, counter 0, 0, 0, # interceptor_gas 0, 0 ] # TODO: publish each message at proper frequency can_msgs = [] for msg in set(msgs): msg_struct = {} indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]] for i in indxs: msg_struct[sgs[i]] = vls[i] if msg in cks_msgs: msg_struct["COUNTER"] = self.rk.frame % 4 msg_data = acura.encode(msg, msg_struct) if msg in cks_msgs: msg_data = fix(msg_data, msg) can_msgs.append([msg, 0, msg_data, 0]) # add the radar message # TODO: use the DBC def to_3_byte(x): return struct.pack("!H", int(x)).encode("hex")[1:] def to_3s_byte(x): return struct.pack("!h", int(x)).encode("hex")[1:] radar_msg = to_3_byte(d_rel*16.0) + \ to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \ to_3s_byte(int(v_rel*32.0)) + \ "0f00000" can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1]) Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes()) # ******** publish a fake model going straight ******** if publish_model: md = messaging.new_message() md.init('model') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0] * 50 x.prob = 1.0 x.std = 1.0 # fake values? Plant.model.send(md.to_bytes()) # ******** update prevs ******** self.speed = speed self.distance = distance self.distance_lead = distance_lead self.speed_prev = speed self.distance_prev = distance self.distance_lead_prev = distance_lead self.rk.keep_time() return (distance, speed, acceleration, distance_lead, brake, gas, steer_torque, live_msgs)