Merge pull request commaai#514 from arne182/071-clean

071 clean updates

panda/board/safety/safety_toyota.h

@@ -4,17 +4,17 @@ const int TOYOTA_MAX_TORQUE = 1500;       // max torque cmd allowed ever
 // rate based torque limit + stay within actually applied
 // packet is sent at 100hz, so this limit is 1000/sec
 const int TOYOTA_MAX_RATE_UP = 10;        // ramp up slow
-const int TOYOTA_MAX_RATE_DOWN = 25;      // ramp down fast
+const int TOYOTA_MAX_RATE_DOWN = 44;      // ramp down fast
 const int TOYOTA_MAX_TORQUE_ERROR = 350;  // max torque cmd in excess of torque motor
 
 // real time torque limit to prevent controls spamming
 // the real time limit is 1500/sec
-const int TOYOTA_MAX_RT_DELTA = 375;      // max delta torque allowed for real time checks
+const int TOYOTA_MAX_RT_DELTA = 475;      // max delta torque allowed for real time checks
 const uint32_t TOYOTA_RT_INTERVAL = 250000;    // 250ms between real time checks
 
 // longitudinal limits
-const int TOYOTA_MAX_ACCEL = 1500;        // 1.5 m/s2
-const int TOYOTA_MIN_ACCEL = -3000;       // 3.0 m/s2
+const int TOYOTA_MAX_ACCEL = 4000;        // 4.0 m/s2
+const int TOYOTA_MIN_ACCEL = -8000;       // 8.0 m/s2
 
 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475;  // ratio between offset and gain from dbc file
 
@@ -37,6 +37,8 @@ int toyota_desired_torque_last = 0;       // last desired steer torque
 int toyota_rt_torque_last = 0;            // last desired torque for real time check
 uint32_t toyota_ts_last = 0;
 int toyota_cruise_engaged_last = 0;       // cruise state
+int ego_speed_toyota = 0;                 // speed
+int toyota_gas_pressed = 0;
 int toyota_gas_prev = 0;
 struct sample_t toyota_torque_meas;       // last 3 motor torques produced by the eps
 
@@ -63,7 +65,12 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if (valid) {
     int bus = GET_BUS(to_push);
     int addr = GET_ADDR(to_push);
-
+    // sample speed
+    if (addr == 0xb4) {
+      // Middle bytes needed
+      ego_speed_toyota = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
+      ego_speed_toyota = to_signed(ego_speed_toyota, 16);
+    }
     // get eps motor torque (0.66 factor in dbc)
     if (addr == 0x260) {
       int torque_meas_new = (GET_BYTE(to_push, 5) << 8) | GET_BYTE(to_push, 6);
@@ -81,9 +88,9 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
 
     // enter controls on rising edge of ACC, exit controls on ACC off
-    if (addr == 0x1D2) {
-      // 5th bit is CRUISE_ACTIVE
-      int cruise_engaged = GET_BYTE(to_push, 0) & 0x20;
+    if (addr == 0x1D3) {
+      // 5th bit is MAIN_ON
+      int cruise_engaged = GET_BYTE(to_push, 1) >> 7;
       if (!cruise_engaged) {
         controls_allowed = 0;
       }
@@ -99,7 +106,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       int gas_interceptor = GET_INTERCEPTOR(to_push);
       if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRESHOLD) &&
           (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRESHOLD)) {
-        controls_allowed = 0;
+        toyota_gas_pressed = 1;
       }
       gas_interceptor_prev = gas_interceptor;
     }
@@ -108,7 +115,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     if (addr == 0x2C1) {
       int gas = GET_BYTE(to_push, 6) & 0xFF;
       if ((gas > 0) && (toyota_gas_prev == 0) && !gas_interceptor_detected) {
-        controls_allowed = 0;
+        toyota_gas_pressed = 1;
       }
       toyota_gas_prev = gas;
     }
@@ -142,7 +149,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     if (addr == 0x200) {
       if (!controls_allowed) {
         if (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)) {
-          tx = 0;
+          toyota_gas_pressed = 1;
         }
       }
     }
