From dc8d7e3939bba1a5ae1dca00d46c71a34032476f Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Mon, 28 Oct 2019 14:12:50 +1000 Subject: [PATCH] Adjust dragon_allow_gas logic --- panda/board/safety/safety_honda.h | 10 ++++----- panda/board/safety/safety_toyota.h | 13 ++++++------ selfdrive/car/honda/carcontroller.py | 22 ++++++++++---------- selfdrive/car/toyota/carcontroller.py | 30 +++++++++++++-------------- 4 files changed, 37 insertions(+), 38 deletions(-) diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 5121e71cf..10c436719 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -19,7 +19,7 @@ bool honda_fwd_brake = false; static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); - //int len = GET_LEN(to_push); + int len = GET_LEN(to_push); int bus = GET_BUS(to_push); // sample speed @@ -63,23 +63,23 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6) // length check because bosch hardware also uses this id (0x201 w/ len = 8) - if (false) { + if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD) && long_controls_allowed) { - controls_allowed = 0; + controls_allowed = 1; } gas_interceptor_prev = gas_interceptor; } // exit controls on rising edge of gas press if no interceptor - if (false) { + if (!gas_interceptor_detected) { if (addr == 0x17C) { int gas = GET_BYTE(to_push, 0); if (gas && !(honda_gas_prev) && long_controls_allowed) { - controls_allowed = 0; + controls_allowed = 1; } honda_gas_prev = gas; } diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 4e5bbe195..d53429a14 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -54,12 +54,11 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off - controls_allowed = 1; - if (false) { + if (addr == 0x1D2) { // 5th bit is CRUISE_ACTIVE int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; if (!cruise_engaged) { - controls_allowed = 0; + controls_allowed = 1; } if (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; @@ -68,22 +67,22 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // exit controls on rising edge of interceptor gas press - if (false) { + if (addr == 0x201) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD) && long_controls_allowed) { - controls_allowed = 0; + controls_allowed = 1; } gas_interceptor_prev = gas_interceptor; } // exit controls on rising edge of gas press - if (false) { + if (addr == 0x2C1) { int gas = GET_BYTE(to_push, 6) & 0xFF; if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected && long_controls_allowed) { - controls_allowed = 0; + controls_allowed = 1; } toyota_gas_prev = gas; } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 93e107191..e0bd669d3 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -88,7 +88,7 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False + # self.dragon_allow_gas = False self.dragon_lat_ctrl = True def update(self, enabled, CS, frame, actuators, \ @@ -97,7 +97,7 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + # self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True # *** apply brake hysteresis *** @@ -190,15 +190,15 @@ class CarController(): # DragonAllowGas # if we detect gas pedal pressed, we do not want OP to apply gas or brake # gasPressed code from interface.py - if not CS.CP.enableGasInterceptor: - gas_pressed = CS.pedal_gas > 0 - else: - gas_pressed = CS.user_gas_pressed - dragon_apply_brake = apply_brake - if self.dragon_allow_gas and gas_pressed: - dragon_apply_brake = 0 - apply_gas = 0 - can_sends.append(hondacan.create_brake_command(self.packer, dragon_apply_brake, pump_on, + # if not CS.CP.enableGasInterceptor: + # gas_pressed = CS.pedal_gas > 0 + # else: + # gas_pressed = CS.user_gas_pressed + # dragon_apply_brake = apply_brake + # if self.dragon_allow_gas and gas_pressed: + # dragon_apply_brake = 0 + # apply_gas = 0 + can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) self.apply_brake_last = apply_brake diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d0d8a5ff9..ff52c7619 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -120,7 +120,7 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False + # self.dragon_allow_gas = False self.dragon_lat_ctrl = True self.dragon_lane_departure_warning = True @@ -130,7 +130,7 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False + # self.dragon_allow_gas = True if params.get("DragonAllowGas", 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 @@ -244,19 +244,19 @@ class CarController(): elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) - # DragonAllowGas - # if we detect gas pedal pressed, we do not want OP to apply gas or brake - # gasPressed code from interface.py - if CS.CP.enableGasInterceptor: - # use interceptor values to disengage on pedal press - gas_pressed = CS.pedal_gas > 15 - else: - gas_pressed = CS.pedal_gas > 0 - - if self.dragon_allow_gas and gas_pressed: - apply_accel = 0 - apply_gas = 0 - + # # DragonAllowGas + # # if we detect gas pedal pressed, we do not want OP to apply gas or brake + # # gasPressed code from interface.py + # if CS.CP.enableGasInterceptor: + # # use interceptor values to disengage on pedal press + # gas_pressed = CS.pedal_gas > 15 + # else: + # gas_pressed = CS.pedal_gas > 0 + # + # if self.dragon_allow_gas and gas_pressed: + # apply_accel = 0 + # apply_gas = 0 + # # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged