diff --git a/cereal b/cereal index 8902e7a778..2c22c7b00b 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 8902e7a77815ca08e795b3006274d02379da7ca6 +Subproject commit 2c22c7b00ba6797734e9520e728cbf58b33a82ce diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f40fc6accf..96109d345c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -26,7 +26,6 @@ from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.locationd.calibrationd import Calibration from system.hardware import HARDWARE SOFT_DISABLE_TIME = 3 # seconds @@ -283,9 +282,11 @@ class Controls: # Handle calibration status cal_status = self.sm['liveCalibration'].calStatus - if cal_status != Calibration.CALIBRATED: - if cal_status == Calibration.UNCALIBRATED: + if cal_status != log.LiveCalibrationData.Status.calibrated: + if cal_status == log.LiveCalibrationData.Status.uncalibrated: self.events.add(EventName.calibrationIncomplete) + elif cal_status == log.LiveCalibrationData.Status.recalibrating: + self.events.add(EventName.calibrationRecalibrating) else: self.events.add(EventName.calibrationInvalid) @@ -697,7 +698,7 @@ class Controls: recent_blinker = (self.sm.frame - self.last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ - and not CC.latActive and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED + and not CC.latActive and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 12b78d7ee3..b935e24615 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -242,8 +242,9 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + first_word = 'Recalibration' if sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibration' return Alert( - "Calibration in Progress: %d%%" % sm['liveCalibration'].calPerc, + f"{first_word} in Progress: {sm['liveCalibration'].calPerc:.0f}%", f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) @@ -720,9 +721,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.calibrationIncomplete: { ET.PERMANENT: calibration_incomplete_alert, - ET.SOFT_DISABLE: soft_disable_alert("Device remount detected: recalibrating"), + ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), }, + + EventName.calibrationRecalibrating: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), + ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), + }, EventName.doorOpen: { ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 091f211175..dc4ffbb1f7 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -39,12 +39,6 @@ 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: np.ndarray) -> bool: return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore @@ -69,6 +63,7 @@ class Calibrator: rpy_init = RPY_INIT wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 + self.cal_status = log.LiveCalibrationData.Status.uncalibrated if param_put and calibration_params: try: @@ -134,16 +129,20 @@ class Calibrator: self.calib_spread = np.zeros(3) if self.valid_blocks < INPUTS_NEEDED: - self.cal_status = Calibration.UNCALIBRATED + if self.cal_status == log.LiveCalibrationData.Status.recalibrating: + self.cal_status = log.LiveCalibrationData.Status.recalibrating + else: + self.cal_status = log.LiveCalibrationData.Status.uncalibrated elif is_calibration_valid(self.rpy): - self.cal_status = Calibration.CALIBRATED + self.cal_status = log.LiveCalibrationData.Status.calibrated else: - self.cal_status = Calibration.INVALID + self.cal_status = log.LiveCalibrationData.Status.invalid # If spread is too high, assume mounting was changed and reset to last block. # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. - if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED: + if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == log.LiveCalibrationData.Status.calibrated: self.reset(self.rpys[self.block_idx - 1], valid_blocks=1, smooth_from=self.rpy) + self.cal_status = log.LiveCalibrationData.Status.recalibrating write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: @@ -210,7 +209,7 @@ class Calibrator: if self.not_car: liveCalibration.validBlocks = INPUTS_NEEDED - liveCalibration.calStatus = Calibration.CALIBRATED + liveCalibration.calStatus = log.LiveCalibrationData.Status.calibrated liveCalibration.calPerc = 100. liveCalibration.rpyCalib = [0, 0, 0] liveCalibration.rpyCalibSpread = self.calib_spread.tolist() diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 6c0307bd64..9ee9e33fb4 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -507,7 +507,7 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra this->calib = live_calib; this->device_from_calib = euler2rot(this->calib); this->calib_from_device = this->device_from_calib.transpose(); - this->calibrated = log.getCalStatus() == 1; + this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; this->observation_values_invalid["liveCalibration"] *= DECAY; } } diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py index 2c6508fd42..18125cc43f 100755 --- a/selfdrive/locationd/test/test_calibrationd.py +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -5,6 +5,7 @@ import unittest import numpy as np import cereal.messaging as messaging +from cereal import log from common.params import Params from selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER, SMOOTH_CYCLES @@ -96,6 +97,7 @@ class TestCalibrationd(unittest.TestCase): [0.0, 0.0, 0.0], [1e-3, 1e-3, 1e-3]) self.assertEqual(c.valid_blocks, 1) + self.assertEqual(c.cal_status, log.LiveCalibrationData.Status.recalibrating) np.testing.assert_allclose(c.rpy, [0.0, 0.0, -0.05], atol=1e-2) if __name__ == "__main__": diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 35eee5b035..836ed9cc4f 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -3,10 +3,10 @@ import gc import cereal.messaging as messaging from cereal import car +from cereal import log from common.params import Params, put_bool_nonblocking from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events -from selfdrive.locationd.calibrationd import Calibration from selfdrive.monitoring.driver_monitor import DriverStatus @@ -22,7 +22,7 @@ def dmonitoringd_thread(sm=None, pm=None): driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) - sm['liveCalibration'].calStatus = Calibration.INVALID + sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid sm['liveCalibration'].rpyCalib = [0, 0, 0] sm['carState'].buttonEvents = [] sm['carState'].standstill = True diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8e4d9f5afc..a7fb186c16 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b4a208cb84a99eb15116d2a445c278b00275fdd5 \ No newline at end of file +8bbb5436ef66dffbe57c3ac5e9b91d262f3f412b \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py index 28e0f70589..29fdf641f1 100755 --- a/selfdrive/test/process_replay/test_fuzzy.py +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -92,7 +92,6 @@ def get_strategy_for_events(event_types, finite=False): 'speedAccuracy': floats(width=32), }) r['LiveCalibration'] = st.fixed_dictionaries({ - 'calStatus': st.integers(min_value=0, max_value=1), 'rpyCalib': st.lists(floats(width=32), min_size=3, max_size=3), }) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index ecb2bb02ca..deed1ec48f 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -40,24 +40,24 @@ source_segments = [ ] segments = [ - ("BODY", "regenFA002A80700|2022-09-27--15-37-02--0"), - ("HYUNDAI", "regenBE53A59065B|2022-09-27--16-52-03--0"), - ("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), - ("TOYOTA", "regen929C5790007|2022-09-27--16-27-47--0"), - ("TOYOTA2", "regenEA3950D7F22|2022-09-27--15-43-24--0"), - ("TOYOTA3", "regen89026F6BD8D|2022-09-27--15-45-37--0"), - ("HONDA", "regenC7D5645EB17|2022-09-27--15-47-29--0"), - ("HONDA2", "regenCC2ECCE5742|2022-09-27--16-18-01--0"), - ("CHRYSLER", "regenC253C4DAC90|2022-09-27--15-51-03--0"), - ("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), - ("SUBARU", "regen1E72BBDCED5|2022-09-27--15-55-31--0"), - ("GM", "regen45B05A80EF6|2022-09-27--15-57-22--0"), - ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), - ("NISSAN", "regenC19D899B46D|2022-09-27--15-59-13--0"), - ("VOLKSWAGEN", "regenD8F7AC4BD0D|2022-09-27--16-41-45--0"), - ("MAZDA", "regenFC3F9ECBB64|2022-09-27--16-03-09--0"), - ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), -] + ("BODY", "aregenECF15D9E559|2023-05-10--14-26-40--0"), + ("HYUNDAI", "aregenAB9F543F70A|2023-05-10--14-28-25--0"), + ("HYUNDAI2", "aregen39F5A028F96|2023-05-10--14-31-00--0"), + ("TOYOTA", "aregen8D6A8B36E8D|2023-05-10--14-32-38--0"), + ("TOYOTA2", "aregenB1933C49809|2023-05-10--14-34-14--0"), + ("TOYOTA3", "aregen5D9915223DC|2023-05-10--14-36-43--0"), + ("HONDA", "aregen484B732B675|2023-05-10--14-38-23--0"), + ("HONDA2", "aregenAF6ACED4713|2023-05-10--14-40-01--0"), + ("CHRYSLER", "aregen99B094E1E2E|2023-05-10--14-41-40--0"), + ("RAM", "aregen5C2487E1EEB|2023-05-10--14-44-09--0"), + ("SUBARU", "aregen98D277B792E|2023-05-10--14-46-46--0"), + ("GM", "aregen377BA28D848|2023-05-10--14-48-28--0"), + ("GM2", "aregen7CA0CC0F0C2|2023-05-10--14-51-00--0"), + ("NISSAN", "aregen7097BF01563|2023-05-10--14-52-43--0"), + ("VOLKSWAGEN", "aregen765AF3D2CB5|2023-05-10--14-54-23--0"), + ("MAZDA", "aregen3053762FF2E|2023-05-10--14-56-53--0"), + ("FORD", "aregenDDE0F89FA1E|2023-05-10--14-59-26--0"), + ] # dashcamOnly makes don't need to be tested until a full port is done excluded_interfaces = ["mock", "mazda", "tesla"] diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index fd14ed15e2..bc4a10fd05 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -273,7 +273,7 @@ void DevicePanel::updateCalibDescription() { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size())); auto calib = cmsg.getRoot().getLiveCalibration(); - if (calib.getCalStatus() != 0) { + if (calib.getCalStatus() != cereal::LiveCalibrationData::Status::UNCALIBRATED) { double pitch = calib.getRpyCalib()[1] * (180 / M_PI); double yaw = calib.getRpyCalib()[2] * (180 / M_PI); desc += tr(" Your device is pointed %1° %2 and %3° %4.") diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 50ffc84f4f..f60ba51ce4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -179,7 +179,7 @@ static void update_state(UIState *s) { scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j); } } - scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; + scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED; scene.calibration_wide_valid = wfde_list.size() == 3; } if (sm.updated("pandaStates")) {