mock: remove steering angle (#26614)

* fix yaw rate in mock interface

* clean that up

* revert

* clean up
pull/26639/head
Shane Smiskol 3 years ago committed by GitHub
parent f64e69eb11
commit b4da592a3c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 27
      selfdrive/car/mock/interface.py

@ -1,31 +1,22 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import math
from cereal import car from cereal import car
from common.conversions import Conversions as CV
from system.swaglog import cloudlog from system.swaglog import cloudlog
import cereal.messaging as messaging import cereal.messaging as messaging
from selfdrive.car import gen_empty_fingerprint, get_safety_config from selfdrive.car import gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase 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): class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState): def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState) super().__init__(CP, CarController, CarState)
cloudlog.debug("Using Mock Car Interface") cloudlog.debug("Using Mock Car Interface")
self.sm = messaging.SubMaster(['gyroscope', 'gpsLocation', 'gpsLocationExternal']) self.sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
self.speed = 0. self.speed = 0.
self.prev_speed = 0. self.prev_speed = 0.
self.yaw_rate = 0.
self.yaw_rate_meas = 0.
@staticmethod @staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, experimental_long=False): 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 # returns a car.CarState
def _update(self, c): def _update(self, c):
self.sm.update(0) 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' gps_sock = 'gpsLocationExternal' if self.sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
if self.sm.updated[gps_sock]: if self.sm.updated[gps_sock]:
self.prev_speed = self.speed self.prev_speed = self.speed
@ -61,10 +47,9 @@ class CarInterface(CarInterfaceBase):
# speeds # speeds
ret.vEgo = self.speed ret.vEgo = self.speed
ret.vEgoRaw = self.speed ret.vEgoRaw = self.speed
a = self.speed - self.prev_speed
ret.aEgo = a ret.aEgo = self.speed - self.prev_speed
ret.brakePressed = a < -0.5 ret.brakePressed = ret.aEgo < -0.5
ret.standstill = self.speed < 0.01 ret.standstill = self.speed < 0.01
ret.wheelSpeeds.fl = self.speed ret.wheelSpeeds.fl = self.speed
@ -72,10 +57,6 @@ class CarInterface(CarInterfaceBase):
ret.wheelSpeeds.rl = self.speed ret.wheelSpeeds.rl = self.speed
ret.wheelSpeeds.rr = 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 return ret
def apply(self, c): def apply(self, c):

Loading…
Cancel
Save