mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 00:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Calibrationd: make recalibrating alert (#28149)

Browse Source 
* 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>
old-commit-hash: 598343aad1
This commit is contained in:
Harald Schäfer
2023-05-15 09:29:57 -07:00
committed by GitHub
parent 3cc0b6e7f2
commit 5ddceb553d
12 changed files with 51 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

Submodule cereal updated: 8902e7a778...2c22c7b00b

9
selfdrive/controls/controlsd.py
View File

@@ -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

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

@@ -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"),

21
selfdrive/locationd/calibrationd.py
View File

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

2
selfdrive/locationd/locationd.cc
View File

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

2
selfdrive/locationd/test/test_calibrationd.py
View File

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

4
selfdrive/monitoring/dmonitoringd.py
View File

@@ -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

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
b4a208cb84a99eb15116d2a445c278b00275fdd5
8bbb5436ef66dffbe57c3ac5e9b91d262f3f412b

1
selfdrive/test/process_replay/test_fuzzy.py
View File

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

36
selfdrive/test/process_replay/test_processes.py
View File

@@ -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"]

2
selfdrive/ui/qt/offroad/settings.cc
View File

@@ -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.")

2
selfdrive/ui/ui.cc
View File

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