From 7b8e236a235657ae4b8010c5af39814fcd444aca Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 2 Dec 2019 18:25:20 +0100 Subject: [PATCH 1/5] Try the reset for pid after gas pedal is --- selfdrive/controls/lib/longcontrol.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 2b6d14a4c682ee..c78abee85f574f 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -146,8 +146,6 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ brake_pressed, cruise_standstill) v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if not (gas_pressed or brake_pressed) and self.actuator_pressed: - self.pid.reset() if self.long_control_state == LongCtrlState.off: self.v_pid = v_ego_pid @@ -156,6 +154,15 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: + if gas_pressed or brake_pressed: + if not self.freeze: + self.pid.i = 0.0 + self.freeze = True + else: + if self.freeze: + self.pid.i = 0.0 + self.reset(v_target_future) + self.freeze = False self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = -brake_max @@ -182,13 +189,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.pid._k_p = (CP.longitudinalTuning.kpBP, [x * 1 for x in CP.longitudinalTuning.kpV]) self.pid._k_i = (CP.longitudinalTuning.kiBP, [x * 1 for x in CP.longitudinalTuning.kiV]) self.pid.k_f=1.0 - if gas_pressed or brake_pressed: - if not self.freeze: - self.freeze = True - else: - if self.freeze: - self.pid.i = 0.0 - self.freeze = False + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed)) From e13c3f55a4bfa8fe226b10864277aba8884a58b4 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 2 Dec 2019 18:31:31 +0100 Subject: [PATCH 2/5] treat brake pressed or gas pressed as LongCtrlState.off --- selfdrive/controls/lib/longcontrol.py | 16 ++-------------- 1 file changed, 2 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index c78abee85f574f..3dd1e7f7a485db 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -70,8 +70,6 @@ def __init__(self, CP, compute_gb): self.last_output_gb = 0.0 self.lastdecelForTurn = False self.lead_data = {'vRel': None, 'a_lead': None, 'x_lead': None, 'status': False} - self.freeze = False - self.actuator_pressed = False def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -147,22 +145,13 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if self.long_control_state == LongCtrlState.off: + if self.long_control_state == LongCtrlState.off or gas_pressed or brake_pressed: self.v_pid = v_ego_pid self.pid.reset() output_gb = 0. # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: - if gas_pressed or brake_pressed: - if not self.freeze: - self.pid.i = 0.0 - self.freeze = True - else: - if self.freeze: - self.pid.i = 0.0 - self.reset(v_target_future) - self.freeze = False self.v_pid = v_target self.pid.pos_limit = gas_max self.pid.neg_limit = -brake_max @@ -191,7 +180,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.pid.k_f=1.0 - output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=(prevent_overshoot or gas_pressed or brake_pressed)) + output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) if prevent_overshoot: output_gb = min(output_gb, 0.0) @@ -222,5 +211,4 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.fcw_countdown = self.fcw_countdown -1 final_gas = 0. final_brake = 1.0 - self.actuator_pressed = gas_pressed or brake_pressed return final_gas, final_brake From 8ed5bfd4d3fd42817c1ae800b3b04305fe98e5b7 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 2 Dec 2019 19:58:19 +0100 Subject: [PATCH 3/5] I also want events --- cereal/arne182.capnp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/cereal/arne182.capnp b/cereal/arne182.capnp index d6f3bd34940bbb..f2389b7b97dfb3 100644 --- a/cereal/arne182.capnp +++ b/cereal/arne182.capnp @@ -7,6 +7,30 @@ $Java.outerClassname("arne182"); @0xca61a35dedbd6328; +struct CarEventArne182 @0x9b1657f34caf3ad4 { + name @0 :EventName; + enable @1 :Bool; + noEntry @2 :Bool; + warning @3 :Bool; + userDisable @4 :Bool; + softDisable @5 :Bool; + immediateDisable @6 :Bool; + preEnable @7 :Bool; + permanent @8 :Bool; + + enum EventNameArne182 @0xbaa8c5d505f727dg { + # TODO: copy from error list + longControlDisabled @0; + longControlBrakeDisabled @1; + } +} + + +struct CarStateArne182 { + events @0 :List(CarEventArne182); +} + + struct Arne182Status { blindspot @0 :Bool; distanceToggle @1 :Float32; From d709c1ae94f1297f1f06f0701ebbb0939b7aab8f Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 2 Dec 2019 19:58:49 +0100 Subject: [PATCH 4/5] Fix hex value --- cereal/arne182.capnp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/arne182.capnp b/cereal/arne182.capnp index f2389b7b97dfb3..48d8255bffe04c 100644 --- a/cereal/arne182.capnp +++ b/cereal/arne182.capnp @@ -18,7 +18,7 @@ struct CarEventArne182 @0x9b1657f34caf3ad4 { preEnable @7 :Bool; permanent @8 :Bool; - enum EventNameArne182 @0xbaa8c5d505f727dg { + enum EventNameArne182 @0xbaa8c5d505f727ea { # TODO: copy from error list longControlDisabled @0; longControlBrakeDisabled @1; From c808c79a54950e3aa3c440124fa1afbb41a23e88 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 2 Dec 2019 20:06:58 +0100 Subject: [PATCH 5/5] add possibility for arne182 events --- selfdrive/controls/lib/drive_helpers.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 611ba9e2c8393a..924606abac4a4d 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,4 +1,4 @@ -from cereal import car +from cereal import car, arne182 from common.numpy_fast import clip, interp from selfdrive.config import Conversions as CV @@ -34,8 +34,11 @@ class EventTypes: PERMANENT = 'permanent' -def create_event(name, types): - event = car.CarEvent.new_message() +def create_event(name, types, isarne182 = False): + if isarne182 + event = arne182.CarEventArne182.new_message() + else: + event = car.CarEvent.new_message() event.name = name for t in types: setattr(event, t, True)