spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge 0f97fab786 into 628eecff06

pull/23732/merge
cydia2020 2022-03-18 23:57:32 +08:00 committed by GitHub
parent 628eecff06 0f97fab786
commit 0845421456
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 26 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
selfdrive/car/toyota/carcontroller.py
View File

@ -1,4 +1,5 @@
from cereal import car
from common.realtime import DT_CTRL
from common.numpy_fast import clip, interp
from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
@ -18,6 +19,10 @@ class CarController:
self.last_standstill = False
self.standstill_req = False
self.steer_rate_limited = False
self.permit_braking = True
self.last_gas_press_frame = 0
self.last_standstill_frame = 0
self.last_zero_speed_frame = 0
self.packer = CANPacker(dbc_name)
self.gas = 0
@ -94,14 +99,31 @@ class CarController:
if (self.frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
# record accelerator depression frame
if CS.out.gasPressed:
self.last_gas_press_frame = frame
# record standstill exit frame
if CS.pcm_acc_status == 7:
self.last_standstill_frame = frame
# record last frame when vego is 0
if CS.out.vEgo == 0:
self.last_zero_speed_frame = frame
# handle permit braking
if (CS.out.gasPressed or 1. / DT_CTRL > (frame - self.last_gas_press_frame)) \
or ((actuators.accel > - 1.95) and (2. / DT_CTRL > (frame - self.last_standstill_frame))) \
or (2. / DT_CTRL > (frame - self.last_zero_speed_frame)):
self.permit_braking = False
else:
self.permit_braking = True
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, self.permit_braking))
self.accel = pcm_accel_cmd
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, 1))
if self.frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

4
selfdrive/car/toyota/toyotacan.py
View File

@ -28,14 +28,14 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
return packer.make_can_msg("STEERING_LTA", 0, values)
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type):
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, permit_braking):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
"ACC_TYPE": acc_type,
"DISTANCE": 0,
"MINI_CAR": lead,
"PERMIT_BRAKING": 1,
"PERMIT_BRAKING": permit_braking,
"RELEASE_STANDSTILL": not standstill_req,
"CANCEL_REQ": pcm_cancel,
"ALLOW_LONG_PRESS": 1,