Update locationd (#20410)

* this was too extreme

* unused import

* capnp wants bools

* update refs
old-commit-hash: 8bddaa7607e659c1cb247b448684d716257a3158
This commit is contained in:
HaraldSchafer
2021-03-19 17:12:34 -07:00
committed by GitHub
parent 4ce5e08ded
commit cf6c7cb2b8
4 changed files with 16 additions and 16 deletions
+7 -7
View File
@@ -5,14 +5,12 @@ import json
import numpy as np
import cereal.messaging as messaging
from cereal import car, log
from cereal import car
from common.params import Params, put_nonblocking
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from selfdrive.locationd.models.constants import GENERATED_DIR
from selfdrive.swaglog import cloudlog
KalmanStatus = log.LiveLocationKalman.Status
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
@@ -38,9 +36,10 @@ class ParamsLearner:
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
yaw_rate_valid = msg.angularVelocityCalibrated.valid
if self.active:
if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid:
if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
@@ -89,9 +88,10 @@ def main(sm=None, pm=None):
params = None
try:
if params is not None and not all((
abs(params.get('angleOffsetAverageDeg')) < 10.0,
min_sr <= params['steerRatio'] <= max_sr)):
angle_offset_sane = abs(params.get('angleOffsetAverageDeg')) < 10.0
steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
params_sane = angle_offset_sane and steer_ratio_sane
if params is not None and not params_sane:
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e: