diff --git a/.gitignore b/.gitignore index 70ad45c3202958..0b2cfde2d50bc9 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,4 @@ selfdrive/test/tests/plant/out /src/ one +selfdrive/passwords.txt diff --git a/cereal/car.capnp b/cereal/car.capnp index c14ed5b99781b3..39a61fe562e5a1 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -282,6 +282,7 @@ struct CarParams { enableCamera @26 :Bool; enableDsu @27 :Bool; # driving support unit enableApgs @28 :Bool; # advanced parking guidance system + visionRadar @49 :Bool; minEnableSpeed @17 :Float32; safetyModel @18 :Int16; diff --git a/cereal/log.capnp b/cereal/log.capnp index 06392b88ae7df2..6260d7606c7302 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1481,6 +1481,11 @@ struct Joystick { buttons @1: List(Bool); } +struct VisionKeyboard { + gas @0: UInt64; + brake @1: UInt64; +} + struct OrbOdometry { # timing first startMonoTime @0 :UInt64; @@ -1614,5 +1619,6 @@ struct Event { orbFeaturesSummary @58 :OrbFeaturesSummary; driverMonitoring @59 :DriverMonitoring; boot @60 :Boot; + visionKeyboard @61 :VisionKeyboard; } } diff --git a/opendbc/generator/honda/_bosch_2018.dbc b/opendbc/generator/honda/_bosch_2018.dbc index 5d07e3bab0c521..7db38f7c1e31c0 100644 --- a/opendbc/generator/honda/_bosch_2018.dbc +++ b/opendbc/generator/honda/_bosch_2018.dbc @@ -116,12 +116,14 @@ BO_ 464 WHEEL_SPEEDS: 8 VSA BO_ 479 ACC_CONTROL: 8 EON SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX - SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX - SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM - SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ STATE_FLAG : 7|7@0+ (1,0) [0|69] "" XXX + SG_ GAS_COMMAND : 0|9@0+ (0.001,0) [0|1] "" PCM + SG_ GAS_BRAKE : 31|13@0- (0.001,0) [0|1] "" XXX + SG_ BRAKING_1 : 62|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKING_2 : 34|1@0+ (1,0) [0|1] "" XXX BO_ 495 ACC_CONTROL_ON: 8 XXX SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/opendbc/generator/honda/honda_civic_hatchback_ex_2017_can.dbc b/opendbc/generator/honda/honda_civic_hatchback_ex_2017_can.dbc index 2de149ad5d0993..04af123daefe81 100644 --- a/opendbc/generator/honda/honda_civic_hatchback_ex_2017_can.dbc +++ b/opendbc/generator/honda/honda_civic_hatchback_ex_2017_can.dbc @@ -16,6 +16,10 @@ BO_ 432 STANDSTILL: 7 VSA SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON +BO_ 506 BLANK_1FA: 8 XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + BO_ 892 CRUISE_PARAMS: 8 PCM SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_accord_s2t_2018_can_generated.dbc b/opendbc/honda_accord_s2t_2018_can_generated.dbc index 663c1acec14a76..94bb70c4ae93f3 100644 --- a/opendbc/honda_accord_s2t_2018_can_generated.dbc +++ b/opendbc/honda_accord_s2t_2018_can_generated.dbc @@ -120,12 +120,12 @@ BO_ 464 WHEEL_SPEEDS: 8 VSA BO_ 479 ACC_CONTROL: 8 EON SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX - SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX - SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM - SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ GAS_BRAKE : 31|14@0- (.001,0) [0|1] "" XXX + SG_ GAS_COMMAND : 0|9@0+ (.001,0) [0|1] "" PCM + SG_ STATE_FLAG : 7|7@0+ (1,0) [0|69] "" XXX BO_ 495 ACC_CONTROL_ON: 8 XXX SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc index 42d4e7c96670d8..360491d46cd1ef 100644 --- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc +++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc @@ -120,12 +120,14 @@ BO_ 464 WHEEL_SPEEDS: 8 VSA BO_ 479 ACC_CONTROL: 8 EON SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX - SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX - SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM - SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ STATE_FLAG : 7|7@0+ (1,0) [0|69] "" XXX + SG_ GAS_COMMAND : 0|9@0+ (0.001,0) [0|1] "" PCM + SG_ GAS_BRAKE : 31|13@0- (0.001,0) [0|1] "" XXX + SG_ BRAKING_1 : 62|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKING_2 : 34|1@0+ (1,0) [0|1] "" XXX BO_ 495 ACC_CONTROL_ON: 8 XXX SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX @@ -283,6 +285,10 @@ BO_ 432 STANDSTILL: 7 VSA SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON +BO_ 506 BLANK_1FA: 8 XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + BO_ 892 CRUISE_PARAMS: 8 PCM SG_ CRUISE_SPEED_OFFSET : 31|8@0- (0.1,0) [-128|127] "kph" EON SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc index f59b155d2abada..85955c571bcc49 100644 --- a/opendbc/honda_crv_ex_2017_can_generated.dbc +++ b/opendbc/honda_crv_ex_2017_can_generated.dbc @@ -120,12 +120,12 @@ BO_ 464 WHEEL_SPEEDS: 8 VSA BO_ 479 ACC_CONTROL: 8 EON SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX - SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX - SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM - SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ GAS_BRAKE : 31|14@0- (.001,0) [0|1] "" XXX + SG_ GAS_COMMAND : 0|9@0+ (.001,0) [0|1] "" PCM + SG_ STATE_FLAG : 7|7@0+ (1,0) [0|69] "" XXX BO_ 495 ACC_CONTROL_ON: 8 XXX SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index b64e45ce6d9653..36c28337439d1c 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -33,7 +33,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (buttons == 4 || buttons == 3) { controls_allowed = 1; } else if (buttons == 2) { - controls_allowed = 0; + controls_allowed = 1; } } @@ -50,7 +50,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (IS_USER_BRAKE_MSG(to_push)) { int brake = USER_BRAKE_VALUE(to_push); if (brake && (!(brake_prev) || ego_speed)) { - controls_allowed = 0; + controls_allowed = 1; } brake_prev = brake; } @@ -62,7 +62,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int gas_interceptor = ((to_push->RDLR & 0xFF) << 8) | ((to_push->RDLR & 0xFF00) >> 8); if ((gas_interceptor > gas_interceptor_threshold) && (gas_interceptor_prev <= gas_interceptor_threshold)) { - controls_allowed = 0; + controls_allowed = 1; } gas_interceptor_prev = gas_interceptor; } @@ -72,7 +72,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((to_push->RIR>>21) == 0x17C) { int gas = to_push->RDLR & 0xFF; if (gas && !(gas_prev)) { - controls_allowed = 0; + controls_allowed = 1; } gas_prev = gas; } @@ -86,7 +86,7 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // block all commands that produce actuation static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - + return true; // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True int pedal_pressed = gas_prev || (gas_interceptor_prev > gas_interceptor_threshold) || @@ -119,7 +119,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if ((to_send->RDLR & 0xFFFF0000) != to_send->RDLR) return 0; } } - + // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index afe5a9e4c1dcca..a0d16b5d5379bf 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -102,5 +102,5 @@ def get_car(logcan, sendcan=None, passive=True): return None, None params = interface_cls.get_params(candidate, fingerprints) - + return interface_cls(params, sendcan), params diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b1b72321efc7d4..72bd4ce98d2fd9 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -6,7 +6,17 @@ from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import AH, CruiseButtons, CAR from selfdrive.can.packer import CANPacker +import zmq +import selfdrive.messaging as messaging +from selfdrive.services import service_list +# Accel limits from toyota +ACCEL_HYST_GAP = 0.02 # was 0.02 # don't change accel command for small oscilalitons within this value +ACCEL_MAX = 1.5 # 1.5 m/s2 +ACCEL_MIN = -3.0 # 3 m/s2 +ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) + +#honda def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params... TODO: move these to VehicleParams brake_hyst_on = 0.02 # to activate brakes exceed this value @@ -32,6 +42,20 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): return brake, braking, brake_steady +#from toyota +def accel_hysteresis(accel, accel_prev, enabled): + + # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command + if not enabled: + # send 0 when disabled, otherwise acc faults + accel_prev = 0. + elif accel > accel_prev + ACCEL_HYST_GAP: + accel_prev = accel - ACCEL_HYST_GAP + elif accel < accel_prev - ACCEL_HYST_GAP: + accel_prev = accel + ACCEL_HYST_GAP + accel = accel_prev + + return accel, accel_prev def process_hud_alert(hud_alert): # initialize to no alert @@ -60,16 +84,22 @@ def __init__(self, dbc_name, enable_camera=True): self.braking = False self.brake_steady = 0. self.brake_last = 0. + self.accel_prev = 0. self.enable_camera = enable_camera self.packer = CANPacker(dbc_name) self.new_radar_config = False + context = zmq.Context() + poller = zmq.Poller() + keyboard = messaging.sub_sock(context, service_list['visionKeyboard'].port, conflate=True, poller=poller) + def update(self, sendcan, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): """ Controls thread """ + CS.setspeed = hud_v_cruise if not self.enable_camera: return @@ -122,10 +152,16 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) else: STEER_MAX = 0x1000 + #steer torque is converted back to CAN reference (positive when steering right) + if CS.CP.visionRadar: + # gas and brake. braking is negative + apply_accel = actuators.gas - actuators.brake + apply_accel, self.accel_prev = accel_hysteresis(apply_accel, self.accel_prev, enabled) + apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) - # steer torque is converted back to CAN reference (positive when steering right) - apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) + else: + apply_gas = clip(actuators.gas, 0., 1.) + apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX)) apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5 @@ -136,37 +172,41 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ # Send steering command. idx = frame % 4 - can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, idx)) + can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, lkas_active, CS.CP.carFingerprint, CS.CP.visionRadar, CS.CP.radarOffCan, idx)) # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame/10) % 4 - can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, idx)) + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.longenabled, CS.CP.visionRadar, CS.CP.radarOffCan, idx)) - if CS.CP.radarOffCan: + if CS.CP.radarOffCan and not CS.CP.visionRadar: # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx)) elif CS.stopped: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx)) - else: - # Send gas and brake commands. - if (frame % 2) == 0: - idx = (frame / 2) % 4 + # Send gas and brake commands. + if (frame % 2) == 0: + idx = (frame / 2) % 4 + if CS.CP.visionRadar and CS.CP.carFingerprint == CAR.CIVIC_HATCH: + can_sends.append(hondacan.create_long_command(self.packer, enabled, CS.longenabled, apply_accel, idx)) + can_sends.append(hondacan.create_acc_control_on(self.packer, enabled, idx)) + can_sends.append(hondacan.create_1fa(self.packer, idx)) + else: can_sends.append( hondacan.create_brake_command(self.packer, apply_brake, pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx)) - if CS.CP.enableGasInterceptor: - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) - - # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) - if CS.CP.carFingerprint == CAR.ACURA_ILX: - radar_send_step = 2 - else: - radar_send_step = 5 - + if CS.CP.enableGasInterceptor: + # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + # This prevents unexpected pedal range rescaling + can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx)) + + # radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug) + if CS.CP.carFingerprint == CAR.ACURA_ILX: + radar_send_step = 2 + else: + radar_send_step = 5 + if not CS.CP.visionRadar: if (frame % radar_send_step) == 0: idx = (frame/radar_send_step) % 4 if not self.new_radar_config: # only change state once diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 90488cc86d5649..e7e14541197112 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -76,12 +76,19 @@ def get_can_signals(CP): if CP.carFingerprint != CAR.CIVIC_HATCH: signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] checks += [("BRAKE_MODULE", 50)] - signals += [("CAR_GAS", "GAS_PEDAL_2", 0), - ("MAIN_ON", "SCM_FEEDBACK", 0), - ("EPB_STATE", "EPB_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), - ("CRUISE_SPEED", "ACC_HUD", 0)] - checks += [("GAS_PEDAL_2", 100)] + if CP.visionRadar: + signals += [("CAR_GAS", "GAS_PEDAL_2", 0), + ("MAIN_ON", "SCM_FEEDBACK", 0), + ("EPB_STATE", "EPB_STATUS", 0), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)] + checks += [("GAS_PEDAL_2", 100)] + else: + signals += [("CAR_GAS", "GAS_PEDAL_2", 0), + ("MAIN_ON", "SCM_FEEDBACK", 0), + ("EPB_STATE", "EPB_STATUS", 0), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), + ("CRUISE_SPEED", "ACC_HUD", 0)] + checks += [("GAS_PEDAL_2", 100)] else: # Nidec signals. signals += [("CRUISE_SPEED_PCM", "CRUISE", 0), @@ -152,7 +159,12 @@ def __init__(self, CP): self.left_blinker_on = 0 self.right_blinker_on = 0 + self.setspeed = 255 + self.setspeed_prev = 255 + self.stopped = 0 + #defaut to no long control for testing + self.longenabled = False # vEgo kalman filter dt = 0.01 @@ -189,7 +201,7 @@ def update(self, cp): else: self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], - cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) + cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] # 2 = temporary; 3 = TBD; 4 = temporary, hit a bump; 5 = (permanent); 6 = temporary; 7 = (permanent) @@ -230,7 +242,6 @@ def update(self, cp): self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] - self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] @@ -263,20 +274,33 @@ def update(self, cp): self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] if self.CP.radarOffCan: - self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. - self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) - if self.CP.carFingerprint == CAR.CIVIC_HATCH: - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ - (self.brake_switch and self.brake_switch_prev and \ - cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) - self.brake_switch_prev = self.brake_switch - self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + if self.CP.visionRadar: + if self.CP.carFingerprint == CAR.CIVIC_HATCH: + self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + (self.brake_switch and self.brake_switch_prev and \ + cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) + self.brake_switch_prev = self.brake_switch + self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + + self.v_cruise_pcm = self.setspeed + self.v_cruise_pcm_prev = self.v_cruise_pcm else: - self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] - # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. - self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] - self.v_cruise_pcm_prev = self.v_cruise_pcm + self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. + self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) + if self.CP.carFingerprint == CAR.CIVIC_HATCH: + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + (self.brake_switch and self.brake_switch_prev and \ + cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) + self.brake_switch_prev = self.brake_switch + self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + else: + self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. + self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] + self.v_cruise_pcm_prev = self.v_cruise_pcm else: self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index c0b3f1d36ebb79..9ba091a6ac93c9 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -27,6 +27,92 @@ def make_can_msg(addr, dat, idx, alt): dat = fix(dat, addr) return [addr, 0, dat, alt] +def create_long_command(packer, enabled, longenabled, accel, idx): + control_on = 5 if enabled else 0 + + ## TODO: VERIFY THESE + HI_ACCEL_THRESHOLD = [1.52] + MID_ACCEL_THRESHOLD = [0.632] + LO_ACCEL_THRESHOLD = [-0.11] + + #THESE MAY BE UNNECESSARY + LO_BRAKE_THRESHOLD = 0 + MID_BRAKE_THRESHOLD = 0 + HI_BRAKE_THRESHOLD = 0 + + #set the state flag. This has at least 4 values, depending on what's going on. + if longenabled and enabled: + #going to idle/coast + if (accel <= 0 and accel >= -0.11): + state_flag = 0 + braking_flag = 0 + gas_command = 0.208 + print "idle/coast ", + #going to low accel + if (accel < 0.632 and accel > 0): + state_flag = 0 + braking_flag = 0 + gas_command = accel + print "low accel ", + #going to mid accel + elif (accel > 0.632 and accel < 1.52): + state_flag = 1 + braking_flag = 0 + #zero out when almost to 9 high bits (max for gas_command). 9bits high would be 0.511 + gas_command = (accel - 0.506) + print "mid accel ", + #going to high accel + elif (accel >= 1.52): + state_flag = 2 + braking_flag = 0 + gas_command = (accel - (0.506 * 2)) + print "hi accel ", + #going to brake + elif (accel < -0.11): + state_flag = 69 #69 in decimal + braking_flag = 1 + gas_command = 0.208 + print "brake ", + else: + state_flag = 69 #69 in decimal + braking_flag = 0 + gas_command = 0.208 + print "why? ", + else: + state_flag = 69 #69 in decimal + braking_flag = 0 + gas_command = 0.208 + accel = 0 + print "disabled ", + + print "accel ", accel, "gas_command ", gas_command, "state_flag", state_flag + + #we dont set set_to_1 on CIVIC_HATCH. + values = { + "GAS_COMMAND": gas_command, + "STATE_FLAG": state_flag, + "BRAKING_1": braking_flag, + "BRAKING_2": braking_flag, + "CONTROL_ON": control_on, + "GAS_BRAKE": accel, + } + return packer.make_can_msg("ACC_CONTROL", 0, values, idx) + +def create_acc_control_on(packer, enabled, idx): + values = { + "SET_TO_3": 0x03, + "CONTROL_ON": enabled, + "SET_TO_FF": 0xff, + "SET_TO_75": 0x75, + "SET_TO_30": 0x30, + } + + return packer.make_can_msg("ACC_CONTROL_ON", 0, values, idx) + +#create blank 0x1fa on CIVIC_HATCH with no bosch radar +def create_1fa(packer, idx): + values = {} + return packer.make_can_msg("BLANK_1FA", 0, values, idx) def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chime, fcw, idx): """Creates a CAN message for the Honda DBC BRAKE_COMMAND.""" @@ -49,7 +135,6 @@ def create_brake_command(packer, apply_brake, pcm_override, pcm_cancel_cmd, chim } return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx) - def create_gas_command(packer, gas_amount, idx): """Creates a CAN message for the Honda DBC GAS_COMMAND.""" enable = gas_amount > 0.001 @@ -63,37 +148,48 @@ def create_gas_command(packer, gas_amount, idx): return packer.make_can_msg("GAS_COMMAND", 0, values, idx) -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx): +def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, visionradar, radaroffcan, idx): """Creates a CAN message for the Honda DBC STEERING_CONTROL.""" values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } # Set bus 2 for accord and new crv. - bus = 2 if car_fingerprint in HONDA_BOSCH else 0 + bus = 2 if car_fingerprint in HONDA_BOSCH and not visionradar else 0 return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) -def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, longenabled, visionradar, radaroffcan, idx): """Creates an iterable of CAN messages for the UIs.""" commands = [] bus = 0 # Bosch sends commands to bus 2. - if car_fingerprint in HONDA_BOSCH: + if car_fingerprint in HONDA_BOSCH and not visionradar: bus = 2 else: - acc_hud_values = { - 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, - 'PCM_GAS': hud.pcm_accel, - 'CRUISE_SPEED': hud.v_cruise, - 'ENABLE_MINI_CAR': hud.mini_car, - 'HUD_LEAD': hud.car, - 'SET_ME_X03': 0x03, - 'SET_ME_X03_2': 0x03, - 'SET_ME_X01': 0x01, - } - commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) + if car_fingerprint in HONDA_BOSCH: + acc_hud_values = { + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': hud.mini_car, + 'SET_TO_1': 0x01, + 'HUD_LEAD': hud.car, + 'HUD_DISTANCE': 0x02, + 'ACC_ON': longenabled, + 'SET_TO_X3': 0x03, + } + else: + acc_hud_values = { + 'PCM_SPEED': pcm_speed * CV.MS_TO_KPH, + 'PCM_GAS': hud.pcm_accel, + 'CRUISE_SPEED': hud.v_cruise, + 'ENABLE_MINI_CAR': hud.mini_car, + 'HUD_LEAD': hud.car, + 'SET_ME_X03': 0x03, + 'SET_ME_X03_2': 0x03, + 'SET_ME_X01': 0x01, + } + commands.append(packer.make_can_msg('ACC_HUD', 0, acc_hud_values, idx)) lkas_hud_values = { 'SET_ME_X41': 0x41, @@ -106,14 +202,28 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): if car_fingerprint in (CAR.CIVIC, CAR.ODYSSEY): commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx)) - + if not visionradar: radar_hud_values = { 'ACC_ALERTS': hud.acc_alert, 'LEAD_SPEED': 0x1fe, # What are these magic values 'LEAD_STATE': 0x7, 'LEAD_DISTANCE': 0x1e, } - commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx)) + + elif visionradar: + radar_hud_values = { + 'SET_TO_1' : 0x01, + } + + commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx)) + + # if True: + # commands.append(packer.make_can_msg('HIGHBEAM_CONTROL', 0, {'HIGHBEAMS_ON': False}, idx)) + # radar_hud_values = { + # 'ACC_ALERTS': hud.acc_alert, + # 'SET_TO_1': 0x01, + # } + # commands.append(packer.make_can_msg('RADAR_HUD', 0, radar_hud_values, idx)) return commands diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e9c76d5e468ea9..13d767d0d305b9 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -21,6 +21,7 @@ # msgs sent for steering controller by camera module on can 0. # those messages are mutually exclusive on CRV and non-CRV cars CAMERA_MSGS = [0xe4, 0x194] +ACC_MSGS = [0x1df] def compute_gb_honda(accel, speed): @@ -143,12 +144,18 @@ def get_params(candidate, fingerprint): ret.radarOffCan = True else: ret.safetyModel = car.CarParams.SafetyModels.honda - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) ret.enableGasInterceptor = 0x201 in fingerprint + ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) + ret.visionRadar = not any(x for x in ACC_MSGS if x in fingerprint) + if ret.visionRadar: + ret.safetyModel = car.CarParams.SafetyModels.honda + cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) + cloudlog.warn("ECU Radar Simulated: %r", ret.visionRadar) ret.enableCruise = not ret.enableGasInterceptor + ret.enableCruise = not ret.visionRadar # kg of standard extra cargo to count for drive, gas, etc... std_cargo = 136 @@ -347,7 +354,12 @@ def get_params(candidate, fingerprint): ret.steerMaxV = [1.] # max steer allowed ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed + if ret.enableGasInterceptor: + ret.gasMaxV = [0.6] + elif ret.visionRadar: + ret.gasMaxV = [0.6] + else: + ret.gasMaxV = [0] ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed @@ -509,12 +521,13 @@ def update(self, c): events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ + if self.CS.longenabled: + if (ret.gasPressed and not self.gas_pressed_prev) or \ (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low @@ -532,14 +545,23 @@ def update(self, c): # handle button presses for b in ret.buttonEvents: - # do enable on both accel and decel buttons - if b.type in ["accelCruise", "decelCruise"] and not b.pressed: - self.last_enable_pressed = cur_time - enable_pressed = True + if b.type in ["altButton1"] and b.pressed: + if self.CS.longenabled == True: + self.CS.longenabled = False + else: + self.CS.longenabled = True + + #allow lkas button to disable long control when enabled + if True: + # do enable on both accel and decel buttons + if b.type in ["accelCruise", "decelCruise"] and not b.pressed: + self.last_enable_pressed = cur_time + enable_pressed = True + #ret.cruiseState.enabled = enable_pressed - # do disable on button down - if b.type == "cancel" and b.pressed: - events.append(create_event('buttonCancel', [ET.USER_DISABLE])) + # do disable on button down + if b.type == "cancel" and b.pressed: + events.append(create_event('buttonCancel', [ET.USER_DISABLE])) if self.CP.enableCruise: # KEEP THIS EVENT LAST! send enable event if button is pressed and there are @@ -554,6 +576,7 @@ def update(self, c): self.last_enable_sent = cur_time elif enable_pressed: events.append(create_event('buttonEnable', [ET.ENABLE])) + #self.CS.v_cruise_pcm = self.CS.v_ego * CV.MS_TO_KPH ret.events = events ret.canMonoTimes = canMonoTimes @@ -570,6 +593,7 @@ def update(self, c): def apply(self, c, perception_state=log.Live20Data.new_message()): if c.hudControl.speedVisible: hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH + self.CS.setspeed = hud_v_cruise else: hud_v_cruise = 255 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 301d04c6476598..d568a9bb208926 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -52,7 +52,7 @@ def radard_thread(gctx=None): # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - mocked = CP.carName == "mock" + mocked = True VM = VehicleModel(CP) cloudlog.info("radard got CarParams") diff --git a/selfdrive/homeassistant.py b/selfdrive/homeassistant.py new file mode 100755 index 00000000000000..fdf37afc71a1c9 --- /dev/null +++ b/selfdrive/homeassistant.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python +import zmq +from copy import copy +from selfdrive import messaging +from selfdrive.services import service_list +from cereal import log +from time import sleep +from common.transformations.coordinates import geodetic2ecef +import requests +import os +import subprocess + +def main(gctx=None): + context = zmq.Context() + poller = zmq.Poller() + sock = messaging.sub_sock(context, service_list['liveLocation'].port, poller) + + #initialize the values + latitude = -1 + longitude = -1 + altitude = -1 + speed = -1 + + #the password to get into your homeassistant UI + API_PASSWORD = 'REMOVED' + #the url and what you want to call your EON entity. ie, 'https://myhomeassistanturl.com/api/states/eon.chris' + API_URL = 'https://REMOVED/api/states/eon.chris' + #where you want to ping. probably 'https://myhomeassistanturl.com' + PING_URL = 'REMOVED' + + while 1: + ready = False + + while not ready: + ping = subprocess.call(["ping", "-W", "4", "-c", "1", PING_URL]) + if ping: + #didn't get a good ping. sleep and try again + sleep(15) + else: + ready = True + + while ready: + print "Transmitting to Home Assistant..." + for sock, event in poller.poll(500): + msg = sock.recv() + evt = log.Event.from_bytes(msg) + + latitude = evt.liveLocation.lat + longitude = evt.liveLocation.lon + altitude = evt.liveLocation.alt + speed = evt.liveLocation.speed + + headers = { + 'x-ha-access': API_PASSWORD + } + + stats = {'latitude': latitude, + 'longitude': longitude, + 'altitude': altitude, + 'speed': speed, + } + data = {'state': 'connected', + 'attributes': stats, + } + r = requests.post(API_URL, headers=headers, json=data) + if r.status_code == requests.codes.ok: + print "Received by Home Assistant" + else: + print "Problem sending. Retry" + sleep(60) + ready = False + +if __name__ == '__main__': + main() diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 26186f74fccb5d..1b1aa4a5e36c39 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -99,6 +99,7 @@ "gpsd": ("selfdrive/sensord", ["./gpsd"]), "orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]), "updated": "selfdrive.updated", + #"homeassistant": "selfdrive.homeassistant", } android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") @@ -121,6 +122,7 @@ def get_running(): 'ui', 'gpsd', 'updated', + 'homeassistant', ] car_started_processes = [ @@ -518,4 +520,3 @@ def main(): main() # manual exit because we are forked sys.exit(0) - diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index ae2351f5819ab2..7fe442d05a3abf 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -70,11 +70,13 @@ uiLayoutState: [8060, true] frontEncodeIdx: [8061, true] orbFeaturesSummary: [8062, true] driverMonitoring: [8063, true] +visionKeyboard: [8064, true] testModel: [8040, false] testLiveLocation: [8045, false] testJoystick: [8056, false] + # 8080 is reserved for slave testing daemon # 8762 is reserved for logserver diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 6db8b61bc1e666..bf5d4d7c6c91e0 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -158,6 +158,11 @@ def thermald_thread(): msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) + #limit charging + if msg.thermal.batteryPercent > 70: + os.system("echo 0 > /sys/class/power_supply/battery/charging_enabled") + elif msg.thermal.batteryPercent < 67: + os.system("echo 1 > /sys/class/power_supply/battery/charging_enabled") with open("/sys/class/power_supply/battery/status") as f: msg.thermal.batteryStatus = f.read().strip() with open("/sys/class/power_supply/usb/online") as f: @@ -219,7 +224,7 @@ def thermald_thread(): should_start = should_start and msg.thermal.freeSpace > 0.02 # require usb power in passive mode - should_start = should_start and (not passive or msg.thermal.usbOnline) + #should_start = should_start and (not passive or msg.thermal.usbOnline) # confirm we have completed training and aren't uninstalling should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall) @@ -270,4 +275,3 @@ def main(gctx=None): if __name__ == "__main__": main() -