Calibrationd: make recalibrating alert (#28149)

* Initial

* fixes

* not an int anymore

* elif

* revert ref

* update ref

* fix alert text

* regen refs

* update ref

* add recalibration unit test

* set into recalibration state

* fix words

* recalib

* text

* Update selfdrive/controls/lib/events.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

cereal

@@ -1 +1 @@
-Subproject commit 8902e7a77815ca08e795b3006274d02379da7ca6
+Subproject commit 2c22c7b00ba6797734e9520e728cbf58b33a82ce

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

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,10 +721,16 @@ 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"),
     ET.NO_ENTRY: NoEntryAlert("Door Open"),

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()

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;
   }
 }

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__":

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

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b4a208cb84a99eb15116d2a445c278b00275fdd5
+8bbb5436ef66dffbe57c3ac5e9b91d262f3f412b

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),
   })
 

selfdrive/test/process_replay/test_processes.py

@@ -40,23 +40,23 @@ 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

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<cereal::Event>().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.")

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")) {