diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a3194cd79e..2be1bcb4a3 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,31 +1,22 @@ #!/usr/bin/env python3 -import math from cereal import car -from common.conversions import Conversions as CV from system.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -# mocked car interface to work with chffrplus -TS = 0.01 # 100Hz -YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter -# low pass gain -LPG = 2 * math.pi * YAW_FR * TS / (1 + 2 * math.pi * YAW_FR * TS) - +# mocked car interface to work with chffrplus class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) cloudlog.debug("Using Mock Car Interface") - self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) + self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) self.speed = 0. self.prev_speed = 0. - self.yaw_rate = 0. - self.yaw_rate_meas = 0. @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): @@ -45,11 +36,6 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): self.sm.update(0) - - # get basic data from phone and gps since CAN isn't connected - if self.sm.updated['gyroscope']: - self.yaw_rate_meas = -self.sm['gyroscope'].gyroUncalibrated.v[0] - gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation' if self.sm.updated[gps_sock]: self.prev_speed = self.speed @@ -61,10 +47,9 @@ class CarInterface(CarInterfaceBase): # speeds ret.vEgo = self.speed ret.vEgoRaw = self.speed - a = self.speed - self.prev_speed - ret.aEgo = a - ret.brakePressed = a < -0.5 + ret.aEgo = self.speed - self.prev_speed + ret.brakePressed = ret.aEgo < -0.5 ret.standstill = self.speed < 0.01 ret.wheelSpeeds.fl = self.speed @@ -72,10 +57,6 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rr = self.speed - self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate - curvature = self.yaw_rate / max(self.speed, 1.) - ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG - return ret def apply(self, c):