37 lines
1.2 KiB
Python
37 lines
1.2 KiB
Python
from iqdbc.can import CANParser
|
|
from iqdbc.car import Bus, structs
|
|
from iqdbc.car.interfaces import CarStateBase
|
|
from iqdbc.car.body.values import DBC
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
|
cp = can_parsers[Bus.main]
|
|
ret = structs.CarState()
|
|
ret_iq = structs.IQCarState()
|
|
|
|
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
|
|
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
|
|
|
|
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
|
|
|
|
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
|
ret.standstill = False
|
|
|
|
ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'],
|
|
cp.vl['VAR_VALUES']['FAULT']])
|
|
|
|
ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1
|
|
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100
|
|
|
|
# irrelevant for non-car
|
|
ret.gearShifter = structs.CarState.GearShifter.drive
|
|
ret.cruiseState.enabled = True
|
|
ret.cruiseState.available = True
|
|
|
|
return ret, ret_iq
|
|
|
|
@staticmethod
|
|
def get_can_parsers(CP, CP_IQ):
|
|
return {Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0)}
|