Merge branch 'master-r' into multilang-ja-20250321

# Conflicts:
#	opendbc_repo
#	panda
pull/34986/head
ichiroMax 3 weeks ago
commit 5d1d962487
  1. 3
      .github/workflows/auto_pr_review.yaml
  2. 10
      .github/workflows/selfdrive_tests.yaml
  3. 2
      .github/workflows/ui_preview.yaml
  4. 16
      cereal/log.capnp
  5. 2
      msgq_repo
  6. 2
      opendbc_repo
  7. 2
      panda
  8. 2
      rednose_repo
  9. 10
      selfdrive/locationd/calibrationd.py
  10. 25
      selfdrive/locationd/models/car_kf.py
  11. 337
      selfdrive/locationd/paramsd.py
  12. 57
      selfdrive/locationd/test/test_paramsd.py
  13. 10
      selfdrive/selfdrived/selfdrived.py
  14. 8
      selfdrive/test/process_replay/process_replay.py
  15. 2
      selfdrive/test/process_replay/ref_commit
  16. 1
      selfdrive/test/test_onroad.py
  17. 58
      selfdrive/ui/translations/main_th.ts
  18. 4
      system/camerad/sensors/ox03c10_registers.h
  19. 2
      tinygrad_repo
  20. 1251
      uv.lock

@ -1,7 +1,7 @@
name: "PR review"
on:
pull_request_target:
types: [opened, reopened, synchronize, edited, edited]
types: [opened, reopened, synchronize, edited]
jobs:
labeler:
@ -50,4 +50,3 @@ jobs:
* all the tests are passing
* the change is [something we merge](https://github.com/commaai/openpilot/blob/master/docs/CONTRIBUTING.md#what-gets-merged)
* include a route or your device' dongle ID if relevant

@ -90,21 +90,29 @@ jobs:
- uses: actions/checkout@v4
with:
submodules: true
- run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV
- name: Homebrew cache
uses: ./.github/workflows/auto-cache
with:
path: ~/Library/Caches/Homebrew
key: brew-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
restore-keys: |
brew-macos-${{ env.CACHE_COMMIT_DATE }}
brew-macos
- name: Install dependencies
run: ./tools/mac_setup.sh
env:
# package install has DeprecationWarnings
PYTHONWARNINGS: default
- run: git lfs pull
- run: echo "CACHE_COMMIT_DATE=$(git log -1 --pretty='format:%cd' --date=format:'%Y-%m-%d-%H:%M')" >> $GITHUB_ENV
- name: Getting scons cache
uses: ./.github/workflows/auto-cache
with:
path: /tmp/scons_cache
key: scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }}-${{ github.sha }}
restore-keys: |
scons-${{ runner.arch }}-macos-${{ env.CACHE_COMMIT_DATE }}
scons-${{ runner.arch }}-macos
- name: Building openpilot
run: . .venv/bin/activate && scons -j$(nproc)

