mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-03-02 09:03:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Reduce paramsd and calibrationd CPU usage (#2119)

Browse Source 
* reduce paramsd cpu

* reduce calibrationd cpu usage

* calibration_helpers was mostly unused

* more calibration cleanup

* update refs

* fix thresholds in CPU test
This commit is contained in:
Adeeb Shihadeh
2020-09-10 12:16:29 -07:00
committed by GitHub
parent 7cc5710974
commit e0004d0981
10 changed files with 46 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
selfdrive/locationd/paramsd.py
View File

@@ -13,8 +13,6 @@ from selfdrive.swaglog import cloudlog
KalmanStatus = log.LiveLocationKalman.Status
CARSTATE_DECIMATION = 5
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
@@ -32,7 +30,6 @@ class ParamsLearner:
self.speed = 0
self.steering_pressed = False
self.steering_angle = 0
self.carstate_counter = 0
self.valid = True
@@ -51,18 +48,16 @@ class ParamsLearner:
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
elif which == 'carState':
self.carstate_counter += 1
if self.carstate_counter % CARSTATE_DECIMATION == 0:
self.steering_angle = msg.steeringAngle
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
self.steering_angle = msg.steeringAngle
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
@@ -72,7 +67,7 @@ class ParamsLearner:
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
@@ -111,12 +106,11 @@ def main(sm=None, pm=None):
sm.update()
for which, updated in sm.updated.items():
if not updated:
continue
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if updated:
t = sm.logMonoTime[which] * 1e-9
learner.handle_log(t, which, sm[which])
if sm.updated['carState'] and (learner.carstate_counter % CARSTATE_DECIMATION == 0):
if sm.updated['liveLocationKalman']:
msg = messaging.new_message('liveParameters')
msg.logMonoTime = sm.logMonoTime['carState']
@@ -135,7 +129,7 @@ def main(sm=None, pm=None):
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
if learner.carstate_counter % 6000 == 0: # once a minute
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,