mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-06 05:52:12 +08:00
Code Issues Packages Projects Releases Wiki Activity

Toyota: don't calculate angle offset when angle rate is high (#30277)

Browse Source 
* comment

* debug

* clean up
old-commit-hash: c32927de9688d506098ada51dedcf8e267abc8bd
This commit is contained in:
Shane Smiskol
2023-10-18 03:59:39 -07:00
committed by GitHub
parent 8f4e2f63f0
commit 9da1c71bfd
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/toyota/carstate.py
+3 -4
View File
@@ -74,6 +74,7 @@ class CarState(CarStateBase):
ret.standstill = ret.vEgoRaw == 0
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# On some cars, the angle measurement is non-zero while initializing
@@ -81,16 +82,14 @@ class CarState(CarStateBase):
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
# Offset seems to be invalid for large steering angles
if abs(ret.steeringAngleDeg) < 90 and cp.can_valid:
# Offset seems to be invalid for large steering angles and high angle rates
if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid:
self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
if self.angle_offset.initialized:
ret.steeringAngleOffsetDeg = self.angle_offset.x
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1