sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

paramsd: refactor VehicleParamsLearner (#34955)

Browse Source 
* Refactor ParamsLearner

* Make it VehicleParamsLearner

* Fix

* Use capnp serialization instead of json

* Fix speed

* Remove redundant comments

* Monitor observed_roll

* Just use init_state

* Comment

* Improve reset

* Set globals api

* Typing for return value

* Redo reset messaging

* Remove usages of math

* Fix process_replay custom_params

* Type ignores for rednose fields

* Remove import

* Reset previous values too

* Update ref_commit

* Revert it

* Bring it back

* Remove more

* Add migration for cached params
pull/34965/head
Kacper Rączy 4 weeks ago committed by GitHub
parent 447f279f83
commit 75cc300480
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 197 additions and 172 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      selfdrive/locationd/models/car_kf.py
  2. 337
      selfdrive/locationd/paramsd.py
  3. 5
      selfdrive/test/process_replay/process_replay.py
  4. 2
      selfdrive/test/process_replay/ref_commit

25
selfdrive/locationd/models/car_kf.py
Unescape View File

@ -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) 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): def __init__(self, generated_dir):
dim_state = self.initial_x.shape[0] dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
dim_state_err = self.P_initial.shape[0] self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
x_init = self.initial_x dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
x_init[States.STEER_RATIO] = steer_ratio
x_init[States.STIFFNESS] = stiffness_factor def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
x_init[States.ANGLE_OFFSET] = angle_offset self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
if P_initial is not None: self.filter.set_global("center_to_front", center_to_front)
self.P_initial = P_initial self.filter.set_global("center_to_rear", center_to_rear)
# init filter self.filter.set_global("stiffness_front", stiffness_front)
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, self.filter.set_global("stiffness_rear", stiffness_rear)
dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)
if __name__ == "__main__": if __name__ == "__main__":

337
selfdrive/locationd/paramsd.py
Unescape View File

@ -1,8 +1,8 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import os import os
import math
import json import json
import numpy as np import numpy as np
import capnp
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import car, log 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.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.common.swaglog import cloudlog from openpilot.common.swaglog import cloudlog
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s 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_MAX_DELTA = np.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_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
ROLL_LOWERED_MAX = math.radians(8) ROLL_LOWERED_MAX = np.radians(8)
ROLL_STD_MAX = math.radians(1.5) ROLL_STD_MAX = np.radians(1.5)
LATERAL_ACC_SENSOR_THRESHOLD = 4.0 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
OFFSET_MAX = 10.0 OFFSET_MAX = 10.0
OFFSET_LOWERED_MAX = 8.0 OFFSET_LOWERED_MAX = 8.0
@ -26,40 +25,58 @@ MIN_ACTIVE_SPEED = 1.0
LOW_ACTIVE_SPEED = 10.0 LOW_ACTIVE_SPEED = 10.0
class ParamsLearner: class VehicleParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None): 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, steer_ratio, stiffness_factor, angle_offset, P_initial) 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 or CarKalman.P_initial
self.kf.filter.set_global("mass", CP.mass) self.kf.set_globals(
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) mass=CP.mass,
self.kf.filter.set_global("center_to_front", CP.centerToFront) rotational_inertia=CP.rotationalInertia,
self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront) center_to_front=CP.centerToFront,
self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront) center_to_rear=CP.wheelbase - CP.centerToFront,
self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) 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.calibrator = PoseCalibrator()
self.speed = 0.0 self.observed_speed = 0.0
self.yaw_rate = 0.0 self.observed_yaw_rate = 0.0
self.yaw_rate_std = 0.0 self.observed_roll = 0.0
self.roll = 0.0
self.steering_angle = 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': if which == 'livePose':
device_pose = Pose.from_live_pose(msg) device_pose = Pose.from_live_pose(msg)
calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) 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 = 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 0 < yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(self.yaw_rate) < 1 # rad/s yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
if yaw_rate_valid: if not yaw_rate_valid:
self.yaw_rate, self.yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
else:
# This is done to bound the yaw rate estimate when localizer values are invalid or calibrating # 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, 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 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 # This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0 roll = 0.0
roll_std = np.radians(10.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 self.active:
if msg.posenetOK: if msg.posenetOK:
self.kf.predict_and_observe(t, self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-self.yaw_rate]]), np.array([[-self.observed_yaw_rate]]),
np.array([np.atleast_2d(self.yaw_rate_std**2)])) np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t, self.kf.predict_and_observe(t,
ObservationKind.ROAD_ROLL, ObservationKind.ROAD_ROLL,
np.array([[self.roll]]), np.array([[self.observed_roll]]),
np.array([np.atleast_2d(roll_std**2)])) np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) 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) self.calibrator.feed_live_calib(msg)
elif which == 'carState': elif which == 'carState':
self.steering_angle = msg.steeringAngleDeg steering_angle = msg.steeringAngleDeg
self.speed = msg.vEgo
in_linear_region = abs(self.steering_angle) < 45 in_linear_region = abs(steering_angle) < 45
self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region self.observed_speed = msg.vEgo
self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region
if self.active: 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.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]])) self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))
if not self.active: if not self.active:
# Reset time when stopped so uncertainty doesn't grow # Reset time when stopped so uncertainty doesn't grow
self.kf.filter.set_filter_time(t) self.kf.filter.set_filter_time(t) # type: ignore
self.kf.filter.reset_rewind() 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): 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 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 (json.JSONDecodeError, KeyError):
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 = 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(): def main():
config_realtime_process([0, 1, 2, 3], 5) config_realtime_process([0, 1, 2, 3], 5)
@ -133,59 +268,12 @@ def main():
sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose') sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState'], poll='livePose')
params_reader = Params() 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) 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") 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)
# 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
while True: while True:
sm.update() sm.update()
@ -196,72 +284,13 @@ def main():
learner.handle_log(t, which, sm[which]) learner.handle_log(t, which, sm[which])
if sm.updated['livePose']: if sm.updated['livePose']:
x = learner.kf.x msg = learner.get_msg(sm.all_checks(), debug=DEBUG)
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_dat = msg.to_bytes()
if sm.frame % 1200 == 0: # once a minute if sm.frame % 1200 == 0: # once a minute
params = { params_reader.put_nonblocking("LiveParameters", msg_dat)
'carFingerprint': CP.carFingerprint,
'steerRatio': liveParameters.steerRatio, pm.send('liveParameters', msg_dat)
'stiffnessFactor': liveParameters.stiffnessFactor,
'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
}
params_reader.put_nonblocking("LiveParameters", json.dumps(params))
pm.send('liveParameters', msg)
if __name__ == "__main__": if __name__ == "__main__":

5
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -2,7 +2,6 @@
import os import os
import time import time
import copy import copy
import json
import heapq import heapq
import signal import signal
from collections import Counter, OrderedDict from collections import Counter, OrderedDict
@ -628,9 +627,7 @@ def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") ->
if len(live_calibration) > 0: if len(live_calibration) > 0:
custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes()
if len(live_parameters) > 0: if len(live_parameters) > 0:
lp_dict = live_parameters[msg_index].to_dict() custom_params["LiveParameters"] = live_parameters[msg_index].as_builder().to_bytes()
lp_dict["carFingerprint"] = CP.carFingerprint
custom_params["LiveParameters"] = json.dumps(lp_dict)
if len(live_torque_parameters) > 0: if len(live_torque_parameters) > 0:
custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes()

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
1904f49bcc97370a842aeee1f831e9ced5a6cad6 887623a18d82088dc5ed9ecdced55eb0d3f718b1
Write Preview
Loading…
Cancel
Save