mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 11:13:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

mock: remove steering angle (#26614)

Browse Source 
* fix yaw rate in mock interface

* clean that up

* revert

* clean up
This commit is contained in:
Shane Smiskol
2022-11-29 01:54:59 -08:00
committed by GitHub
parent f64e69eb11
commit b4da592a3c
1 changed files with 4 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
selfdrive/car/mock/interface.py
View File

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