@ -82,7 +82,7 @@ jobs:
if: github.event_name == 'pull_request_target'
id: find_diff
run: >-
sudo apt-get install -y imagemagick
sudo apt-get update && sudo apt-get install -y imagemagick
scenes=$(find ${{ github.workspace }}/pr_ui/*.png -type f -printf "%f\n" | cut -d '.' -f 1 | grep -v 'pair_device')
A=($scenes)

@ -2278,6 +2278,21 @@ struct LiveTorqueParametersData {
useParams @12 :Bool;
}
struct LiveDelayData {
lateralDelay @0 :Float32;
validBlocks @1 :Int32;
status @2 :Status;
lateralDelayEstimate @3 :Float32;
points @4 :List(Float32);
enum Status {
unestimated @0;
estimated @1;
invalid @2;
}
}
struct LiveMapDataDEPRECATED {
speedLimitValid @0 :Bool;
speedLimit @1 :Float32;
@ -2508,6 +2523,7 @@ struct Event {
gnssMeasurements @91 :GnssMeasurements;
liveParameters @61 :LiveParametersData;
liveTorqueParameters @94 :LiveTorqueParametersData;
liveDelay @146 : LiveDelayData;
cameraOdometry @63 :CameraOdometry;
thumbnail @66: Thumbnail;
onroadEvents @134: List(OnroadEvent);

@ -1 +1 @@
Subproject commit ad9020c430362d17c0edf97747d344389234be4d
Subproject commit 94e738992ca8a12c2d9bb7bdeeaab4713543773e

@ -1 +1 @@
Subproject commit 0235e0e5d54d92f0df689426f79e9ba298b26b77
Subproject commit 4179672dae72e29b8ff9072d9be154515312e350

@ -1 +1 @@
Subproject commit 998ac9d5d82f10d7dd926ac66800b1efa20ff9b2
Subproject commit 8a583aaa8c1a27a3d13747b7ecead6a3d73a4e81

@ -1 +1 @@
Subproject commit 05fd60278a95190cc5a61f40730c82eaa1d986e1
Subproject commit 8b860529199569401d5d8e105bc1be600fca49a8

@ -11,7 +11,7 @@ import capnp
import numpy as np
from typing import NoReturn
from cereal import log
from cereal import log, car
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
from openpilot.common.params import Params
@ -258,16 +258,18 @@ def main() -> NoReturn:
config_realtime_process([0, 1, 2, 3], 5)
pm = messaging.PubMaster(['liveCalibration'])
sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll='cameraOdometry')
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
params_reader = Params()
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
calibrator = Calibrator(param_put=True)
calibrator.not_car = CP.notCar
while 1:
timeout = 0 if sm.frame == -1 else 100
sm.update(timeout)
calibrator.not_car = sm['carParams'].notCar
if sm.updated['cameraOdometry']:
calibrator.handle_v_ego(sm['carState'].vEgo)
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,

@ -160,19 +160,18 @@ class CarKalman(KalmanFilter):
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
dim_state = self.initial_x.shape[0]
dim_state_err = self.P_initial.shape[0]
x_init = self.initial_x
x_init[States.STEER_RATIO] = steer_ratio
x_init[States.STIFFNESS] = stiffness_factor
x_init[States.ANGLE_OFFSET] = angle_offset
if P_initial is not None:
self.P_initial = P_initial
# init filter
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":

@ -1,8 +1,8 @@
#!/usr/bin/env python3
import os
import math
import json
import numpy as np
import capnp
import cereal.messaging as messaging
from cereal import car, log
@ -13,12 +13,11 @@ from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.common.swaglog import cloudlog
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
ROLL_LOWERED_MAX = math.radians(8)
ROLL_STD_MAX = math.radians(1.5)
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
ROLL_LOWERED_MAX = np.radians(8)
ROLL_STD_MAX = np.radians(1.5)
LATERAL_ACC_SENSOR_THRESHOLD = 4.0
OFFSET_MAX = 10.0
OFFSET_LOWERED_MAX = 8.0
@ -26,40 +25,58 @@ MIN_ACTIVE_SPEED = 1.0
LOW_ACTIVE_SPEED = 10.0
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
class VehicleParamsLearner:
def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
self.kf = CarKalman(GENERATED_DIR)
self.x_initial = CarKalman.initial_x.copy()
self.x_initial[States.STEER_RATIO] = steer_ratio
self.x_initial[States.STIFFNESS] = stiffness_factor
self.x_initial[States.ANGLE_OFFSET] = angle_offset
self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
self.kf.filter.set_global("center_to_front", CP.centerToFront)
self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
self.kf.set_globals(
mass=CP.mass,
rotational_inertia=CP.rotationalInertia,
center_to_front=CP.centerToFront,
center_to_rear=CP.wheelbase - CP.centerToFront,
stiffness_front=CP.tireStiffnessFront,
stiffness_rear=CP.tireStiffnessRear
)
self.active = False
self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
self.calibrator = PoseCalibrator()
self.speed = 0.0
self.yaw_rate = 0.0
self.yaw_rate_std = 0.0
self.roll = 0.0
self.steering_angle = 0.0
self.observed_speed = 0.0
self.observed_yaw_rate = 0.0
self.observed_roll = 0.0
self.avg_offset_valid = True
self.total_offset_valid = True
self.roll_valid = True
self.reset(None)
def reset(self, t: float | None):
self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
def handle_log(self, t, which, msg):
self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
self.avg_angle_offset = self.angle_offset
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
if which == 'livePose':
device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
yaw_rate_valid = msg.angularVelocityDevice.valid
yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s
if yaw_rate_valid:
self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
else:
yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
if not yaw_rate_valid:
# This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
self.yaw_rate, self.yaw_rate_std = 0.0, np.radians(10.0)
yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
self.observed_yaw_rate = yaw_rate
localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
@ -72,18 +89,18 @@ class ParamsLearner:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
roll_std = np.radians(10.0)
self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)
if self.active:
if msg.posenetOK:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-self.yaw_rate]]),
np.array([np.atleast_2d(self.yaw_rate_std**2)]))
np.array([[-self.observed_yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL,
np.array([[self.roll]]),
np.array([[self.observed_roll]]),
np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
@ -99,20 +116,79 @@ class ParamsLearner:
self.calibrator.feed_live_calib(msg)
elif which == 'carState':
self.steering_angle = msg.steeringAngleDeg
self.speed = msg.vEgo
steering_angle = msg.steeringAngleDeg
in_linear_region = abs(self.steering_angle) < 45
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
in_linear_region = abs(steering_angle) < 45
self.observed_speed = msg.vEgo
self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
if not self.active:
# Reset time when stopped so uncertainty doesn't grow
self.kf.filter.set_filter_time(t)
self.kf.filter.reset_rewind()
self.kf.filter.set_filter_time(t) # type: ignore
self.kf.filter.reset_rewind() # type: ignore
def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
x = self.kf.x
P = np.sqrt(self.kf.P.diagonal())
if not np.all(np.isfinite(x)):
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
self.reset(self.kf.t)
x = self.kf.x
self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL].item())
if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
# Account for the opposite signs of the yaw rates
# At low speeds, bumping into a curb can cause the yaw rate to be very high
sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
else:
sensors_valid = True
self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
msg = messaging.new_message('liveParameters')
msg.valid = valid
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
liveParameters.roll = float(self.roll)
liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
liveParameters.angleOffsetDeg = float(self.angle_offset)
liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
liveParameters.angleOffsetValid = bool(self.total_offset_valid)
liveParameters.valid = all((
liveParameters.angleOffsetAverageValid,
liveParameters.angleOffsetValid ,
self.roll_valid,
roll_std < ROLL_STD_MAX,
liveParameters.stiffnessFactorValid,
liveParameters.steerRatioValid,
))
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
if debug:
liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
liveParameters.debugFilterState.value = x.tolist()
liveParameters.debugFilterState.std = P.tolist()
return msg
def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
@ -123,6 +199,65 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
return current_valid
# TODO: Remove this function after few releases (added in 0.9.9)
def migrate_cached_vehicle_params_if_needed(params_reader: Params):
last_parameters_data = params_reader.get("LiveParameters")
if last_parameters_data is None:
return
try:
last_parameters_dict = json.loads(last_parameters_data)
last_parameters_msg = messaging.new_message('liveParameters')
last_parameters_msg.liveParameters.valid = True
last_parameters_msg.liveParameters.steerRatio = last_parameters_dict['steerRatio']
last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_dict['stiffnessFactor']
last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_dict['angleOffsetAverageDeg']
params_reader.put("LiveParameters", last_parameters_msg.to_bytes())
except Exception:
pass
def retrieve_initial_vehicle_params(params_reader: Params, CP: car.CarParams, replay: bool, debug: bool):
last_parameters_data = params_reader.get("LiveParameters")
last_carparams_data = params_reader.get("CarParamsPrevRoute")
steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
retrieve_success = False
if last_parameters_data is not None and last_carparams_data is not None:
try:
with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
lp = last_lp_msg.liveParameters
# Check if car model matches
if last_CP.carFingerprint != CP.carFingerprint:
raise Exception("Car model mismatch")
# Check if starting values are sane
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
if not steer_ratio_sane:
raise Exception(f"Invalid starting values found {lp}")
initial_filter_std = np.array(lp.debugFilterState.std)
if debug and len(initial_filter_std) != 0:
p_initial = np.diag(initial_filter_std)
steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
retrieve_success = True
except Exception as e:
cloudlog.error(f"Failed to retrieve initial values: {e}")
if not replay:
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
stiffness_factor = 1.0
if not retrieve_success:
cloudlog.info("Parameter learner resetting to default values")
return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
def main():
config_realtime_process([0, 1, 2, 3], 5)
@ -133,59 +268,12 @@ def main():
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
params_reader = Params()
# wait for stats about the car to come in from controls
cloudlog.info("paramsd is waiting for CarParams")
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
cloudlog.info("paramsd got CarParams")
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
migrate_cached_vehicle_params_if_needed(params_reader)
params = params_reader.get("LiveParameters")
# Check if car model matches
if params is not None:
params = json.loads(params)
if params.get('carFingerprint', None) != CP.carFingerprint:
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
# Check if starting values are sane
if params is not None:
try:
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
if not steer_ratio_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
if not REPLAY:
# When driving in wet conditions the stiffness can go down, and then be too low on the next drive
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
pInitial = None
if DEBUG:
pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial)
angle_offset_average = params['angleOffsetAverageDeg']
angle_offset = angle_offset_average
roll = 0.0
avg_offset_valid = True
total_offset_valid = True
roll_valid = True
steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params_reader, CP, REPLAY, DEBUG)
learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)
while True:
sm.update()
@ -196,72 +284,13 @@ def main():
learner.handle_log(t, which, sm[which])
if sm.updated['livePose']:
x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)):
cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
x = learner.kf.x
angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
roll_std = float(P[States.ROAD_ROLL].item())
if learner.active and learner.speed > LOW_ACTIVE_SPEED:
# Account for the opposite signs of the yaw rates
# At low speeds, bumping into a curb can cause the yaw rate to be very high
sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
else:
sensors_valid = True
avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
msg = messaging.new_message('liveParameters')
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
liveParameters.sensorValid = sensors_valid
liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
liveParameters.roll = float(roll)
liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
liveParameters.angleOffsetDeg = float(angle_offset)
liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr
liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
liveParameters.angleOffsetAverageValid = bool(avg_offset_valid)
liveParameters.angleOffsetValid = bool(total_offset_valid)
liveParameters.valid = all((
liveParameters.angleOffsetAverageValid,
liveParameters.angleOffsetValid ,
roll_valid,
roll_std < ROLL_STD_MAX,
liveParameters.stiffnessFactorValid,
liveParameters.steerRatioValid,
))
liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
if DEBUG:
liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
liveParameters.debugFilterState.value = x.tolist()
liveParameters.debugFilterState.std = P.tolist()
msg.valid = sm.all_checks()
msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
msg_dat = msg.to_bytes()
if sm.frame % 1200 == 0: # once a minute
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': liveParameters.steerRatio,
'stiffnessFactor': liveParameters.stiffnessFactor,
'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
}
params_reader.put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
params_reader.put_nonblocking("LiveParameters", msg_dat)
pm.send('liveParameters', msg_dat)
if __name__ == "__main__":

@ -0,0 +1,57 @@
import random
import numpy as np
import json
from cereal import messaging
from openpilot.selfdrive.locationd.paramsd import retrieve_initial_vehicle_params, migrate_cached_vehicle_params_if_needed
from openpilot.selfdrive.locationd.models.car_kf import CarKalman
from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE
from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams
from openpilot.common.params import Params
from openpilot.tools.lib.logreader import LogReader
def get_random_live_parameters(CP):
msg = messaging.new_message("liveParameters")
msg.liveParameters.steerRatio = (random.random() + 0.5) * CP.steerRatio
msg.liveParameters.stiffnessFactor = random.random()
msg.liveParameters.angleOffsetAverageDeg = random.random()
msg.liveParameters.debugFilterState.std = [random.random() for _ in range(CarKalman.P_initial.shape[0])]
return msg
class TestParamsd:
def test_read_saved_params(self):
params = Params()
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
CP = next(m for m in lr if m.which() == "carParams").carParams
msg = get_random_live_parameters(CP)
params.put("LiveParameters", msg.to_bytes())
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
migrate_cached_vehicle_params_if_needed(params) # this is not tested here but should not mess anything up or throw an error
sr, sf, offset, p_init = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)
np.testing.assert_equal(p_init.shape, CarKalman.P_initial.shape)
np.testing.assert_allclose(np.diagonal(p_init), msg.liveParameters.debugFilterState.std)
# TODO Remove this test after the support for old format is removed
def test_read_saved_params_old_format(self):
params = Params()
lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
CP = next(m for m in lr if m.which() == "carParams").carParams
msg = get_random_live_parameters(CP)
params.put("LiveParameters", json.dumps(msg.liveParameters.to_dict()))
params.put("CarParamsPrevRoute", CP.as_builder().to_bytes())
migrate_cached_vehicle_params_if_needed(params)
sr, sf, offset, _ = retrieve_initial_vehicle_params(params, CP, replay=True, debug=True)
np.testing.assert_allclose(sr, msg.liveParameters.steerRatio)
np.testing.assert_allclose(sf, msg.liveParameters.stiffnessFactor)
np.testing.assert_allclose(offset, msg.liveParameters.angleOffsetAverageDeg)

@ -20,12 +20,12 @@ from openpilot.selfdrive.selfdrived.events import Events, ET
from openpilot.selfdrive.selfdrived.state import StateMachine
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
from openpilot.system.hardware import HARDWARE
from openpilot.system.version import get_build_metadata
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
ThermalStatus = log.DeviceState.ThermalStatus
@ -115,6 +115,12 @@ class SelfdriveD:
self.state_machine = StateMachine()
self.rk = Ratekeeper(100, print_delay_threshold=None)
# some comma three with NVMe experience NVMe dropouts mid-drive that
# cause loggerd to crash on write, so ignore it only on that platform
self.ignored_processes = set()
if HARDWARE.get_device_type() == 'tici' and os.path.exists('/dev/nvme0'):
self.ignored_processes = {'loggerd', }
# Determine startup event
self.startup_event = EventName.startup if build_metadata.openpilot.comma_remote and build_metadata.tested_channel else EventName.startupMaster
if not car_recognized:
@ -258,7 +264,7 @@ class SelfdriveD:
if not_running != self.not_running_prev:
cloudlog.event("process_not_running", not_running=not_running, error=True)
self.not_running_prev = not_running
if self.sm.recv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes):
self.events.add(EventName.processNotRunning)
else:
if not SIMULATION and not self.rk.lagging:

@ -2,7 +2,6 @@
import os
import time
import copy
import json
import heapq
import signal
from collections import Counter, OrderedDict
@ -517,9 +516,10 @@ CONFIGS = [
),
ProcessConfig(
proc_name="calibrationd",
pubs=["carState", "cameraOdometry", "carParams"],
pubs=["carState", "cameraOdometry"],
subs=["liveCalibration"],
ignore=["logMonoTime"],
init_callback=get_car_params_callback,
should_recv_callback=calibration_rcv_callback,
),
ProcessConfig(
@ -628,9 +628,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
if len(live_calibration) > 0:
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
if len(live_parameters) > 0:
lp_dict = live_parameters[msg_index].to_dict()
lp_dict["carFingerprint"] = CP.carFingerprint
custom_params["LiveParameters"] = json.dumps(lp_dict)
custom_params["LiveParameters"] = live_parameters[msg_index].as_builder().to_bytes()
if len(live_torque_parameters) > 0:
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()

@ -1 +1 @@
1904f49bcc97370a842aeee1f831e9ced5a6cad6
887623a18d82088dc5ed9ecdced55eb0d3f718b1

@ -95,6 +95,7 @@ TIMINGS = {
"modelV2": [2.5, 0.35],
"driverStateV2": [2.5, 0.40],
"livePose": [2.5, 0.35],
"liveParameters": [2.5, 0.35],
"wideRoadCameraState": [1.5, 0.35],
}

@ -117,31 +117,31 @@
<name>DeveloperPanel</name>
<message>
<source>Joystick Debug Mode</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>Longitudinal Maneuver Mode</source>
<translation type="unfinished"></translation>
<translation>/</translation>
</message>
<message>
<source>openpilot Longitudinal Control (Alpha)</source>
<translation type="unfinished">/ openpilot (Alpha)</translation>
<translation>/ openpilot (Alpha)</translation>
</message>
<message>
<source>WARNING: openpilot longitudinal control is in alpha for this car and will disable Automatic Emergency Braking (AEB).</source>
<translation type="unfinished">คำเตอน: การควบคมการเร/ openpilot alpha (AEB) </translation>
<translation>คำเตอน: การควบคมการเร/ openpilot alpha (AEB) </translation>
</message>
<message>
<source>On this car, openpilot defaults to the car&apos;s built-in ACC instead of openpilot&apos;s longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha.</source>
<translation type="unfinished"> openpilot / ACC openpilot openpilot / openpilot / alpha</translation>
<translation> openpilot / ACC openpilot openpilot / openpilot / alpha</translation>
</message>
<message>
<source>Enable ADB</source>
<translation type="unfinished"></translation>
<translation> ADB</translation>
</message>
<message>
<source>ADB (Android Debug Bridge) allows connecting to your device over USB or over the network. See https://docs.comma.ai/how-to/connect-to-comma for more info.</source>
<translation type="unfinished"></translation>
<translation>ADB (Android Debug Bridge) USB https://docs.comma.ai/how-to/connect-to-comma</translation>
</message>
</context>
<context>
@ -309,35 +309,37 @@
<name>FirehosePanel</name>
<message>
<source>🔥 Firehose Mode 🔥</source>
<translation type="unfinished"></translation>
<translation>🔥 🔥</translation>
</message>
<message>
<source>openpilot learns to drive by watching humans, like you, drive.
Firehose Mode allows you to maximize your training data uploads to improve openpilot&apos;s driving models. More data means bigger models, which means better Experimental Mode.</source>
<translation type="unfinished"></translation>
<translation>openpilot
openpilot </translation>
</message>
<message>
<source>Firehose Mode: ACTIVE</source>
<translation type="unfinished"></translation>
<translation>โหมดสายยางดบเพล: เปดใชงาน</translation>
</message>
<message>
<source>ACTIVE</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>For maximum effectiveness, bring your device inside and connect to a good USB-C adapter and Wi-Fi weekly.&lt;br&gt;&lt;br&gt;Firehose Mode can also work while you&apos;re driving if connected to a hotspot or unlimited SIM card.&lt;br&gt;&lt;br&gt;&lt;br&gt;&lt;b&gt;Frequently Asked Questions&lt;/b&gt;&lt;br&gt;&lt;br&gt;&lt;i&gt;Does it matter how or where I drive?&lt;/i&gt; Nope, just drive as you normally would.&lt;br&gt;&lt;br&gt;&lt;i&gt;Do all of my segments get pulled in Firehose Mode?&lt;/i&gt; No, we selectively pull a subset of your segments.&lt;br&gt;&lt;br&gt;&lt;i&gt;What&apos;s a good USB-C adapter?&lt;/i&gt; Any fast phone or laptop charger should be fine.&lt;br&gt;&lt;br&gt;&lt;i&gt;Does it matter which software I run?&lt;/i&gt; Yes, only upstream openpilot (and particular forks) are able to be used for training.</source>
<translation type="unfinished"></translation>
<translation> USB-C Wi-Fi &lt;br&gt;&lt;br&gt; &lt;br&gt;&lt;br&gt;&lt;br&gt;&lt;b&gt;&lt;/b&gt;&lt;br&gt;&lt;br&gt;&lt;i&gt;วการขบหรอสถานทบขผลหรอไม?&lt;/i&gt; &lt;br&gt;&lt;br&gt;&lt;i&gt;?&lt;/i&gt;ไมใช เราจะเลอกดงขอมลเพยงบางสวนจากเซกเมนตของคณ&lt;br&gt;&lt;br&gt;&lt;i&gt;อะแดปเตอร USB-C แบบไหนด?&lt;/i&gt; &lt;br&gt;&lt;br&gt;&lt;i&gt;?&lt;/i&gt; openpilot ( fork ) </translation>
</message>
<message numerus="yes">
<source>&lt;b&gt;%n segment(s)&lt;/b&gt; of your driving is in the training dataset so far.</source>
<translation type="unfinished">
<numerusform></numerusform>
<translation>
<numerusform> &lt;b&gt;%n &lt;/b&gt; </numerusform>
</translation>
</message>
<message>
<source>&lt;span stylesheet=&apos;font-size: 60px; font-weight: bold; color: #e74c3c;&apos;&gt;INACTIVE&lt;/span&gt;: connect to an unmetered network</source>
<translation type="unfinished"></translation>
<translation>&lt;span stylesheet=&apos;font-size: 60px; font-weight: bold; color: #e74c3c;&apos;&gt;&lt;/span&gt;: </translation>
</message>
</context>
<context>
@ -485,11 +487,11 @@ Firehose Mode allows you to maximize your training data uploads to improve openp
</message>
<message>
<source>Waiting to start</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>System Unresponsive</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
</context>
<context>
@ -512,7 +514,7 @@ Firehose Mode allows you to maximize your training data uploads to improve openp
</message>
<message>
<source>Please connect to Wi-Fi to complete initial pairing</source>
<translation type="unfinished"></translation>
<translation> Wi-Fi </translation>
</message>
</context>
<context>
@ -554,7 +556,7 @@ Firehose Mode allows you to maximize your training data uploads to improve openp
</message>
<message>
<source>Remote snapshots</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
</context>
<context>
@ -670,11 +672,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>Developer</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>Firehose</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
</context>
<context>
@ -995,11 +997,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>Welcome to openpilot</source>
<translation type="unfinished"></translation>
<translation> openpilot</translation>
</message>
<message>
<source>You must accept the Terms and Conditions to use openpilot. Read the latest terms at &lt;span style=&apos;color: #465BEA;&apos;&gt;https://comma.ai/terms&lt;/span&gt; before continuing.</source>
<translation type="unfinished"></translation>
<translation> openpilot &lt;span style=&apos;color: #465BEA;&apos;&gt;https://comma.ai/terms&lt;/span&gt; กอนดำเนนการตอ</translation>
</message>
</context>
<context>
@ -1106,11 +1108,11 @@ This may take up to a minute.</source>
</message>
<message>
<source>Always-On Driver Monitoring</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>Enable driver monitoring even when openpilot is not engaged.</source>
<translation type="unfinished"></translation>
<translation> openpilot </translation>
</message>
</context>
<context>
@ -1152,15 +1154,15 @@ This may take up to a minute.</source>
<name>WiFiPromptWidget</name>
<message>
<source>Open</source>
<translation type="unfinished"></translation>
<translation></translation>
</message>
<message>
<source>Maximize your training data uploads to improve openpilot&apos;s driving models.</source>
<translation type="unfinished"></translation>
<translation> openpilot</translation>
</message>
<message>
<source>&lt;span style=&apos;font-family: &quot;Noto Color Emoji&quot;;&apos;&gt;🔥&lt;/span&gt; Firehose Mode &lt;span style=&apos;font-family: Noto Color Emoji;&apos;&gt;🔥&lt;/span&gt;</source>
<translation type="unfinished"></translation>
<translation>&lt;span style=&apos;font-family: &quot;Noto Color Emoji&quot;;&apos;&gt;🔥&lt;/span&gt; &lt;span style=&apos;font-family: Noto Color Emoji;&apos;&gt;🔥&lt;/span&gt;</translation>
</message>
</context>
<context>

@ -65,7 +65,6 @@ const struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3008, 0x80}, // io_pad_sel
// FSIN (frame sync) with external pulses
{0x3822, 0x33}, // wait for pulse before first frame
{0x3009, 0x2},
{0x3015, 0x2},
{0x383E, 0x80},
@ -73,6 +72,9 @@ const struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
// causes issues on some devices
//{0x3822, 0x33}, // wait for pulse before first frame
{0x3892, 0x44},
{0x3823, 0x41},

@ -1 +1 @@
Subproject commit 70266e9f94d5a247ccbb2f3a46e72a2fbdaf7a8e
Subproject commit 0e34f9082e9730b5df9c055b094a43e4565e413b

1251
uv.lock

File diff suppressed because it is too large Load Diff
Loading…
Cancel
Save