brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 13:48:05 -05:00
parent 2fbe9dbea1
commit 931db76fc6
432 changed files with 12973 additions and 3300 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
selfdrive/car/mazda/carcontroller.py
View File

@@ -1,13 +1,14 @@
from cereal import car
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.car.mazda import mazdacan
from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController:
class CarController(CarControllerBase):
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.apply_steer_last = 0
@@ -35,13 +36,13 @@ class CarController:
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
# Cancel Stock ACC if it's enabled while OP is disengaged
# Send at a rate of 10hz until we sync with stock ACC state
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL))
else:
self.brake_counter = 0
if CC.cruiseControl.resume and self.frame % 5 == 0:
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
# Send Resume button when planner wants car to move
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME))
self.apply_steer_last = apply_steer
@@ -54,7 +55,7 @@ class CarController:
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
# send steering command
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP,
self.frame, apply_steer, CS.cam_lkas))
new_actuators = CC.actuators.copy()

16
selfdrive/car/mazda/carstate.py
View File

@@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1
from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags
class CarState(CarStateBase):
def __init__(self, CP):
@@ -18,9 +18,16 @@ class CarState(CarStateBase):
self.lkas_allowed_speed = False
self.lkas_disabled = False
self.prev_distance_button = 0
self.distance_button = 0
def update(self, cp, cp_cam, frogpilot_variables):
ret = car.CarState.new_message()
self.prev_distance_button = self.distance_button
self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"]
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["FL"],
cp.vl["WHEEL_SPEEDS"]["FR"],
@@ -103,6 +110,9 @@ class CarState(CarStateBase):
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = not self.lkas_disabled
return ret
@staticmethod
@@ -116,7 +126,7 @@ class CarState(CarStateBase):
("WHEEL_SPEEDS", 100),
]
if CP.carFingerprint in GEN1:
if CP.flags & MazdaFlags.GEN1:
messages += [
("ENGINE_DATA", 100),
("CRZ_CTRL", 50),
@@ -136,7 +146,7 @@ class CarState(CarStateBase):
def get_cam_can_parser(CP):
messages = []
if CP.carFingerprint in GEN1:
if CP.flags & MazdaFlags.GEN1:
messages += [
# sig_address, frequency
("CAM_LANEINFO", 2),

2
selfdrive/car/mazda/fingerprints.py
View File

@@ -10,6 +10,7 @@ FW_VERSIONS = {
],
(Ecu.engine, 0x7e0, None): [
b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PW67-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -31,6 +32,7 @@ FW_VERSIONS = {
],
(Ecu.transmission, 0x7e1, None): [
b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PW66-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',

33
selfdrive/car/mazda/interface.py
View File

@@ -1,17 +1,18 @@
#!/usr/bin/env python3
from cereal import car
from cereal import car, custom
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
class CarInterface(CarInterfaceBase):
@staticmethod
def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs):
def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
ret.carName = "mazda"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
ret.radarUnavailable = True
@@ -20,27 +21,9 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 0.8
ret.tireStiffnessFactor = 0.70 # not optimized yet
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
if candidate in (CAR.CX5, CAR.CX5_2022):
ret.mass = 3655 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 15.5
elif candidate in (CAR.CX9, CAR.CX9_2021):
ret.mass = 4217 * CV.LB_TO_KG
ret.wheelbase = 3.1
ret.steerRatio = 17.6
elif candidate == CAR.MAZDA3:
ret.mass = 2875 * CV.LB_TO_KG
ret.wheelbase = 2.7
ret.steerRatio = 14.0
elif candidate == CAR.MAZDA6:
ret.mass = 3443 * CV.LB_TO_KG
ret.wheelbase = 2.83
ret.steerRatio = 15.5
if candidate not in (CAR.CX5_2022, ):
ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS
@@ -52,8 +35,14 @@ class CarInterface(CarInterfaceBase):
def _update(self, c, frogpilot_variables):
ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
# TODO: add button types for inc and dec
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events
events = self.create_common_events(ret, frogpilot_variables)
events = self.create_common_events(ret)
if self.CS.lkas_disabled:
events.add(EventName.lkasDisabled)

10
selfdrive/car/mazda/mazdacan.py
View File

@@ -1,7 +1,7 @@
from openpilot.selfdrive.car.mazda.values import GEN1, Buttons
from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags
def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
def create_steering_control(packer, CP, frame, apply_steer, lkas):
tmp = apply_steer + 2048
@@ -45,7 +45,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas):
csum = csum % 256
values = {}
if car_fingerprint in GEN1:
if CP.flags & MazdaFlags.GEN1:
values = {
"LKAS_REQUEST": apply_steer,
"CTR": ctr,
@@ -88,12 +88,12 @@ def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool)
return packer.make_can_msg("CAM_LANEINFO", 0, values)
def create_button_cmd(packer, car_fingerprint, counter, button):
def create_button_cmd(packer, CP, counter, button):
can = int(button == Buttons.CANCEL)
res = int(button == Buttons.RESUME)
if car_fingerprint in GEN1:
if CP.flags & MazdaFlags.GEN1:
values = {
"CAN_OFF": can,
"CAN_OFF_INV": (can + 1) % 2,

88
selfdrive/car/mazda/values.py
View File

@@ -1,10 +1,10 @@
from dataclasses import dataclass, field
from enum import StrEnum
from typing import Dict, List, Union
from enum import IntFlag
from cereal import car
from openpilot.selfdrive.car import dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
@@ -26,29 +26,60 @@ class CarControllerParams:
pass
class CAR(StrEnum):
CX5 = "MAZDA CX-5"
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
CX9_2021 = "MAZDA CX-9 2021"
CX5_2022 = "MAZDA CX-5 2022"
@dataclass
class MazdaCarInfo(CarInfo):
class MazdaCarDocs(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.mazda]))
CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {
CAR.CX5: MazdaCarInfo("Mazda CX-5 2017-21"),
CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"),
CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"),
CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"),
CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"),
CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"),
}
@dataclass(frozen=True, kw_only=True)
class MazdaCarSpecs(CarSpecs):
tireStiffnessFactor: float = 0.7 # not optimized yet
class MazdaFlags(IntFlag):
# Static flags
# Gen 1 hardware: same CAN messages and same camera
GEN1 = 1
@dataclass
class MazdaPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None))
flags: int = MazdaFlags.GEN1
class CAR(Platforms):
CX5 = MazdaPlatformConfig(
"MAZDA CX-5",
[MazdaCarDocs("Mazda CX-5 2017-21")],
MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5)
)
CX9 = MazdaPlatformConfig(
"MAZDA CX-9",
[MazdaCarDocs("Mazda CX-9 2016-20")],
MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6)
)
MAZDA3 = MazdaPlatformConfig(
"MAZDA 3",
[MazdaCarDocs("Mazda 3 2017-18")],
MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0)
)
MAZDA6 = MazdaPlatformConfig(
"MAZDA 6",
[MazdaCarDocs("Mazda 6 2017-20")],
MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5)
)
CX9_2021 = MazdaPlatformConfig(
"MAZDA CX-9 2021",
[MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")],
CX9.specs
)
CX5_2022 = MazdaPlatformConfig(
"MAZDA CX-5 2022",
[MazdaCarDocs("Mazda CX-5 2022-24")],
CX5.specs,
)
class LKAS_LIMITS:
@@ -76,15 +107,4 @@ FW_QUERY_CONFIG = FwQueryConfig(
],
)
DBC = {
CAR.CX5: dbc_dict('mazda_2017', None),
CAR.CX9: dbc_dict('mazda_2017', None),
CAR.MAZDA3: dbc_dict('mazda_2017', None),
CAR.MAZDA6: dbc_dict('mazda_2017', None),
CAR.CX9_2021: dbc_dict('mazda_2017', None),
CAR.CX5_2022: dbc_dict('mazda_2017', None),
}
# Gen 1 hardware: same CAN messages and same camera
GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022}
DBC = CAR.create_dbc_map()