From ceee2b071f1426c459fa03ef929f26e774bcffff Mon Sep 17 00:00:00 2001 From: arne182 Date: Wed, 20 Mar 2019 09:43:10 +0100 Subject: [PATCH 01/24] If lane is 1.05m from center --- selfdrive/controls/lib/planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index a2d40c2b707cbb..03a09504dfa2d7 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -532,8 +532,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.05 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 From 6aa2814efe46c1598690d5a301f04ed4d01757ce Mon Sep 17 00:00:00 2001 From: arne182 Date: Thu, 21 Mar 2019 17:56:05 +0100 Subject: [PATCH 02/24] Add initial torque request for LDA --- selfdrive/car/toyota/carcontroller.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 3afd369ea30245..301b9f166fdcb2 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -198,6 +198,12 @@ def update(self, sendcan, enabled, CS, frame, actuators, apply_steer_req = 0 else: apply_steer_req = 1 + if not enabled and CS.lkas_barriers ==2: + apply_steer = self.last_steer + 30 + apply_steer_req = 1 + if not enabled and CS.lkas_barriers ==3: + apply_steer = self.last_steer - 30 + 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) From 8ab2f097d999fb22cb20a5820c432e190fe82921 Mon Sep 17 00:00:00 2001 From: arne182 Date: Thu, 21 Mar 2019 18:03:41 +0100 Subject: [PATCH 03/24] If ["EPS_STATUS"]['LKA_STATE'] is 17 lka is not working --- selfdrive/car/toyota/carstate.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 6dde08f593654c..cdd97cfde029aa 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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: From 5ac34fe27ab2abd549686391d2f7f163b1d71537 Mon Sep 17 00:00:00 2001 From: arne182 Date: Thu, 21 Mar 2019 20:50:17 +0100 Subject: [PATCH 04/24] Do not increase steering torque if blinker on Also maximum Torque of 800. Good for when open pilot is not enabled. Must be increased if in conjunction with open pilot. --- selfdrive/car/toyota/carcontroller.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 301b9f166fdcb2..bac6146abd262e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -198,11 +198,13 @@ def update(self, sendcan, enabled, CS, frame, actuators, apply_steer_req = 0 else: apply_steer_req = 1 - if not enabled and CS.lkas_barriers ==2: + if not enabled and CS.lkas_barriers ==2 and not CS.right_blinker_on: apply_steer = self.last_steer + 30 + apply_steer = min(apply_steer , 800) apply_steer_req = 1 - if not enabled and CS.lkas_barriers ==3: + if not enabled and CS.lkas_barriers ==3 and not CS.left_blinker_on: apply_steer = self.last_steer - 30 + apply_steer = max(apply_steer , -800) apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ From 3a2815827469fcaa40bb838ff5fa74ddd017eb67 Mon Sep 17 00:00:00 2001 From: arne182 Date: Fri, 22 Mar 2019 13:58:27 +0100 Subject: [PATCH 05/24] Allow controls even if cruise is off --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 9cab1512f167cb..89fc2005bfc30d 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -54,7 +54,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; } else if (!cruise_engaged) { - controls_allowed = 0; + controls_allowed = 1; } toyota_cruise_engaged_last = cruise_engaged; } From fc14df4d338bd6355e6e7494b46fd9e8ccbb6e89 Mon Sep 17 00:00:00 2001 From: arne182 Date: Fri, 22 Mar 2019 16:55:12 +0100 Subject: [PATCH 06/24] Add debug --- selfdrive/car/toyota/carcontroller.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index bac6146abd262e..dd6bb4c8a625db 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -198,13 +198,18 @@ def update(self, sendcan, enabled, CS, frame, actuators, apply_steer_req = 0 else: apply_steer_req = 1 - if not enabled and CS.lkas_barriers ==2 and not CS.right_blinker_on: + if not enabled and rightLane_Depart and not CS.right_blinker_on: apply_steer = self.last_steer + 30 apply_steer = min(apply_steer , 800) + print "right" + print apply_steer apply_steer_req = 1 - if not enabled and CS.lkas_barriers ==3 and not CS.left_blinker_on: + + if not enabled and leftLane_Depart and not CS.left_blinker_on: apply_steer = self.last_steer - 30 apply_steer = max(apply_steer , -800) + print "left" + print apply_steer apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ From 82873ee47607fe23bb366cbb8bd84b011275201d Mon Sep 17 00:00:00 2001 From: arne182 Date: Fri, 22 Mar 2019 18:48:56 +0100 Subject: [PATCH 07/24] increase right lane depart detection --- selfdrive/controls/lib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 03a09504dfa2d7..509aba65319067 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -532,7 +532,7 @@ 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.05 and not CS.carState.rightBlinker) + 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 From 916d6a4914d70b56f6e494147305e02e43c29e8a Mon Sep 17 00:00:00 2001 From: arne182 Date: Fri, 22 Mar 2019 18:50:01 +0100 Subject: [PATCH 08/24] Reduce the rate of applied steer --- selfdrive/car/toyota/carcontroller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index dd6bb4c8a625db..9f8694fc859c70 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -199,14 +199,14 @@ def update(self, sendcan, enabled, CS, frame, actuators, else: apply_steer_req = 1 if not enabled and rightLane_Depart and not CS.right_blinker_on: - apply_steer = self.last_steer + 30 + 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 not CS.left_blinker_on: - apply_steer = self.last_steer - 30 + apply_steer = self.last_steer - 3 apply_steer = max(apply_steer , -800) print "left" print apply_steer From 7bb0754e2499ec18f12fd6c1d1cd7110660011f7 Mon Sep 17 00:00:00 2001 From: arne182 Date: Fri, 22 Mar 2019 20:39:49 +0100 Subject: [PATCH 09/24] Only apply torque above 40 km/hr --- selfdrive/car/toyota/carcontroller.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 9f8694fc859c70..1bb60042fe61ae 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -198,14 +198,14 @@ def update(self, sendcan, enabled, CS, frame, actuators, apply_steer_req = 0 else: apply_steer_req = 1 - if not enabled and rightLane_Depart and not CS.right_blinker_on: + 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 not CS.left_blinker_on: + 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" @@ -301,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 From 7d05fcdcb73c1230b9f613abbab8ffbabb73f26f Mon Sep 17 00:00:00 2001 From: arne182 Date: Sat, 23 Mar 2019 10:32:51 +0100 Subject: [PATCH 10/24] Only enable steer if cruise is off Gas and brake should not be available for safety reasons. --- panda/board/safety/safety_toyota.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 89fc2005bfc30d..990cd9e11f7043 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -54,7 +54,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (cruise_engaged && !toyota_cruise_engaged_last) { controls_allowed = 1; } else if (!cruise_engaged) { - controls_allowed = 1; + controls_allowed = 0; } toyota_cruise_engaged_last = cruise_engaged; } @@ -100,7 +100,7 @@ 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); @@ -124,12 +124,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; From a60c041ed9d620b7c7346e2f1c5337d302dde9dc Mon Sep 17 00:00:00 2001 From: arne182 Date: Sun, 24 Mar 2019 10:53:14 +0100 Subject: [PATCH 11/24] Increase longitudinal limits by 20% --- panda/board/safety/safety_toyota.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 990cd9e11f7043..94720dc8b2ec93 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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.5 m/s2 +const int TOYOTA_MIN_ACCEL = -3060; // 3.0 m/s2 // global actuation limit state int toyota_actuation_limits = 1; // by default steer limits are imposed From aa8f6ae1f51f565b4e3b29619ec4c261b64e60d9 Mon Sep 17 00:00:00 2001 From: arne182 Date: Sun, 24 Mar 2019 10:54:05 +0100 Subject: [PATCH 12/24] Fix typo --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 94720dc8b2ec93..85dc8d78122f0a 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -17,7 +17,7 @@ const int TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits const int TOYOTA_MAX_ACCEL = 1800; // 1.5 m/s2 -const int TOYOTA_MIN_ACCEL = -3060; // 3.0 m/s2 +const int TOYOTA_MIN_ACCEL = -3600; // 3.0 m/s2 // global actuation limit state int toyota_actuation_limits = 1; // by default steer limits are imposed From 3aae63587b52901a239e2a53278f70072746e435 Mon Sep 17 00:00:00 2001 From: arne182 Date: Sun, 24 Mar 2019 10:54:43 +0100 Subject: [PATCH 13/24] Update comments --- panda/board/safety/safety_toyota.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 85dc8d78122f0a..58d600fafad04f 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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 = 1800; // 1.5 m/s2 -const int TOYOTA_MIN_ACCEL = -3600; // 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 From 0662d4c6face7b41c052c1f8f506ca10e419dd97 Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 16:37:14 +0100 Subject: [PATCH 14/24] Update safety_toyota.h --- panda/board/safety/safety_toyota.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 58d600fafad04f..97bc59d643e1f1 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -104,7 +104,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); - + if (!cruise_engaged){ + violation |= max_limit_check(desired_torque, 800, -800); + } // *** 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); From 3364acb0739d45df62bbbcbb97119d8d127b6cf3 Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 16:43:32 +0100 Subject: [PATCH 15/24] Allow 20% more torque --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 97bc59d643e1f1..15c3ca4411ff54 100644 --- a/panda/board/safety/safety_toyota.h +++ b/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 From bbf9fa60e3f5dd0c3e203df91e79cd6ae8e2df19 Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 17:07:24 +0100 Subject: [PATCH 16/24] Update safety_toyota.h --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 15c3ca4411ff54..a155d8923d7100 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -104,7 +104,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); - if (!cruise_engaged){ + if (toyota_cruise_engaged_last){ violation |= max_limit_check(desired_torque, 800, -800); } // *** torque rate limit check *** From 8dc25b180219b06a3a9e12823be511cfadf06471 Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 17:09:59 +0100 Subject: [PATCH 17/24] Add not --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index a155d8923d7100..eb520d3415f89d 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -104,7 +104,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // *** global torque limit check *** violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); - if (toyota_cruise_engaged_last){ + if (!toyota_cruise_engaged_last){ violation |= max_limit_check(desired_torque, 800, -800); } // *** torque rate limit check *** From 978e4b2dc1f6060c8e50e5641eb730579cef0213 Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 17:15:38 +0100 Subject: [PATCH 18/24] Clean up code --- panda/board/safety/safety_toyota.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index eb520d3415f89d..6485864ba5ba7a 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -103,9 +103,11 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { 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){ violation |= max_limit_check(desired_torque, 800, -800); + } 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, From 3225b96c50cfbfed6c8d7c5b5a7deed4772350fe Mon Sep 17 00:00:00 2001 From: arne182 Date: Mon, 25 Mar 2019 23:01:29 +0100 Subject: [PATCH 19/24] Remove debug --- selfdrive/car/toyota/carcontroller.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 1bb60042fe61ae..bbb60fca31cc27 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -201,15 +201,15 @@ def update(self, sendcan, enabled, CS, frame, actuators, 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 + #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 + #print "left" + #print apply_steer apply_steer_req = 1 self.steer_angle_enabled, self.ipas_reset_counter = \ @@ -301,7 +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" + #print "here" else: can_sends.append(create_steer_command(self.packer, 0., 0, frame)) # rav4h with dsu disconnected From 150eed89bcc2d72dec7f910b8e3049eaf6465611 Mon Sep 17 00:00:00 2001 From: arne182 Date: Tue, 26 Mar 2019 20:26:15 +0100 Subject: [PATCH 20/24] Add speed restriction of 45km/hr Thank to @pd0wm we have now safety until 45km/hr. Below that the stock LKA did not work anyway. --- panda/board/safety/safety_toyota.h | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 6485864ba5ba7a..58bfa70f1f2aed 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -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 = 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 = (to_push->RDLR >> (5*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)); @@ -105,7 +112,12 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // *** global torque limit check *** if (!toyota_cruise_engaged_last){ - violation |= max_limit_check(desired_torque, 800, -800); + if (ego_speed > 4500){ + violation |= max_limit_check(desired_torque, 800, -800); + } else { + violation = 1; + } + } else { violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE); } From 9187045e47234742b4a5614f7d32fef715cedb64 Mon Sep 17 00:00:00 2001 From: arne182 Date: Tue, 26 Mar 2019 22:20:37 +0100 Subject: [PATCH 21/24] Was already defined --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 58bfa70f1f2aed..19418e5f54484f 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -28,7 +28,7 @@ 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 = 0; // speed +//int ego_speed = 0; // speed struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps From 7b73e2ff7d9111d65c18809aa7af7c38ef1c5ac1 Mon Sep 17 00:00:00 2001 From: arne182 Date: Tue, 26 Mar 2019 22:24:05 +0100 Subject: [PATCH 22/24] Change variable name --- panda/board/safety/safety_toyota.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 19418e5f54484f..2c9488a989a21a 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -28,7 +28,7 @@ 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 = 0; // speed +int ego_speed_toyota = 0; // speed struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps @@ -36,7 +36,7 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // sample speed if ((to_push->RIR>>21) == 0xb4) { // Middle bytes needed - ego_speed = (to_push->RDLR >> (5*8)) & 0xFFFF; //Speed is 100x + ego_speed_toyota = (to_push->RDLR >> (5*8)) & 0xFFFF; //Speed is 100x }// Special thanks to Willem Melching for the code // get eps motor torque (0.66 factor in dbc) @@ -112,7 +112,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // *** global torque limit check *** if (!toyota_cruise_engaged_last){ - if (ego_speed > 4500){ + if (ego_speed_toyota > 4500){ violation |= max_limit_check(desired_torque, 800, -800); } else { violation = 1; From 6c2062664673e9ac976d66edfe33d37c6ed30e37 Mon Sep 17 00:00:00 2001 From: arne182 Date: Wed, 27 Mar 2019 06:05:44 +0100 Subject: [PATCH 23/24] Messages split into two 32 bit is maximum so rdlr and rdhr --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 2c9488a989a21a..60e1b6e49f1322 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -36,7 +36,7 @@ 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->RDLR >> (5*8)) & 0xFFFF; //Speed is 100x + 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) From 6e9dcba79aadfe9bde844d0d1862c588aa3599b5 Mon Sep 17 00:00:00 2001 From: arne182 Date: Wed, 27 Mar 2019 09:16:11 +0100 Subject: [PATCH 24/24] Torque allowed up to 805 --- panda/board/safety/safety_toyota.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 60e1b6e49f1322..a1343855385bde 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -113,7 +113,7 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { if (!toyota_cruise_engaged_last){ if (ego_speed_toyota > 4500){ - violation |= max_limit_check(desired_torque, 800, -800); + violation |= max_limit_check(desired_torque, 805, -805); } else { violation = 1; }