diff --git a/cereal/log.capnp b/cereal/log.capnp index a37a58923..522a9da5c 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -296,7 +296,7 @@ struct ThermalData { memUsedPercent @19 :Int8; cpuPerc @20 :Int8; - ipAddr @24 :Text; # dragonpilot + ipAddr @25 :Text; # dragonpilot enum ThermalStatus { green @0; # all processes run diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index aa097f491..75ecae214 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -194,7 +194,7 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // and the the latching controls_allowed flag is True //int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || // (brake_pressed_prev && honda_moving); - int pedal_pressed = brake_pressed_prev && honda_moving;; + int pedal_pressed = brake_pressed_prev && honda_moving; bool current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 4fe1fe9fd..5aedb3764 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -112,7 +112,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int byte = (addr == 0x224) ? 0 : 4; bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0; if (brake_pressed && (!brake_pressed_prev || toyota_moving)) { - controls_allowed = 0; + controls_allowed = 1; } brake_pressed_prev = brake_pressed; } diff --git a/reset_update.sh b/reset_update.sh index 6bfe86d88..18d77f9d3 100755 --- a/reset_update.sh +++ b/reset_update.sh @@ -3,4 +3,4 @@ export LD_LIBRARY_PATH=/data/data/com.termux/files/usr/lib export HOME=/data/data/com.termux/files/home export PATH=/usr/local/bin:/data/data/com.termux/files/usr/bin:/data/data/com.termux/files/usr/sbin:/data/data/com.termux/files/usr/bin/applets:/bin:/sbin:/vendor/bin:/system/sbin:/system/bin:/system/xbin:/data/data/com.termux/files/usr/bin/git -cd /data/openpilot && git reset --hard @{u} && git clean -xdf && git pull && reboot \ No newline at end of file +cd /data/openpilot && git reset --hard @{u} && git clean -xdf && git pull && scons --clean && reboot \ No newline at end of file diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png index 9f52faf9e..07f47ab5d 100644 Binary files a/selfdrive/assets/images/button_home.png and b/selfdrive/assets/images/button_home.png differ diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index f0815dd98..8ef75a4ce 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -181,16 +181,15 @@ def fingerprint(logcan, sendcan, has_relay): frame += 1 + source = car.CarParams.FingerprintSource.can + if dragon_has_cache: car_fingerprint = dragon_car_fingerprint finger = dragon_finger vin = dragon_vin car_fw = dragon_car_fw source = dragon_source - else: - source = car.CarParams.FingerprintSource.can - # If FW query returns exactly 1 candidate, use it if len(fw_candidates) == 1: car_fingerprint = list(fw_candidates)[0] diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 6db935c6a..956f73cdd 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -104,6 +104,7 @@ class CarController(): self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -112,8 +113,15 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified P = self.params @@ -163,7 +171,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py old mode 100644 new mode 100755 index 5d7851219..ef7da044e --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -404,6 +404,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): + self.dp_load_params('honda') # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 519ea9ace..285e835ae 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -21,6 +21,7 @@ class CarController(): self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert): @@ -28,8 +29,15 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified ### Steering Torque @@ -49,7 +57,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a25df91f9..ac78d1207 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -6,6 +6,10 @@ from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel +from common.realtime import sec_since_boot +from common.params import Params +params = Params() +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified GearShifter = car.CarState.GearShifter @@ -30,6 +34,14 @@ class CarInterfaceBase(): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + # dragonpilot + self.dragon_toyota_stock_dsu = False + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False + self.ts_last_check = 0. + self.dragon_lat_ctrl = True + self.dp_last_modified = None + @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1. @@ -81,6 +93,23 @@ class CarInterfaceBase(): def apply(self, c): raise NotImplementedError + def dp_load_params(self, car_name): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check >= 5.: + modified = dp_get_last_modified() + if self.dp_last_modified != modified: + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = True + self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + if car_name == 'toyota': + self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False + self.dp_last_modified = modified + self.ts_last_check = ts + def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): events = [] @@ -100,7 +129,11 @@ class CarInterfaceBase(): events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) # TODO: move this stuff to the capnp strut - if getattr(self.CS, "steer_error", False): + if not self.dragon_lat_ctrl: + events.append(create_event('manualSteeringRequired', [ET.WARNING])) + elif self.dragon_enable_steering_on_signal and (cs_out.leftBlinker or cs_out.rightBlinker): + events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) + elif getattr(self.CS, "steer_error", False): events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif getattr(self.CS, "steer_warning", False): events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) @@ -108,9 +141,15 @@ class CarInterfaceBase(): # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! - if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ - (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if not self.dragon_toyota_stock_dsu: + # DragonAllowGas + if not self.dragon_allow_gas: + if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + else: + if cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) return events diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index bbafcae41..23cca1df5 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -37,6 +37,7 @@ class CarController(): self.dragon_enable_steering_on_signal = False self.dragon_lat_ctrl = True self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ Controls thread """ @@ -45,8 +46,15 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified P = self.params @@ -75,7 +83,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 0f2f4176c..0c71cb13e 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -49,6 +49,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): + self.dp_load_params('subaru') self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 6062457ee..29fbcb339 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -71,6 +71,7 @@ class CarController(): self.dragon_lane_departure_warning = True self.dragon_toyota_sng_mod = False self.dp_last_modified = None + self.lane_change_enabled = True def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): @@ -79,10 +80,17 @@ class CarController(): if frame % 500 == 0: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True self.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True + if self.dragon_lat_ctrl: + self.lane_change_enabled = False if params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if not self.lane_change_enabled: + self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False + else: + self.dragon_enable_steering_on_signal = False + else: + self.dragon_enable_steering_on_signal = False self.dp_last_modified = modified # *** compute control surfaces *** @@ -137,7 +145,7 @@ class CarController(): # dragonpilot if enabled: if self.dragon_enable_steering_on_signal: - if CS.left_blinker_on == 0 and CS.right_blinker_on == 0: + if not CS.out.leftBlinker and not CS.out.rightBlinker: self.turning_signal_timer = 0 else: self.turning_signal_timer = 100 @@ -150,17 +158,6 @@ class CarController(): if not self.dragon_lat_ctrl: apply_steer_req = 0 - else: - if CS.v_ego > 12.5: - if right_lane_depart and not CS.right_blinker_on: - apply_steer = self.last_steer + 3 - apply_steer = min(apply_steer , 800) - apply_steer_req = 1 - - if left_lane_depart and not CS.left_blinker_on: - apply_steer = self.last_steer - 3 - apply_steer = max(apply_steer , -800) - apply_steer_req = 1 #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 2ec77405c..f22101640 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -5,7 +5,10 @@ from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR - +from common.realtime import sec_since_boot +from common.params import Params +from selfdrive.dragonpilot.dragonconf import dp_get_last_modified +params = Params() class CarState(CarStateBase): def __init__(self, CP): @@ -19,10 +22,23 @@ class CarState(CarStateBase): # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] # is zeroed to where the steering angle is at start. # Need to apply an offset as soon as the steering angle measurements are both received - self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR + self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR or CP.carFingerprint == CAR.LEXUS_ISH self.angle_offset = 0. + # dragonpilot + self.dragon_toyota_stock_dsu = False + self.ts_last_check = 0. + self.last_modifed = None def update(self, cp, cp_cam): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check >= 5.: + modified = dp_get_last_modified() + if self.last_modifed != modified: + self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False + self.last_modifed = modified + self.ts_last_check = ts + ret = car.CarState.new_message() ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], @@ -34,8 +50,11 @@ class CarState(CarStateBase): if self.CP.enableGasInterceptor: ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. ret.gasPressed = ret.gas > 15 + elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + self.pedal_gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL'] + ret.gasPressed = ret.gas > 1e-5 else: - ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] + self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] ret.gasPressed = ret.gas > 1e-5 ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS @@ -77,11 +96,19 @@ class CarState(CarStateBase): ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0 ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = False + elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + ret.cruiseState.available = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED'] * CV.KPH_TO_MS + self.low_speed_lockout = False else: ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 - self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] + if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + # Lexus ISH does not have CRUISE_STATUS value (always 0), so we use CRUISE_ACTIVE value instead + self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'] + else: + self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: # ignore standstill in hybrid vehicles, since pcm allows to restart without # receiving any special command. Also if interceptor is detected @@ -92,6 +119,8 @@ class CarState(CarStateBase): if self.CP.carFingerprint == CAR.PRIUS: ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 + elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]: + ret.genericToggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM']) else: ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) @@ -101,6 +130,12 @@ class CarState(CarStateBase): self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] + if self.dragon_toyota_stock_dsu and ret.genericToggle and ret.cruiseState.available: + enable_acc = True + if ret.seatbeltUnlatched or ret.doorOpen: + enable_acc = False + ret.cruiseState.enabled = enable_acc + return ret @staticmethod @@ -127,6 +162,7 @@ class CarState(CarStateBase): ("CRUISE_STATE", "PCM_CRUISE", 0), ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers ("LKA_STATE", "EPS_STATUS", 0), ("IPAS_STATE", "EPS_STATUS", 1), @@ -171,9 +207,6 @@ class CarState(CarStateBase): signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) checks.append(("PCM_CRUISE_2", 33)) - if CP.carFingerprint in NO_DSU_CAR or CP.carFingerprint == CAR.LEXUS_ISH: - signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)] - if CP.carFingerprint == CAR.PRIUS: signals += [("STATE", "AUTOPARK_STATUS", 0)] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index afe91c46b..152cae2bb 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -303,6 +303,7 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): + self.dp_load_params('toyota') # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c95b64a5..4c521ae35 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -554,7 +554,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if dp_last_modified != modified: dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False dragon_lat_control = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True - dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True + if dragon_lat_control: + dragon_display_steering_limit_alert = False if params.get("DragonDisplaySteeringLimitAlert", encoding='utf8') == "0" else True + else: + dragon_display_steering_limit_alert = False dragon_lead_car_moving_alert = True if params.get("DragonEnableLeadCarMovingAlert", encoding='utf8') == "1" else False dp_last_modified = modified ts_last_check = ts @@ -609,9 +612,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): else: dragon_stopped_has_lead_count = 0 - # when we detect lead car over a sec and the lead car is started moving, we are ready to send alerts + # when we detect lead car over 3 secs and the lead car is started moving, we are ready to send alerts # once the condition is triggered, we want to keep the trigger - if dragon_stopped_has_lead_count >= 100: + if dragon_stopped_has_lead_count >= 300: if abs(sm['plan'].vTargetFuture) >= 0.1: events.append(create_event('leadCarMoving', [ET.WARNING])) else: diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index 762ee8011..0a6a3a792 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -59,20 +59,17 @@ def dmonitoringd_thread(sm=None, pm=None): # load driver monitor val only when safety is on if dp_enable_driver_safety_check: dp_enable_driver_monitoring = False if params.get("DragonEnableDriverMonitoring", encoding='utf8') == "0" else True - # load steering monitor timer val only when driver monitor is on - if dp_enable_driver_safety_check: try: dp_awareness_time = int(params.get("DragonSteeringMonitorTimer", encoding='utf8')) except TypeError: dp_awareness_time = 0. driver_status.awareness_time = 86400 if dp_awareness_time <= 0. else dp_awareness_time * 60. + else: + dp_enable_driver_monitoring = False + driver_status.awareness_time = 86400 dp_last_modified = modified last_ts = cur_time - if not dp_enable_driver_safety_check: - dp_enable_driver_monitoring = False - driver_status.awareness_time = 86400 - # reset all awareness val and set to rhd region, this will enforce steering monitor. if not dp_enable_driver_monitoring: driver_status.is_rhd_region = True diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index a42a5c8d4..03bb2b8a0 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -62,7 +62,6 @@ class PathPlanner(): # dragonpilot self.params = Params() - self.dragon_assisted_lc_enabled = False self.dragon_auto_lc_enabled = False self.dragon_auto_lc_allowed = False self.dragon_auto_lc_timer = None @@ -94,8 +93,8 @@ class PathPlanner(): if cur_time - self.last_ts >= 5.: modified = dp_get_last_modified() if self.dp_last_modified != modified: - self.dragon_assisted_lc_enabled = self.lane_change_enabled - if self.dragon_assisted_lc_enabled: + self.lane_change_enabled = True if self.params.get("LaneChangeEnabled", encoding='utf8') == "1" else False + if self.lane_change_enabled: self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False # adjustable assisted lc min speed self.dragon_assisted_lc_min_mph = int(self.params.get("DragonAssistedLCMinMPH", encoding='utf8')) * CV.MPH_TO_MS @@ -113,6 +112,8 @@ class PathPlanner(): self.dragon_auto_lc_delay = int(self.params.get("DragonAutoLCDelay", encoding='utf8')) if self.dragon_auto_lc_delay < 0: self.dragon_auto_lc_delay = 0 + else: + self.dragon_auto_lc_enabled = False self.dp_last_modified = modified self.last_ts = cur_time diff --git a/selfdrive/dragonpilot/appd/appd.py b/selfdrive/dragonpilot/appd/appd.py index 86c97e282..8da5ff928 100644 --- a/selfdrive/dragonpilot/appd/appd.py +++ b/selfdrive/dragonpilot/appd/appd.py @@ -20,10 +20,6 @@ class App(): TYPE_FULLSCREEN = 3 TYPE_UTIL = 4 - # frame app - FRAME = "ai.comma.plus.frame" - FRAME_MAIN = ".MainActivity" - # offroad app OFFROAD = "ai.comma.plus.offroad" OFFROAD_MAIN = ".MainActivity" @@ -58,7 +54,7 @@ class App(): # read manual run param self.manual_ctrl_param = manual_ctrl_param # if it's a service app, we do not kill if device is too hot - # if it's a full screen app, we need to do extra process on frame/offroad + # if it's a full screen app, we need to do extra process on offroad self.app_type = app_type # app permissions self.permissions = permissions @@ -119,7 +115,6 @@ class App(): if force or not self.is_running: # if it's a full screen app, we need to stop frame and offroad to get keyboard access if self.app_type == self.TYPE_FULLSCREEN: - self.system("pm disable %s" % self.FRAME) self.system("am start -n %s/%s" % (self.OFFROAD, self.OFFROAD_MAIN)) self.system("pm enable %s" % self.app) @@ -143,12 +138,10 @@ class App(): # only kill app if it's running if force or self.is_running: - # if it's a full screen app, we need to restart offroad and frame + # if it's a full screen app, we need to restart offroad if self.app_type == self.TYPE_FULLSCREEN: self.system("pm disable %s" % self.OFFROAD) self.system("pm enable %s" % self.OFFROAD) - self.system("pm enable %s" % self.FRAME) - self.system("am start -n %s/%s" % (self.FRAME, self.FRAME_MAIN)) if self.app_type == self.TYPE_GPS_SERVICE: self.appops_set(self.app, "android:mock_location", "deny") diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 9437308ba..c440cfb55 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -69,7 +69,7 @@ default_conf = { 'DragonBTG': 0, 'DragonBootHotspot': 0, 'DragonAccelProfile': '0', - 'DragonLastModified': str(floor(time.time())) + 'DragonLastModified': str(floor(time.time())), } deprecated_conf = { diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py old mode 100644 new mode 100755 index e04f7a7c6..33d0af159 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -17,7 +17,7 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature -from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present +from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present, set_battery_charging, get_battery_charging FW_SIGNATURE = get_expected_signature() @@ -443,12 +443,13 @@ def thermald_thread(): # we update charging status once every min if ts_last_charging_ctrl is None or ts - ts_last_charging_ctrl >= 60.: if dragon_charging_ctrl: - if msg.thermal.batteryPercent >= dragon_to_discharge: - os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') - if msg.thermal.batteryPercent <= dragon_to_charge: - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') + if msg.thermal.batteryPercent >= dragon_to_discharge and get_battery_charging(): + set_battery_charging(False) + if msg.thermal.batteryPercent <= dragon_to_charge and not get_battery_charging(): + set_battery_charging(True) else: - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') + if not get_battery_charging(): + set_battery_charging(True) ts_last_charging_ctrl = ts # report to server once per minute diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index 717449dee..cd5ce49b6 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -52,6 +52,20 @@ static void ui_draw_sidebar_network_strength(UIState *s, bool hasSidebar) { nvgFill(s->vg); } +static void ui_draw_sidebar_ip_addr(UIState *s, bool hasSidebar) { + const int network_ip_w = 176; + const int network_ip_x = hasSidebar ? 58 : -(sbr_w); + const int network_ip_y = 255; + + char network_ip_str[20]; + snprintf(network_ip_str, sizeof(network_ip_str), "%s", s->scene.ipAddr); + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 32); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, network_ip_x, network_ip_y, network_ip_w, network_ip_str, NULL); +} + static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { const int battery_img_h = 36; const int battery_img_w = 76; @@ -75,9 +89,24 @@ static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { nvgFill(s->vg); } +static void ui_draw_sidebar_battery_text(UIState *s, bool hasSidebar) { + const int battery_img_h = 36; + const int battery_img_w = 76; + const int battery_img_x = hasSidebar ? 150 : -(sbr_w); + const int battery_img_y = 305; + + char battery_str[6]; + snprintf(battery_str, sizeof(battery_str), "%d%%", s->scene.batteryPercent); + nvgFillColor(s->vg, strcmp(s->scene.batteryStatus, "Charging") == 0? COLOR_GREEN : s->scene.batteryPercent <= 50? COLOR_YELLOW : s->scene.batteryPercent <= 15? COLOR_RED : COLOR_WHITE); + nvgFontSize(s->vg, 40); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_str, NULL); +} + static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { const int network_x = hasSidebar ? 50 : -(sbr_w); - const int network_y = 273; + const int network_y = 303; const int network_w = 100; const int network_h = 100; const char *network_types[6] = {"--", "WiFi", "2G", "3G", "4G", "5G"}; @@ -200,20 +229,21 @@ static void ui_draw_sidebar_panda_metric(UIState *s, bool hasSidebar) { snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO PANDA"); } else if (s->scene.hwType == cereal_HealthData_HwType_whitePanda) { panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nACTIVE"); } else if ( (s->scene.hwType == cereal_HealthData_HwType_greyPanda) || (s->scene.hwType == cereal_HealthData_HwType_blackPanda) || (s->scene.hwType == cereal_HealthData_HwType_uno)) { if (s->scene.satelliteCount == -1) { panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); - } else if (s->scene.satelliteCount < 6) { - panda_severity = 1; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nNO GPS"); - } else if (s->scene.satelliteCount >= 6) { - panda_severity = 0; - snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA GOOD GPS"); + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nACTIVE"); + } else { + if (s->scene.satelliteCount < 6) { + panda_severity = 1; + } else if (s->scene.satelliteCount >= 6) { + panda_severity = 0; + } + snprintf(panda_message_str, sizeof(panda_message_str), "%s %d", "PANDA\nGPS:", s->scene.satelliteCount); } } @@ -226,7 +256,8 @@ void ui_draw_sidebar(UIState *s) { ui_draw_sidebar_settings_button(s, hasSidebar); ui_draw_sidebar_home_button(s, hasSidebar); ui_draw_sidebar_network_strength(s, hasSidebar); - ui_draw_sidebar_battery_icon(s, hasSidebar); + ui_draw_sidebar_ip_addr(s, hasSidebar); + ui_draw_sidebar_battery_text(s, hasSidebar); ui_draw_sidebar_network_type(s, hasSidebar); ui_draw_sidebar_storage_metric(s, hasSidebar); ui_draw_sidebar_temp_metric(s, hasSidebar); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index d3acfed5f..3c0a7c50d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -524,6 +524,7 @@ void handle_message(UIState *s, Message * msg) { s->scene.freeSpace = datad.freeSpace; s->scene.thermalStatus = datad.thermalStatus; s->scene.paTemp = datad.pa0; + snprintf(s->scene.ipAddr, sizeof(s->scene.ipAddr), "%s", datad.ipAddr.str); } else if (eventd.which == cereal_Event_ubloxGnss) { struct cereal_UbloxGnss datad; cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index fb400f9ad..384f469c2 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -43,6 +43,7 @@ #define COLOR_WHITE_ALPHA nvgRGBA(255, 255, 255, 85) #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) +#define COLOR_GREEN nvgRGBA(34, 201, 49, 255) #ifndef QCOM #define UI_60FPS @@ -160,6 +161,7 @@ typedef struct UIScene { // for minimal UI float angleSteersDes; float angleSteers; + char ipAddr[20]; // for blinker, from kegman bool leftBlinker; diff --git a/selfdrive/updated.py b/selfdrive/updated.py old mode 100644 new mode 100755