mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-24 21:12:05 +08:00
IQ.Lvbs CI [bot]
29 lines
831 B
Python
29 lines
831 B
Python
import math
|
|
|
|
import numpy as np
|
|
from cereal import log
|
|
|
|
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
|
|
|
|
|
def get_calibrated_rpy(live_calib: log.LiveCalibrationData) -> np.ndarray | None:
|
|
if live_calib.calStatus != log.LiveCalibrationData.Status.calibrated:
|
|
return None
|
|
|
|
if len(live_calib.rpyCalib) != 3:
|
|
return None
|
|
|
|
calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
|
|
return calib_rpy if np.isfinite(calib_rpy).all() else None
|
|
|
|
|
|
def get_render_path_height(live_calib: log.LiveCalibrationData) -> float:
|
|
if live_calib.calStatus != log.LiveCalibrationData.Status.calibrated:
|
|
return float(HEIGHT_INIT[0])
|
|
|
|
if len(live_calib.height) != 1:
|
|
return float(HEIGHT_INIT[0])
|
|
|
|
height = float(live_calib.height[0])
|
|
return height if math.isfinite(height) else float(HEIGHT_INIT[0])
|