mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-01 03:43:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

paramsd: fix unbounded yaw rate while calibrating (#34806)

Browse Source 
* rm

* comments

* default values when yaw rate invalid

* clean up

Revert "default values when yaw rate invalid"

This reverts commit e983abb3b009f15a57ebdfbadd4f616aba5b266e.

do the same for yaw rate we do for roll

and

* 1 is fine

* update refs

---------

Co-authored-by: Kacper Rączy <gfw.kra@gmail.com>
This commit is contained in:
Shane Smiskol
2025-03-07 00:44:09 -06:00
committed by GitHub
parent e61a4ac76e
commit 0789877cd3
3 changed files with 17 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/paramsd.py
+15 -14
View File
@@ -46,18 +46,25 @@ class ParamsLearner:
self.yaw_rate_std = 0.0
self.roll = 0.0
self.steering_angle = 0.0
self.roll_valid = False
def handle_log(self, t, which, msg):
if which == 'livePose':
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s
if yaw_rate_valid:
self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
else:
# This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
self.yaw_rate, self.yaw_rate_std = 0.0, np.radians(1)
localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
self.roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
if self.roll_valid:
roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
if roll_valid:
roll = localizer_roll
# Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
roll_std = 2 * localizer_roll_std
@@ -67,18 +74,12 @@ class ParamsLearner:
roll_std = np.radians(10.0)
self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s
if self.active:
if msg.posenetOK:
if yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-self.yaw_rate]]),
np.array([np.atleast_2d(self.yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-self.yaw_rate]]),
np.array([np.atleast_2d(self.yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL,