|
|
|
@ -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): |
|
|
|
|