diff --git a/opendbc_repo/opendbc/car/byd/carcontroller.py b/opendbc_repo/opendbc/car/byd/carcontroller.py index fea956a..5cd8bb0 100644 --- a/opendbc_repo/opendbc/car/byd/carcontroller.py +++ b/opendbc_repo/opendbc/car/byd/carcontroller.py @@ -4,7 +4,7 @@ from opendbc.car import Bus, structs from opendbc.car.lateral import apply_meas_steer_torque_limits from opendbc.car.interfaces import CarControllerBase from opendbc.car.byd.bydcan import BydCAN -from opendbc.car.byd.values import CarControllerParams +from opendbc.car.byd.values import CAR, CarControllerParams VisualAlert = structs.CarControl.HUDControl.VisualAlert ButtonType = structs.CarState.ButtonEvent.Type @@ -30,6 +30,11 @@ class CarController(CarControllerBase): self.apply_accel_last = 0 + # EPS steering feedback forwarding (792 STEERING_TORQUE) only applies to + # platforms whose EPS reports on 792 (HAN). The SEAL EPS reports on + # 508 STEERING_TORQUE_ANGLE which is forwarded stock for now. + self.send_steering_feedback = CP.carFingerprint != CAR.BYD_SEAL_PERFORMANCE_25 + def update(self, CC, CC_SP, CS, now_nanos): # car control running in 100Hz actuators = CC.actuators @@ -75,11 +80,12 @@ class CarController(CarControllerBase): lkas_request_prepare, lkas_active, lkas_mode, CS.eps_activated) can_sends.append(pack) - pack = self.can.create_steering_torque(CS.eps_steering_torque_msg, - CS.mpc_lkas_output, - CS.mpc_lkas_request_prepare, - CS.mpc_lkas_active) - can_sends.append(pack) + if self.send_steering_feedback: + pack = self.can.create_steering_torque(CS.eps_steering_torque_msg, + CS.mpc_lkas_output, + CS.mpc_lkas_request_prepare, + CS.mpc_lkas_active) + can_sends.append(pack) self.apply_torque_last = apply_torque diff --git a/opendbc_repo/opendbc/car/byd/carstate.py b/opendbc_repo/opendbc/car/byd/carstate.py index 3a47c82..6326bf6 100644 --- a/opendbc_repo/opendbc/car/byd/carstate.py +++ b/opendbc_repo/opendbc/car/byd/carstate.py @@ -3,7 +3,7 @@ from opendbc.car import Bus, create_button_events, structs from opendbc.can.parser import CANParser from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.byd.values import DBC, CANBUS, STEER_THRESHOLD, GEAR_MAP +from opendbc.car.byd.values import CAR, DBC, CANBUS, STEER_THRESHOLD, GEAR_MAP from opendbc.sunnypilot.car.byd.mads import MadsCarState GearShifter = structs.CarState.GearShifter @@ -15,6 +15,13 @@ class CarState(CarStateBase, MadsCarState): CarStateBase.__init__(self, CP, CP_SP) MadsCarState.__init__(self, CP, CP_SP) + # EPS steering feedback message differs by platform: + # HAN uses 792 STEERING_TORQUE, SEAL uses 508 STEERING_TORQUE_ANGLE + if CP.carFingerprint == CAR.BYD_SEAL_PERFORMANCE_25: + self.steer_msg = "STEERING_TORQUE_ANGLE" + else: + self.steer_msg = "STEERING_TORQUE" + self.mpc_lkas_cmd_msg = None self.eps_steering_torque_msg = None self.eps_prepared = False @@ -45,12 +52,12 @@ class CarState(CarStateBase, MadsCarState): # Steering wheel ret.steeringAngleDeg = cp.vl["STEER_MODULE"]["STEER_ANGLE"] ret.steeringRateDeg = cp.vl["STEER_MODULE"]["STEERING_RATE"] - ret.steeringTorque = cp.vl["STEERING_TORQUE"]["Steer_Torque_Sensor"] - ret.steeringTorqueEps = cp.vl["STEERING_TORQUE"]["MAIN_TORQUE"] + ret.steeringTorque = cp.vl[self.steer_msg]["Steer_Torque_Sensor"] + ret.steeringTorqueEps = cp.vl[self.steer_msg]["MAIN_TORQUE"] # TODO: looks like STEER_THRESHOLD is too small ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 5) - ret.steerFaultPermanent = cp.vl["STEERING_TORQUE"]["TORQUE_FAILED"] != 0 - ret.steerFaultTemporary = cp.vl["STEERING_TORQUE"]["TORQUE_FAILED"] != 0 + ret.steerFaultPermanent = cp.vl[self.steer_msg]["TORQUE_FAILED"] != 0 + ret.steerFaultTemporary = cp.vl[self.steer_msg]["TORQUE_FAILED"] != 0 ret.stockAeb = cp_cam.vl["ACC_HUD_ADAS"]["AEB"] == 1 ret.stockFcw = cp_cam.vl["ACC_HUD_ADAS"]["FCW"] == 1 # Forward Collision Warning @@ -87,10 +94,10 @@ class CarState(CarStateBase, MadsCarState): # Messages needed by carcontroller # for generate MPC_LKAS_CMD self.mpc_lkas_cmd_msg = copy.copy(cp_cam.vl["MPC_LKAS_CMD"]) - self.eps_prepared = cp.vl["STEERING_TORQUE"]["LKSPrepare"] != 0 - self.eps_activated = cp.vl["STEERING_TORQUE"]["Cruise_Activated"] != 0 - # for generate STEERING_TORQUE - self.eps_steering_torque_msg = copy.copy(cp.vl["STEERING_TORQUE"]) + self.eps_prepared = cp.vl[self.steer_msg]["LKSPrepare"] != 0 + self.eps_activated = cp.vl[self.steer_msg]["Cruise_Activated"] != 0 + # for generate steering feedback message + self.eps_steering_torque_msg = copy.copy(cp.vl[self.steer_msg]) self.mpc_lkas_output = cp_cam.vl["MPC_LKAS_CMD"]["LKAS_Output"] self.mpc_lkas_active = cp_cam.vl["MPC_LKAS_CMD"]["LKAS_ACTIVE"] != 0 self.mpc_lkas_request_prepare = cp_cam.vl["MPC_LKAS_CMD"]["LKASPrepare"] != 0 @@ -109,7 +116,6 @@ class CarState(CarStateBase, MadsCarState): ("BRAKE_APPLIED", 10), ("PEDAL", 10), ("STEER_MODULE", 10), - ("STEERING_TORQUE", 10), ("DRIVE_STATE", 10), ("STALKS", float('nan')), ("METER_CLUSTER", float('nan')), @@ -117,6 +123,15 @@ class CarState(CarStateBase, MadsCarState): ("PCM_BUTTONS", float('nan')), ] + # EPS steering feedback message differs by platform; alive-check only + # the one this car has, parse the other opportunistically + if CP.carFingerprint == CAR.BYD_SEAL_PERFORMANCE_25: + pt_messages.append(("STEERING_TORQUE_ANGLE", 10)) + pt_messages.append(("STEERING_TORQUE", float('nan'))) + else: + pt_messages.append(("STEERING_TORQUE", 10)) + pt_messages.append(("STEERING_TORQUE_ANGLE", float('nan'))) + cam_messages = [ ("ACC_HUD_ADAS", 10), # stock camera command echoes, only present when stock ADAS is active diff --git a/opendbc_repo/opendbc/safety/modes/byd.h b/opendbc_repo/opendbc/safety/modes/byd.h index e2a3389..eda0f36 100644 --- a/opendbc_repo/opendbc/safety/modes/byd.h +++ b/opendbc_repo/opendbc/safety/modes/byd.h @@ -3,9 +3,12 @@ // MPC_LKAS_CMD (790), little endian #define BYD_GET_LKAS_OUTPUT(msg) (to_signed((((msg)->data[3] & 0x7U) << 8U) | (msg)->data[2], 11)) #define BYD_LKAS_ACTIVE_BIT 28U -// STEERING_TORQUE (792), little endian +// STEERING_TORQUE (792, HAN), little endian #define BYD_GET_MAIN_TORQUE(msg) (to_signed((((msg)->data[2] & 0xFU) << 8U) | (msg)->data[1], 12)) #define BYD_GET_DRIVER_TORQUE(msg) (to_signed((((msg)->data[4] & 0xFU) << 8U) | (msg)->data[3], 12)) +// STEERING_TORQUE_ANGLE (508, SEAL), little endian +#define BYD_GET_MAIN_TORQUE_ANGLE(msg) (to_signed((((msg)->data[5] & 0xFU) << 8U) | (msg)->data[4], 12)) +#define BYD_GET_DRIVER_TORQUE_ANGLE(msg) (to_signed((((msg)->data[1] & 0xFU) << 4U) | ((msg)->data[0] >> 4U), 12)) // PCM_BUTTONS (944) #define BYD_LKAS_ON_BTN_BIT 14U @@ -73,7 +76,7 @@ static void byd_rx_hook(const CANPacket_t *msg) { if (msg->addr == 289U) { vehicle_moving = (((msg->data[1] & 0x0FU) << 8U) | msg->data[0]) != 0U; } - // STEERING_TORQUE: sample EPS output torque and driver torque + // STEERING_TORQUE (HAN): sample EPS output torque and driver torque if (msg->addr == 792U) { int torque_meas_new = BYD_GET_MAIN_TORQUE(msg); update_sample(&torque_meas, torque_meas_new); @@ -83,6 +86,17 @@ static void byd_rx_hook(const CANPacket_t *msg) { int torque_driver_new = BYD_GET_DRIVER_TORQUE(msg); update_sample(&torque_driver, torque_driver_new); } + // STEERING_TORQUE_ANGLE (SEAL): same feedback on a different message/layout + if (msg->addr == 508U) { + int torque_meas_new = BYD_GET_MAIN_TORQUE_ANGLE(msg); + update_sample(&torque_meas, torque_meas_new); + torque_meas.min--; + torque_meas.max++; + + // driver torque factor is 0.1 on this message + int torque_driver_new = (BYD_GET_DRIVER_TORQUE_ANGLE(msg) + 5) / 10; + update_sample(&torque_driver, torque_driver_new); + } // PCM_BUTTONS: dedicated LKAS steering wheel button for MADS if (msg->addr == 944U) { mads_button_press = GET_BIT(msg, BYD_LKAS_ON_BTN_BIT) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED; @@ -118,7 +132,8 @@ static safety_config byd_init(uint16_t param) { {.msg = {{834, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // PEDAL {.msg = {{289, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ESC {.msg = {{287, 0, 5, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // STEER_MODULE - {.msg = {{792, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // STEERING_TORQUE + {.msg = {{792, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, // STEERING_TORQUE (HAN) + {508, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, // STEERING_TORQUE_ANGLE (SEAL) {.msg = {{944, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // PCM_BUTTONS {.msg = {{813, 2, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // ACC_HUD_ADAS {.msg = {{307, 0, 8, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // STALKS diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index eda2dbb..1ba3cfe 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 618bf1a..1d4a2d3 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 991ed8b..ba75c54 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 478e118..db252b3 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 6c30fb2..262a818 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 92f70b0..fadf6ed 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index a63cbb8..3a2e313 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 14f9e0f..d7ac9af 100755 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-de77c89c-DEBUG"; +const uint8_t gitversion[19] = "DEV-264ebf95-DEBUG"; diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 905296b..98124b2 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index e18e9ac..d116ea1 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 5575cf9..35e9d35 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index dfd68f9..64cc791 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 83de861..2db86b2 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 08cac8f..f67a642 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 3ffd2c0..af987ec 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index 8b78e31..3e7ae43 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 389996d..aab7252 100755 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-de77c89c-DEBUG \ No newline at end of file +DEV-264ebf95-DEBUG \ No newline at end of file diff --git a/panda/tests/libpanda/libpanda.so b/panda/tests/libpanda/libpanda.so index 8807e78..4b0b22f 100755 Binary files a/panda/tests/libpanda/libpanda.so and b/panda/tests/libpanda/libpanda.so differ