Add paramsd Stdev to message (#22443)

* add STD to message

* use modified names for car_kf state stdev atttributes

* update refs

* add modofied cereal

* update refs
pull/22462/head
Vivek Aithal 4 years ago committed by GitHub
parent 519fdc0816
commit 244fbde56e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 2
      cereal
  2. 5
      selfdrive/locationd/paramsd.py
  3. 2
      selfdrive/test/process_replay/ref_commit

@ -1 +1 @@
Subproject commit d5a85fe5d7e97d0374bbb94b3d64aaf1ab9cee83 Subproject commit 517fd56398c0d3ae4d0a142849b81e2f025d8572

@ -139,6 +139,7 @@ def main(sm=None, pm=None):
if sm.updated['liveLocationKalman']: if sm.updated['liveLocationKalman']:
x = learner.kf.x x = learner.kf.x
P = np.sqrt(learner.kf.P.diagonal())
if not all(map(math.isfinite, x)): if not all(map(math.isfinite, x)):
cloudlog.error("NaN in liveParameters estimate. Resetting to default values") cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
@ -162,6 +163,10 @@ def main(sm=None, pm=None):
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0, 0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr, min_sr <= msg.liveParameters.steerRatio <= max_sr,
)) ))
msg.liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
msg.liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
msg.liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
msg.liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
if sm.frame % 1200 == 0: # once a minute if sm.frame % 1200 == 0: # once a minute
params = { params = {

@ -1 +1 @@
a69e0b5fe4f506b8b9d372bfa68d8a47cf105a3f d643d6ff47522e00d06035ab0cb9e14d1c0c0ae0
Loading…
Cancel
Save