spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Reduce paramsd and calibrationd CPU usage (#2119) 

* reduce paramsd cpu

* reduce calibrationd cpu usage

* calibration_helpers was mostly unused

* more calibration cleanup

* update refs

* fix thresholds in CPU test
pull/2153/head
Adeeb Shihadeh 2020-09-10 12:16:29 -07:00 committed by GitHub
parent 7cc5710974
commit e0004d0981
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 46 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
release/files_common
View File

@ -291,7 +291,6 @@ selfdrive/locationd/models/car_kf.py
selfdrive/locationd/models/constants.py
selfdrive/locationd/calibrationd.py
selfdrive/locationd/calibration_helpers.py
selfdrive/logcatd/SConscript
selfdrive/logcatd/logcatd_android.cc

2
selfdrive/controls/controlsd.py
View File

@ -21,7 +21,7 @@ from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.locationd.calibrationd import Calibration
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1

4
selfdrive/controls/lib/events.py
View File

@ -3,7 +3,7 @@ from functools import total_ordering
from cereal import log, car
from common.realtime import DT_CTRL
from selfdrive.config import Conversions as CV
from selfdrive.locationd.calibration_helpers import Filter
from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
@ -184,7 +184,7 @@ def below_steer_speed_alert(CP, sm, metric):
Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3)
def calibration_incomplete_alert(CP, sm, metric):
speed = int(Filter.MIN_SPEED * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
speed = int(MIN_SPEED_FILTER * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
unit = "km/h" if metric else "mph"
return Alert(
"Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc,

10
selfdrive/locationd/calibration_helpers.py
View File

@ -1,10 +0,0 @@
import math
class Filter:
MIN_SPEED = 7 # m/s (~15.5mph)
MAX_YAW_RATE = math.radians(3) # per second
class Calibration:
UNCALIBRATED = 0
CALIBRATED = 1
INVALID = 2

50
selfdrive/locationd/calibrationd.py
View File

@ -2,7 +2,7 @@
'''
This process finds calibration values. More info on what these calibration values
are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
While the roll calibration is a real value that can be estimated, here we assume it zero,
While the roll calibration is a real value that can be estimated, here we assume it's zero,
and the image input into the neural network is not corrected for roll.
'''
@ -12,7 +12,6 @@ import json
import numpy as np
import cereal.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog
from common.params import Params, put_nonblocking
from common.transformations.model import model_height
@ -34,6 +33,10 @@ PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
class Calibration:
UNCALIBRATED = 0
CALIBRATED = 1
INVALID = 2
def is_calibration_valid(rpy):
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
@ -47,7 +50,6 @@ def sanity_clip(rpy):
np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
class Calibrator():
def __init__(self, param_put=False):
self.param_put = param_put
@ -60,12 +62,9 @@ class Calibrator():
self.just_calibrated = False
self.v_ego = 0
# Read calibration
if param_put:
calibration_params = Params().get("CalibrationParams")
else:
calibration_params = None
if calibration_params:
# Read saved calibration
calibration_params = Params().get("CalibrationParams")
if param_put and calibration_params:
try:
calibration_params = json.loads(calibration_params)
self.rpy = calibration_params["calib_radians"]
@ -85,11 +84,7 @@ class Calibrator():
self.cal_status = Calibration.UNCALIBRATED
else:
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.rpy) else Calibration.INVALID
end_status = self.cal_status
self.just_calibrated = False
if start_status == Calibration.UNCALIBRATED and end_status != Calibration.UNCALIBRATED:
self.just_calibrated = True
self.just_calibrated = start_status == Calibration.UNCALIBRATED and self.cal_status != Calibration.UNCALIBRATED
def handle_v_ego(self, v_ego):
self.v_ego = v_ego
@ -115,6 +110,7 @@ class Calibrator():
self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
self.update_status()
# TODO: this should use the liveCalibration struct from cereal
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
cal_params = {"calib_radians": list(self.rpy),
"valid_blocks": self.valid_blocks}
@ -145,37 +141,29 @@ class Calibrator():
def calibrationd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['cameraOdometry', 'carState'])
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry'])
if pm is None:
pm = messaging.PubMaster(['liveCalibration'])
calibrator = Calibrator(param_put=True)
send_counter = 0
while 1:
sm.update()
# if no inputs still publish calibration
if not sm.updated['carState'] and not sm.updated['cameraOdometry']:
calibrator.send_data(pm)
continue
if sm.updated['carState']:
calibrator.handle_v_ego(sm['carState'].vEgo)
if send_counter % 25 == 0:
calibrator.send_data(pm)
send_counter += 1
sm.update(100)
if sm.updated['cameraOdometry']:
calibrator.handle_v_ego(sm['carState'].vEgo)
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
sm['cameraOdometry'].rot,
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].rotStd)
sm['cameraOdometry'].rot,
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].rotStd)
if DEBUG and new_rpy is not None:
print('got new rpy', new_rpy)
# 4Hz driven by cameraOdometry
if sm.frame % 5 == 0:
calibrator.send_data(pm)
def main(sm=None, pm=None):
calibrationd_thread(sm, pm)

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,

