diff --git a/release/files_common b/release/files_common index 7f8920d023..3389ae1a4f 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 58d78d2dbf..7b0f54c0db 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index f9c962ec3a..6ab1fb02a9 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -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, diff --git a/selfdrive/locationd/calibration_helpers.py b/selfdrive/locationd/calibration_helpers.py deleted file mode 100644 index c7886d3019..0000000000 --- a/selfdrive/locationd/calibration_helpers.py +++ /dev/null @@ -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 diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 7cca1c535f..84ff2df4ca 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -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) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index ad820b4ea2..1d1d353315 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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, diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index fdfe05c3a9..9fd0e0ae44 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -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): diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index d071bcf07c..3b05b3871f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -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, ), ] diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 43c1bc4843..2ebb086ef3 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b47257005e7408737c66463c45f5db36153a849d \ No newline at end of file +5c2c99c4572d68f17ffa3a33acf8e4e205ca94c2 \ No newline at end of file diff --git a/selfdrive/test/test_cpu_usage.py b/selfdrive/test/test_cpu_usage.py index 98f6284fde..4fffc0d1d5 100755 --- a/selfdrive/test/test_cpu_usage.py +++ b/selfdrive/test/test_cpu_usage.py @@ -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),