brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 03:26:25 -05:00
parent 90b100e98a
commit 9bb33c11ac
20 changed files with 1541 additions and 2670 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
selfdrive/locationd/calibrationd.py
View File

@@ -10,7 +10,7 @@ import gc
import os
import capnp
import numpy as np
from typing import List, NoReturn, Optional
from typing import NoReturn
from cereal import log
import cereal.messaging as messaging
@@ -89,7 +89,7 @@ class Calibrator:
valid_blocks: int = 0,
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
height_init: np.ndarray = HEIGHT_INIT,
smooth_from: Optional[np.ndarray] = None) -> None:
smooth_from: np.ndarray = None) -> None:
if not np.isfinite(rpy_init).all():
self.rpy = RPY_INIT.copy()
else:
@@ -125,7 +125,7 @@ class Calibrator:
self.old_rpy = smooth_from
self.old_rpy_weight = 1.0
def get_valid_idxs(self) -> List[int]:
def get_valid_idxs(self) -> list[int]:
# exclude current block_idx from validity window
before_current = list(range(self.block_idx))
after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
@@ -175,12 +175,12 @@ class Calibrator:
else:
return self.rpy
def handle_cam_odom(self, trans: List[float],
rot: List[float],
wide_from_device_euler: List[float],
trans_std: List[float],
road_transform_trans: List[float],
road_transform_trans_std: List[float]) -> Optional[np.ndarray]:
def handle_cam_odom(self, trans: list[float],
rot: list[float],
wide_from_device_euler: list[float],
trans_std: list[float],
road_transform_trans: list[float],
road_transform_trans_std: list[float]) -> np.ndarray | None:
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))