@@ -194,8 +201,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       }
 
       // no torque if controls is not allowed
-      if (!controls_allowed && (desired_torque != 0)) {
+      if (!controls_allowed) {
+        if (ego_speed_toyota > 4500){
+          violation |= max_limit_check(desired_torque, 805, -805);
+        } else {
         violation = 1;
+        }
       }
 
       // reset to 0 if either controls is not allowed or there's a violation

selfdrive/car/toyota/carcontroller.py

@@ -11,8 +11,8 @@
 
 # Accel limits
 ACCEL_HYST_GAP = 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_MAX = 3.5  # 1.5 m/s2
+ACCEL_MIN = -3.5 # 3   m/s2
 ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
 
 
@@ -126,6 +126,18 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
     apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
     apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
 
+    if CS.CP.enableGasInterceptor:
+      if CS.pedal_gas > 15.0:
+        apply_accel = max(apply_accel, 0.06)
+      if CS.brake_pressed:
+        apply_gas = 0.0
+        apply_accel = min(apply_accel, 0.00)
+    else:
+      if CS.pedal_gas > 0.0:
+        apply_accel = max(apply_accel, 0.0)
+      if CS.brake_pressed and CS.v_ego > 1:
+        apply_accel = min(apply_accel, 0.0)
+
     # steer torque
     new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
 

selfdrive/car/toyota/carstate.py

@@ -121,6 +121,8 @@ def __init__(self, CP):
     self.right_blinker_on = 0
     self.angle_offset = 0.
     self.init_angle_offset = False
+    self.v_cruise_pcmlast = 41.0
+    self.setspeedoffset = 34.0
 
     if not travis:
       self.arne_pm = messaging_arne.PubMaster(['liveTrafficData', 'arne182Status'])
@@ -211,6 +213,41 @@ def update(self, cp, cp_cam):
     else:
       self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
       self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
+    if self.CP.carFingerprint in TSS2_CAR:
+      minimum_set_speed = 27.0
+    else:
+      minimum_set_speed = 41.0
+    if cp.vl["PCM_CRUISE"]['CRUISE_STATE'] and not self.pcm_acc_status:
+      if self.v_ego < 11.38:
+        self.setspeedoffset = max(min(int(minimum_set_speed-self.v_ego*3.6),(minimum_set_speed-7.0)),0.0)
+        self.v_cruise_pcmlast = self.v_cruise_pcm
+      else:
+        self.setspeedoffset = 0
+        self.v_cruise_pcmlast = self.v_cruise_pcm
+    if self.v_cruise_pcm < self.v_cruise_pcmlast:
+      if self.setspeedcounter > 0 and self.v_cruise_pcm > minimum_set_speed:
+        self.setspeedoffset = self.setspeedoffset + 4
+      else:
+        if math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed)  + 169.0*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(self.v_cruise_pcm-(minimum_set_speed-1.0))) > 0:
+          self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed)  + 169*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(self.v_cruise_pcm-(minimum_set_speed-1.0)))
+      self.setspeedcounter = 50
+    if self.v_cruise_pcmlast < self.v_cruise_pcm:
+      if self.setspeedcounter > 0 and (self.setspeedoffset - 4) > 0:
+        self.setspeedoffset = self.setspeedoffset - 4
+      else:
+        self.setspeedoffset = self.setspeedoffset + math.floor((int((-self.v_cruise_pcm)*(minimum_set_speed-7.0)/(169.0-minimum_set_speed)  + 169*(minimum_set_speed-7.0)/(169.0-minimum_set_speed))-self.setspeedoffset)/(170-self.v_cruise_pcm))
+      self.setspeedcounter = 50
+    if self.setspeedcounter > 0:
+      self.setspeedcounter = self.setspeedcounter - 1
+    self.v_cruise_pcmlast = self.v_cruise_pcm
+    if int(self.v_cruise_pcm) - self.setspeedoffset < 7:
+      self.setspeedoffset = int(self.v_cruise_pcm) - 7
+    if int(self.v_cruise_pcm) - self.setspeedoffset > 169:
+      self.setspeedoffset = int(self.v_cruise_pcm) - 169
+
+
+    self.v_cruise_pcm = min(max(7, int(self.v_cruise_pcm) - self.setspeedoffset),169)
+
     self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
     self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
     self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)

