mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Paramsd handle liveLocation not valid and add alerts

Browse Source 
old-commit-hash: 44484102db471a5f85ad078a81b9d8b9456b5233
This commit is contained in:
Willem Melching
2020-05-19 17:00:30 -07:00
parent 6e41973c3c
commit d2a41fad6e
2 changed files with 21 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
selfdrive/locationd/models/car_kf.py
View File

@@ -49,8 +49,8 @@ class CarKalman(KalmanFilter):
Q = np.diag([
(.05/100)**2,
.01**2,
math.radians(0.002)**2,
math.radians(0.1)**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
@@ -60,7 +60,7 @@ class CarKalman(KalmanFilter):
obs_noise = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
@@ -139,7 +139,7 @@ class CarKalman(KalmanFilter):
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0):
dim_state = self.initial_x.shape[0]
dim_state_err = self.initial_P_diag.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

38
selfdrive/locationd/paramsd.py
View File

@@ -5,12 +5,14 @@ import json
import numpy as np
import cereal.messaging as messaging
from cereal import car
from cereal import car, log
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
FixStatus = log.LiveLocationKalman.Status
CARSTATE_DECIMATION = 5
@@ -32,6 +34,8 @@ class ParamsLearner:
self.steering_angle = 0
self.carstate_counter = 0
self.valid = True
def handle_log(self, t, which, msg):
if which == 'liveLocationKalman':
@@ -39,21 +43,13 @@ class ParamsLearner:
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
if self.active:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
if msg.inputsOK and msg.posenetOK and msg.status == FixStatus.valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[[-yaw_rate]]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
# Clamp values
# x = self.kf.x
# if not (10 < x[States.STEER_RATIO] < 25):
# self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[[15.0]]]))
# if not (0.5 < x[States.STIFFNESS] < 3.0):
# self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[[1.0]]]))
elif which == 'carState':
self.carstate_counter += 1
if self.carstate_counter % CARSTATE_DECIMATION == 0:
@@ -105,6 +101,7 @@ def main(sm=None, pm=None):
cloudlog.info("Parameter learner resetting to default values")
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
i = 0
while True:
@@ -132,6 +129,12 @@ def main(sm=None, pm=None):
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
abs(msg.liveParameters.angleOffset) < 10.0,
0.5 <= msg.liveParameters.stiffnessFactor <= 2.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
i += 1
if i % 6000 == 0: # once a minute
@@ -143,13 +146,6 @@ def main(sm=None, pm=None):
}
put_nonblocking("LiveParameters", json.dumps(params))
# P = learner.kf.P
# print()
# print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5)
# print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5)
# print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5)
# print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5)
pm.send('liveParameters', msg)