Merge pull request commaai#118 from arne182/stocklka

Stocklka feature

Author: arne182

panda/board/safety/safety_toyota.h

@@ -2,7 +2,7 @@ int toyota_giraffe_switch_1 = 0;          // is giraffe switch 1 high?
 int toyota_camera_forwarded = 0;          // should we forward the camera bus?
 
 // global torque limit
-const int TOYOTA_MAX_TORQUE = 1500;       // max torque cmd allowed ever
+const int TOYOTA_MAX_TORQUE = 1800;       // max torque cmd allowed ever
 
 // rate based torque limit + stay within actually applied
 // packet is sent at 100hz, so this limit is 1000/sec
@@ -16,8 +16,8 @@ const int TOYOTA_MAX_RT_DELTA = 375;      // max delta torque allowed for real t
 const int 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 = 1800;        // 1.8 m/s2
+const int TOYOTA_MIN_ACCEL = -3600;       // 3.6 m/s2
 
 // global actuation limit state
 int toyota_actuation_limits = 1;          // by default steer limits are imposed
@@ -28,10 +28,17 @@ 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
 struct sample_t toyota_torque_meas;       // last 3 motor torques produced by the eps
 
 
 static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  // sample speed
+  if ((to_push->RIR>>21) == 0xb4) {
+    // Middle bytes needed
+    ego_speed_toyota = (to_push->RDHR >>  8) & 0xFFFF; //Speed is 100x
+  }// Special thanks to Willem Melching for the code
+  
   // get eps motor torque (0.66 factor in dbc)
   if ((to_push->RIR>>21) == 0x260) {
     int torque_meas_new = (((to_push->RDHR) & 0xFF00) | ((to_push->RDHR >> 16) & 0xFF));
@@ -100,11 +107,20 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       uint32_t ts = TIM2->CNT;
 
       // only check if controls are allowed and actuation_limits are imposed
-      if (controls_allowed && toyota_actuation_limits) {
+      if (toyota_actuation_limits) {
 
         // *** global torque limit check ***
-        violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);
-
+        
+        if (!toyota_cruise_engaged_last){
+          if (ego_speed_toyota > 4500){
+            violation |= max_limit_check(desired_torque, 805, -805);
+          } else {
+            violation = 1;
+          }
+          
+        } else {
+          violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);
+        }
         // *** torque rate limit check ***
         violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last,
           &toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR);
@@ -124,12 +140,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
       }
 
       // no torque if controls is not allowed
-      if (!controls_allowed && (desired_torque != 0)) {
-        violation = 1;
-      }
+      //if (!controls_allowed && (desired_torque != 0)) {
+      //  violation = 1;
+      //}
 
       // reset to 0 if either controls is not allowed or there's a violation
-      if (violation || !controls_allowed) {
+      if (violation) {
         toyota_desired_torque_last = 0;
         toyota_rt_torque_last = 0;
         toyota_ts_last = ts;

selfdrive/car/toyota/carcontroller.py

@@ -198,6 +198,19 @@ def update(self, sendcan, enabled, CS, frame, actuators,
       apply_steer_req = 0
     else:
       apply_steer_req = 1
+    if not enabled and rightLane_Depart and CS.v_ego > 12.5 and not CS.right_blinker_on:
+      apply_steer = self.last_steer + 3
+      apply_steer = min(apply_steer , 800)
+      #print "right"
+      #print apply_steer
+      apply_steer_req = 1
+      
+    if not enabled and leftLane_Depart and CS.v_ego > 12.5 and not CS.left_blinker_on:
+      apply_steer = self.last_steer - 3
+      apply_steer = max(apply_steer , -800)
+      #print "left"
+      #print apply_steer
+      apply_steer_req = 1
 
     self.steer_angle_enabled, self.ipas_reset_counter = \
       ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
@@ -288,6 +301,7 @@ def update(self, sendcan, enabled, CS, frame, actuators,
       else:
         if CS.lane_departure_toggle_on:
           can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
+          #print "here"
         else:
           can_sends.append(create_steer_command(self.packer, 0., 0, frame))
         # rav4h with dsu disconnected

selfdrive/car/toyota/carstate.py

@@ -378,7 +378,8 @@ def update(self, cp, cp_cam):
       if self.read_distance_lines == 3:
         self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
       self.read_distance_lines_prev = self.read_distance_lines
-    
+    if cp.vl["EPS_STATUS"]['LKA_STATE'] == 17:
+      self.cstm_btns.set_button_status("lka", 0)
     if bool(cp.vl["JOEL_ID"]['ACC_SLOW']) <> self.acc_slow_on_prev:
       self.acc_slow_on = bool(cp.vl["JOEL_ID"]['ACC_SLOW'])
       if self.acc_slow_on:

selfdrive/controls/lib/planner.py

@@ -542,8 +542,8 @@ def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
     plan_send.plan.aTarget = self.a_acc
     plan_send.plan.vTargetFuture = self.v_acc_future
     plan_send.plan.hasLead = self.mpc1.prev_lead_status
-    plan_send.plan.hasrightLaneDepart = bool(PP.r_poly[3] > -1.15 and not CS.carState.rightBlinker)
-    plan_send.plan.hasleftLaneDepart = bool(PP.l_poly[3] < 1.15 and not CS.carState.leftBlinker)
+    plan_send.plan.hasrightLaneDepart = bool(PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker)
+    plan_send.plan.hasleftLaneDepart = bool(PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker)
     plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
 
     plan_send.plan.vCurvature = self.v_curvature