Merge pull request #3 from arne182/066-clean

066 clean
sshane · Dec 2, 2019 · 062f1ed · 062f1ed
2 parents cb0f14f + c808c79
commit 062f1ed
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 17 deletions.
diff --git 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 @0xbaa8c5d505f727ea {
+    # TODO: copy from error list
+    longControlDisabled @0;
+    longControlBrakeDisabled @1;
+  }
+}
+
+
+struct CarStateArne182 {
+  events @0 :List(CarEventArne182);
+}
+
+
 struct Arne182Status { 
   blindspot @0 :Bool;
   distanceToggle @1 :Float32;

diff --git 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)

diff --git 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"""
@@ -146,10 +144,8 @@ 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:
+    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.
@@ -182,15 +178,9 @@ 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))
+      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)
@@ -221,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