selfdrive/car/toyota/interface.py

@@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController):
     self.CP = CP
     self.VM = VehicleModel(CP)
-
+    self.waiting = False
     self.frame = 0
     self.gas_pressed_prev = False
     self.brake_pressed_prev = False
@@ -34,7 +34,7 @@ def __init__(self, CP, CarController):
 
   @staticmethod
   def compute_gb(accel, speed):
-    return float(accel) / 3.0
+    return float(accel) / 4.0
 
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
@@ -94,7 +94,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
       ret.lateralTuning.init('lqr')
 
       ret.lateralTuning.lqr.scale = 1500.0
-      ret.lateralTuning.lqr.ki = 0.05
+      ret.lateralTuning.lqr.ki = 0.06
 
       ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268]
       ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05]
@@ -242,8 +242,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     # steer, gas, brake limitations VS speed
     ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS]  # breakpoints at 1 and 40 kph
     ret.steerMaxV = [1., 1.]  # 2/3rd torque allowed above 45 kph
-    ret.brakeMaxBP = [0.]
-    ret.brakeMaxV = [1.]
+    ret.brakeMaxBP = [0., 35., 55.]
+    ret.brakeMaxV = [1.0, 0.8, 0.7]
 
     ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
     # In TSS2 cars the camera does long control
@@ -271,15 +271,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False,
     ret.startAccel = 0.0
 
     if ret.enableGasInterceptor:
-      ret.gasMaxBP = [0., 9., 35]
+      ret.gasMaxBP = [0., 9., 55]
       ret.gasMaxV = [0.2, 0.5, 0.7]
-      ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
-      ret.longitudinalTuning.kiV = [0.18, 0.12]
+      ret.longitudinalTuning.kpV = [0.50, 0.4, 0.3]  # braking tune
+      ret.longitudinalTuning.kiV = [0.135, 0.1]
     else:
-      ret.gasMaxBP = [0.]
-      ret.gasMaxV = [0.5]
-      ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
-      ret.longitudinalTuning.kiV = [0.54, 0.36]
+      ret.gasMaxBP = [0., 9., 55]
+      ret.gasMaxV = [0.2, 0.5, 0.7]
+      ret.longitudinalTuning.kpV = [0.325, 0.325, 0.325]  # braking tune from rav4h
+      ret.longitudinalTuning.kiV = [0.1, 0.10]
 
     return ret
 
@@ -333,7 +333,18 @@ def update(self, c, can_strings):
     ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
 
     # cruise state
-    ret.cruiseState.enabled = self.CS.pcm_acc_active
+    if not self.cruise_enabled_prev:
+      self.waiting = False
+      ret.cruiseState.enabled = self.CS.pcm_acc_active
+    else:
+      if self.keep_openpilot_engaged:
+        ret.cruiseState.enabled = bool(self.CS.main_on)
+      if not self.CS.pcm_acc_active:
+        #eventsArne182.append(create_event_arne('longControlDisabled', [ET.WARNING]))
+        ret.brakePressed = True
+        self.waiting = False
+    if self.CS.v_ego < 1 or not self.keep_openpilot_engaged:
+      ret.cruiseState.enabled = self.CS.pcm_acc_active
     ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = 0.
@@ -404,7 +415,13 @@ def update(self, c, can_strings):
       events.append(create_event('pcmEnable', [ET.ENABLE]))
     elif not ret.cruiseState.enabled:
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
-
+    if not self.waiting and ret.vEgo < 0.3 and not ret.gasPressed and self.CP.carFingerprint == CAR.RAV4H:
+      self.waiting = True
+    if self.waiting:
+      if ret.gasPressed:
+        self.waiting = False
+      #else:
+      #  eventsArne182.append(create_event_arne('waitingMode', [ET.WARNING]))
     # 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 \
        (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):

selfdrive/controls/lib/pathplanner.py

@@ -1,4 +1,3 @@
-import os
 import math
 from common.realtime import sec_since_boot, DT_MDL
 from selfdrive.swaglog import cloudlog
@@ -12,9 +11,8 @@
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
-LOG_MPC = os.environ.get('LOG_MPC', False)
 
-LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
 
 DESIRES = {
@@ -219,12 +217,12 @@ def update(self, sm, pm, CP, VM):
 
     pm.send('pathPlan', plan_send)
 
-    if LOG_MPC:
-      dat = messaging.new_message()
-      dat.init('liveMpc')
-      dat.liveMpc.x = list(self.mpc_solution[0].x)
-      dat.liveMpc.y = list(self.mpc_solution[0].y)
-      dat.liveMpc.psi = list(self.mpc_solution[0].psi)
-      dat.liveMpc.delta = list(self.mpc_solution[0].delta)
-      dat.liveMpc.cost = self.mpc_solution[0].cost
-      pm.send('liveMpc', dat)
+     
+    dat = messaging.new_message()
+    dat.init('liveMpc')
+    dat.liveMpc.x = list(self.mpc_solution[0].x)
+    dat.liveMpc.y = list(self.mpc_solution[0].y)
+    dat.liveMpc.psi = list(self.mpc_solution[0].psi)
+    dat.liveMpc.delta = list(self.mpc_solution[0].delta)
+    dat.liveMpc.cost = self.mpc_solution[0].cost
+    pm.send('liveMpc', dat)

selfdrive/ui/paint.cc

@@ -666,13 +666,13 @@ static void ui_draw_vision_event(UIState *s) {
     nvgSave(s->vg);
     nvgTranslate(s->vg,bg_wheel_x,(bg_wheel_y + (bdr_s*1.5)));
     nvgRotate(s->vg,-img_rotation);
-	  nvgBeginPath(s->vg);
-	  NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)),
-	    img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha);
-	  nvgRect(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), img_wheel_size, img_wheel_size);
-	  nvgFillPaint(s->vg, imgPaint);
+    nvgBeginPath(s->vg);
+    NVGpaint imgPaint = nvgImagePattern(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)),
+	img_wheel_size, img_wheel_size, 0, s->img_wheel, img_wheel_alpha);
+    nvgRect(s->vg, img_wheel_x-bg_wheel_x, img_wheel_y-(bg_wheel_y + (bdr_s*1.5)), img_wheel_size, img_wheel_size);
+    nvgFillPaint(s->vg, imgPaint);
     nvgFill(s->vg);
-	  nvgRestore(s->vg);
+    nvgRestore(s->vg);
   }
 }
 

selfdrive/ui/ui.cc

@@ -275,6 +275,7 @@ void handle_message(UIState *s, Message * msg) {
       s->scene.v_cruise_update_ts = eventd.logMonoTime;
     }
     s->scene.v_cruise = datad.vCruise;
+    s->scene.angleSteers = datad.angleSteers;
     s->scene.v_ego = datad.vEgo;
     s->scene.curvature = datad.curvature;
     s->scene.engaged = datad.enabled;

selfdrive/ui/ui.hpp

@@ -85,7 +85,9 @@ typedef struct UIScene {
   uint64_t v_cruise_update_ts;
   float v_ego;
   bool decel_for_model;
-
+
+  float angleSteers;
+
   float speedlimit;
   float speedlimitaheaddistance;
   bool speedlimitahead_valid;