mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-07 01:06:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

Road Roll Compensation Rebased (#23251)

Browse Source 
* first commit

* update refs
This commit is contained in:
HaraldSchafer
2021-12-16 17:34:12 -08:00
committed by GitHub
parent 285addeef2
commit cf466222f6
13 changed files with 177 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/paramsd.py
+30 -8
View File
@@ -16,10 +16,12 @@ from selfdrive.swaglog import cloudlog
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
@@ -30,9 +32,10 @@ class ParamsLearner:
self.active = False
self.speed = 0
self.speed = 0.0
self.roll = 0.0
self.steering_pressed = False
self.steering_angle = 0
self.steering_angle = 0.0
self.valid = True
@@ -41,16 +44,34 @@ class ParamsLearner:
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
if roll_valid:
roll = localizer_roll
roll_std = np.radians(1.0)
else:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
roll_std = np.radians(10.0)
self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
yaw_rate_valid = msg.angularVelocityCalibrated.valid
yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
if self.active:
if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
if msg.inputsOK and msg.posenetOK:
if yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
ObservationKind.ROAD_ROLL,
np.array([[self.roll]]),
np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
elif which == 'carState':
@@ -152,6 +173,7 @@ def main(sm=None, pm=None):
msg.liveParameters.sensorValid = True
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.roll = float(x[States.ROAD_ROLL])
msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
msg.liveParameters.angleOffsetDeg = angle_offset
msg.liveParameters.valid = all((