mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Lateral Acceleration Error alert (Sensor Invalid) (#25291)

Browse Source 
* sensor flag in paramsd

* increase lateral acc err alert threshold

* add invalid sensor alert from paramsd

* update ref, bool before adding to msg

* account for sign difference in locationd and paramsd

* revert ref
old-commit-hash: e77dc1ab444ce41749d287337ace74f32170cd65
This commit is contained in:
Vivek Aithal
2022-07-28 13:10:01 -07:00
committed by GitHub
parent 1cc3755472
commit e23f58937f
2 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/controlsd.py
View File

@@ -347,7 +347,7 @@ class Controls:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['lateralPlan'].mpcSolutionValid:
self.events.add(EventName.plannerError)
if not self.sm['liveLocationKalman'].sensorsOK and not NOSENSOR:
if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR:
if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs
self.events.add(EventName.sensorDataInvalid)
if not self.sm['liveLocationKalman'].posenetOK:

20
selfdrive/locationd/paramsd.py
View File

@@ -16,6 +16,8 @@ from system.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)
LATERAL_ACC_SENSOR_THRESHOLD = 4.0
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
@@ -31,6 +33,8 @@ class ParamsLearner:
self.active = False
self.speed = 0.0
self.yaw_rate = 0.0
self.yaw_rate_std = 0.0
self.roll = 0.0
self.steering_pressed = False
self.steering_angle = 0.0
@@ -39,8 +43,8 @@ class ParamsLearner:
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
self.yaw_rate = msg.angularVelocityCalibrated.value[2]
self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
@@ -56,8 +60,8 @@ class ParamsLearner:
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
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:
@@ -65,8 +69,8 @@ class ParamsLearner:
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)]))
np.array([[-self.yaw_rate]]),
np.array([np.atleast_2d(self.yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL,
@@ -173,12 +177,14 @@ def main(sm=None, pm=None):
angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET]), angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
# Account for the opposite signs of the yaw rates
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE] + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
msg = messaging.new_message('liveParameters')
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = True
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO])
liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
liveParameters.roll = float(x[States.ROAD_ROLL])