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

paramsd: Add condition to sensor valid check (#30066)

Browse Source 
add condition to sensorvalid check
This commit is contained in:
Vivek Aithal
2023-09-27 21:00:36 -07:00
committed by GitHub
parent 86a334ae19
commit 0461ea8a82
1 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/paramsd.py
+9 -3
View File
@@ -23,6 +23,8 @@ ROLL_STD_MAX = math.radians(1.5)
LATERAL_ACC_SENSOR_THRESHOLD = 4.0
OFFSET_MAX = 10.0
OFFSET_LOWERED_MAX = 8.0
MIN_ACTIVE_SPEED = 1.0
LOW_ACTIVE_SPEED = 10.0
class ParamsLearner:
@@ -95,7 +97,7 @@ class ParamsLearner:
self.speed = msg.vEgo
in_linear_region = abs(self.steering_angle) < 45
self.active = self.speed > 1 and in_linear_region
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
@@ -204,8 +206,12 @@ def main(sm=None, pm=None):
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL].item())
# Account for the opposite signs of the yaw rates
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
if learner.active and learner.speed > LOW_ACTIVE_SPEED:
# Account for the opposite signs of the yaw rates
# At low speeds, bumping into a curb can cause the yaw rate to be very high
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
else:
sensors_valid = True
avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)