mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-24 01:33:46 +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: 598343aad18a7896c6b65ff6e19f9e27ba19cc10
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
selfdrive/locationd/calibrationd.py
+10 -11
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()
selfdrive/locationd/locationd.cc
+1 -1
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;
}
}
selfdrive/locationd/test/test_calibrationd.py
+2
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__":