2
selfdrive/monitoring/dmonitoringd.py
View File

@ -6,7 +6,7 @@ from common.params import Params
import cereal.messaging as messaging
from selfdrive.controls.lib.events import Events
from selfdrive.monitoring.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.locationd.calibrationd import Calibration
def dmonitoringd_thread(sm=None, pm=None):

18
selfdrive/test/process_replay/process_replay.py
View File

@ -201,16 +201,10 @@ def calibration_rcv_callback(msg, CP, cfg, fsm):
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
# should_recv always true to increment frame
recv_socks = []
if msg.which() == 'carState' and ((fsm.frame + 1) % 25) == 0:
frame = fsm.frame + 1 # incrementing hasn't happened yet in SubMaster
if frame == 0 or (msg.which() == 'cameraOdometry' and (frame % 5) == 0):
recv_socks = ["liveCalibration"]
return recv_socks, msg.which() == 'carState'
def paramsd_rcv_callback(msg, CP, cfg, fsm):
recv_socks = []
if msg.which() == 'carState' and ((fsm.frame + 2) % 5) == 0:
recv_socks = ["liveParameters"]
return recv_socks, msg.which() == 'carState'
return recv_socks, fsm.frame == 0 or msg.which() == 'cameraOdometry'
CONFIGS = [
ProcessConfig(
@ -283,12 +277,12 @@ CONFIGS = [
ProcessConfig(
proc_name="paramsd",
pub_sub={
"carState": ["liveParameters"],
"liveLocationKalman": []
"liveLocationKalman": ["liveParameters"],
"carState": []
},
ignore=["logMonoTime", "valid"],
init_callback=get_car_params,
should_recv_callback=paramsd_rcv_callback,
should_recv_callback=None,
tolerance=NUMPY_TOLERANCE,
),
]

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
b47257005e7408737c66463c45f5db36153a849d
5c2c99c4572d68f17ffa3a33acf8e4e205ca94c2

4
selfdrive/test/test_cpu_usage.py
View File

@ -19,17 +19,17 @@ def print_cpu_usage(first_proc, last_proc):
("./loggerd", 33.90),
("selfdrive.locationd.locationd", 29.5),
("selfdrive.controls.plannerd", 11.84),
("selfdrive.locationd.paramsd", 11.53),
("selfdrive.locationd.paramsd", 10.5),
("./_modeld", 7.12),
("selfdrive.controls.radard", 9.54),
("./camerad", 7.07),
("./_sensord", 6.17),
("selfdrive.locationd.calibrationd", 6.0),
("./_ui", 5.82),
("./boardd", 3.63),
("./_dmonitoringmodeld", 2.67),
("selfdrive.logmessaged", 2.71),
("selfdrive.thermald.thermald", 2.41),
("selfdrive.locationd.calibrationd", 2.0),
("selfdrive.monitoring.dmonitoringd", 1.90),
("./proclogd", 1.54),
("./_gpsd", 0.09),