From bf9249923d62b9e257903c7f314282a2a4dfb589 Mon Sep 17 00:00:00 2001 From: Ryley Date: Thu, 21 Apr 2022 19:15:39 -0500 Subject: [PATCH] Create global_ri.py and add radar_command to mazdacan.py --- selfdrive/car/mazda/mazdacan.py | 105 ++++++++++++++++++++++++++++++++ selfdrive/global_ri.py | 11 ++++ 2 files changed, 116 insertions(+) create mode 100644 selfdrive/global_ri.py diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index cfec71620..313ca4e1e 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,6 +1,8 @@ import copy from selfdrive.car.mazda.values import GEN1, Buttons +from common.numpy_fast import clip +from selfdrive import global_ri as RI def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): @@ -129,3 +131,106 @@ def create_button_cmd(packer, car_fingerprint, counter, button): } return packer.make_can_msg("CRZ_BTNS", 0, values) + +def create_radar_command(packer, car_fingerprint, frame, actuators, enabled, cp_cam, cp, cs): + RI.active = True + accel = 0 + radar_accel = int(cp_cam.vl["CRZ_INFO"]["ACCEL_CMD"]) # get stock accel command. dbc offset should be applied already. + ret = [] + + # request low speed mode transition + if cs.speed < 30: #kmh + if not RI.low_speed_mode: + RI.reset = True # request reset of PID loop + RI.radar_accel = radar_accel # save radar accel value to have a smooth transition into low speed mode + else: + RI.low_speed_mode = True # stay in low speed mode + # TODO + # when exiting low speed mode, the OP command is not the same as the MRCC command and this can be felt as a jerk. + # To solve this issue, improve tuning of the OP command. + # Or solve radar track and keep OP in control at high speed. + + # after we have transitioned to low speed mode, we use the vision only accel command + if RI.low_speed_mode: # this is set true in longcontrol.py + accel = actuators.accel * 2000 + clip(accel, -4000, 1000) + else: + accel = radar_accel + + if car_fingerprint in GEN1: + values_21B = { + "ACC_ACTIVE" : int(enabled), + "ACC_SET_ALLOWED" : int(bool(int(cp.vl["GEAR"]["GEAR"]) & 4)), # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh. + "CRZ_ENDED" : 0, # this should keep acc on down to 5km/h on my 2018 M3 + "ACCEL_CMD" : accel, + "STATIC_1" : int(cp_cam.vl["CRZ_INFO"]["STATIC_1"]), #0x7FF, + "STATUS" : int(cp_cam.vl["CRZ_INFO"]["STATUS"]), #1 + "MYSTERY_BIT" : int(cp_cam.vl["CRZ_INFO"]["MYSTERY_BIT"]), + "CTR1" : int(cp_cam.vl["CRZ_INFO"]["CTR1"]) + } + + values_21C = { + "CRZ_ACTIVE" : int(enabled), + "CRZ_AVAILABLE" : int(cp_cam.vl["CRZ_CTRL"]["CRZ_AVAILABLE"]), + "DISTANCE_SETTING" : int(cp_cam.vl["CRZ_CTRL"]["DISTANCE_SETTING"]), + "ACC_ACTIVE_2" : int(enabled), + "DISABLE_TIMER_1" : 0, + "DISABLE_TIMER_2" : 0, + "NEW_SIGNAL_1" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_1"]), + "NEW_SIGNAL_2" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_2"]), + "NEW_SIGNAL_3" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_3"]), + "NEW_SIGNAL_4" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_4"]), + "NEW_SIGNAL_5" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_5"]), + "NEW_SIGNAL_6" : int(cp_cam.vl["CRZ_CTRL"]["NEW_SIGNAL_6"]), + } + + ret.append(packer.make_can_msg("CRZ_INFO", 0, values_21B)) + ret.append(packer.make_can_msg("CRZ_CTRL", 0, values_21C)) + + if (frame % 10 == 0): + values_361 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "SPEED_INVERSE" : int(cp_cam.vl["RADAR_361"]["SPEED_INVERSE"]), + "IS_MOVING" : int(cp_cam.vl["RADAR_361"]["IS_MOVING"]), + "DISTANCE_LEAD" : int(cp_cam.vl["RADAR_361"]["DISTANCE_LEAD"]), + "DISTANCE_RELATED" : int(cp_cam.vl["RADAR_361"]["DISTANCE_RELATED"]), + "RELATIVE_VEL_LEAD" : int(cp_cam.vl["RADAR_361"]["RELATIVE_VEL_LEAD"]), + "STATIC_1" : int(cp_cam.vl["RADAR_361"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_361"]["STATIC_2"]) + } + values_362 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "STEER_ANGLE" : int(cp_cam.vl["RADAR_362"]["STEER_ANGLE"]), + "STATIC_1" : int(cp_cam.vl["RADAR_362"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_362"]["STATIC_2"]), + "STATIC_3" : int(cp_cam.vl["RADAR_362"]["STATIC_3"]) + } + values_363 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(cp_cam.vl["RADAR_363"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_363"]["STATIC_2"]) + } + values_364 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(cp_cam.vl["RADAR_364"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_364"]["STATIC_2"]) + } + values_365 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(cp_cam.vl["RADAR_365"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_365"]["STATIC_2"]) + } + values_366 = { + "CTR" : int(cp_cam.vl["RADAR_361"]["CTR"]), + "STATIC_1" : int(cp_cam.vl["RADAR_366"]["STATIC_1"]), + "STATIC_2" : int(cp_cam.vl["RADAR_366"]["STATIC_2"]) + } + + ret.append(packer.make_can_msg("RADAR_361", 0, values_361)) + ret.append(packer.make_can_msg("RADAR_362", 0, values_362)) + ret.append(packer.make_can_msg("RADAR_363", 0, values_363)) + ret.append(packer.make_can_msg("RADAR_364", 0, values_364)) + ret.append(packer.make_can_msg("RADAR_365", 0, values_365)) + ret.append(packer.make_can_msg("RADAR_366", 0, values_366)) + + return ret diff --git a/selfdrive/global_ri.py b/selfdrive/global_ri.py new file mode 100644 index 000000000..6659e5410 --- /dev/null +++ b/selfdrive/global_ri.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 + +global active +global reset +global low_speed_mode +global radar_accel + +active = False +reset = False +low_speed_mode = False +radar_accel = 0 \ No newline at end of file