2020-02-26 17:19:02 -07:00
|
|
|
#!/usr/bin/env python3
|
|
|
|
import math
|
|
|
|
|
2020-03-23 17:57:05 -06:00
|
|
|
import json
|
|
|
|
import numpy as np
|
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
import cereal.messaging as messaging
|
2020-05-19 18:00:30 -06:00
|
|
|
from cereal import car, log
|
2020-03-23 17:57:05 -06:00
|
|
|
from common.params import Params, put_nonblocking
|
2020-05-14 16:36:56 -06:00
|
|
|
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
|
|
|
|
from selfdrive.locationd.models.constants import GENERATED_DIR
|
2020-03-23 17:57:05 -06:00
|
|
|
from selfdrive.swaglog import cloudlog
|
2020-02-26 17:19:02 -07:00
|
|
|
|
2020-05-19 19:13:11 -06:00
|
|
|
KalmanStatus = log.LiveLocationKalman.Status
|
2020-05-19 18:00:30 -06:00
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
|
|
|
|
class ParamsLearner:
|
2020-03-23 17:57:05 -06:00
|
|
|
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
|
2020-05-14 16:36:56 -06:00
|
|
|
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
|
2020-03-12 13:52:31 -06:00
|
|
|
|
|
|
|
self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member
|
|
|
|
self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member
|
|
|
|
self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member
|
|
|
|
self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member
|
|
|
|
self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member
|
|
|
|
self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member
|
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
self.active = False
|
|
|
|
|
|
|
|
self.speed = 0
|
|
|
|
self.steering_pressed = False
|
|
|
|
self.steering_angle = 0
|
|
|
|
|
2020-05-19 18:00:30 -06:00
|
|
|
self.valid = True
|
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
def handle_log(self, t, which, msg):
|
2020-03-09 13:45:04 -06:00
|
|
|
if which == 'liveLocationKalman':
|
|
|
|
|
2020-03-09 14:29:30 -06:00
|
|
|
yaw_rate = msg.angularVelocityCalibrated.value[2]
|
2020-03-23 17:57:05 -06:00
|
|
|
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
|
|
|
|
|
2020-04-23 15:38:25 -06:00
|
|
|
if self.active:
|
2020-05-19 19:13:11 -06:00
|
|
|
if msg.inputsOK and msg.posenetOK and msg.status == KalmanStatus.valid:
|
2020-05-19 18:00:30 -06:00
|
|
|
self.kf.predict_and_observe(t,
|
|
|
|
ObservationKind.ROAD_FRAME_YAW_RATE,
|
|
|
|
np.array([[[-yaw_rate]]]),
|
|
|
|
np.array([np.atleast_2d(yaw_rate_std**2)]))
|
2020-03-23 17:57:05 -06:00
|
|
|
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
|
2020-02-26 17:19:02 -07:00
|
|
|
|
|
|
|
elif which == 'carState':
|
2020-09-10 13:16:29 -06:00
|
|
|
self.steering_angle = msg.steeringAngle
|
|
|
|
self.steering_pressed = msg.steeringPressed
|
|
|
|
self.speed = msg.vEgo
|
2020-04-23 15:38:25 -06:00
|
|
|
|
2020-09-10 13:16:29 -06:00
|
|
|
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
|
|
|
|
self.active = self.speed > 5 and in_linear_region
|
2020-02-26 17:19:02 -07:00
|
|
|
|
2020-09-10 13:16:29 -06:00
|
|
|
if self.active:
|
|
|
|
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
|
|
|
|
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
|
2020-03-23 17:57:05 -06:00
|
|
|
|
|
|
|
if not self.active:
|
|
|
|
# Reset time when stopped so uncertainty doesn't grow
|
|
|
|
self.kf.filter.filter_time = t
|
|
|
|
self.kf.filter.reset_rewind()
|
2020-02-26 17:19:02 -07:00
|
|
|
|
|
|
|
|
|
|
|
def main(sm=None, pm=None):
|
|
|
|
if sm is None:
|
2020-09-10 13:16:29 -06:00
|
|
|
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
|
2020-02-26 17:19:02 -07:00
|
|
|
if pm is None:
|
|
|
|
pm = messaging.PubMaster(['liveParameters'])
|
|
|
|
|
2020-03-23 17:57:05 -06:00
|
|
|
params_reader = Params()
|
|
|
|
# wait for stats about the car to come in from controls
|
|
|
|
cloudlog.info("paramsd is waiting for CarParams")
|
|
|
|
CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
|
|
|
|
cloudlog.info("paramsd got CarParams")
|
|
|
|
|
2020-10-19 11:04:33 -06:00
|
|
|
min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
|
|
|
|
|
2020-03-23 17:57:05 -06:00
|
|
|
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
|
|
|
|
|
2020-10-19 11:04:33 -06:00
|
|
|
if (params is not None) and not all((
|
|
|
|
abs(params['angleOffsetAverage']) < 10.0,
|
|
|
|
min_sr <= params['steerRatio'] <= max_sr)):
|
|
|
|
cloudlog.info(f"Invalid starting values found {params}")
|
|
|
|
params = None
|
|
|
|
|
2020-03-23 17:57:05 -06:00
|
|
|
if params is None:
|
|
|
|
params = {
|
|
|
|
'carFingerprint': CP.carFingerprint,
|
|
|
|
'steerRatio': CP.steerRatio,
|
|
|
|
'stiffnessFactor': 1.0,
|
|
|
|
'angleOffsetAverage': 0.0,
|
|
|
|
}
|
|
|
|
cloudlog.info("Parameter learner resetting to default values")
|
|
|
|
|
2020-06-12 13:09:04 -06:00
|
|
|
# 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
|
|
|
|
|
2020-03-23 17:57:05 -06:00
|
|
|
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
|
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
while True:
|
|
|
|
sm.update()
|
|
|
|
|
|
|
|
for which, updated in sm.updated.items():
|
2020-09-10 13:16:29 -06:00
|
|
|
if updated:
|
|
|
|
t = sm.logMonoTime[which] * 1e-9
|
|
|
|
learner.handle_log(t, which, sm[which])
|
2020-02-26 17:19:02 -07:00
|
|
|
|
2020-09-10 13:16:29 -06:00
|
|
|
if sm.updated['liveLocationKalman']:
|
2020-03-05 18:32:52 -07:00
|
|
|
msg = messaging.new_message('liveParameters')
|
2020-02-26 17:19:02 -07:00
|
|
|
msg.logMonoTime = sm.logMonoTime['carState']
|
|
|
|
|
|
|
|
msg.liveParameters.posenetValid = True
|
|
|
|
msg.liveParameters.sensorValid = True
|
|
|
|
|
|
|
|
x = learner.kf.x
|
|
|
|
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
|
|
|
|
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
|
|
|
|
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
|
2020-03-24 10:39:57 -06:00
|
|
|
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
|
2020-05-19 18:00:30 -06:00
|
|
|
msg.liveParameters.valid = all((
|
|
|
|
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
|
|
|
|
abs(msg.liveParameters.angleOffset) < 10.0,
|
2020-06-09 18:36:16 -06:00
|
|
|
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
|
2020-05-19 18:00:30 -06:00
|
|
|
min_sr <= msg.liveParameters.steerRatio <= max_sr,
|
|
|
|
))
|
2020-02-26 17:19:02 -07:00
|
|
|
|
2020-09-10 13:16:29 -06:00
|
|
|
if sm.frame % 1200 == 0: # once a minute
|
2020-03-23 17:57:05 -06:00
|
|
|
params = {
|
|
|
|
'carFingerprint': CP.carFingerprint,
|
|
|
|
'steerRatio': msg.liveParameters.steerRatio,
|
|
|
|
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
|
|
|
|
'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
|
|
|
|
}
|
|
|
|
put_nonblocking("LiveParameters", json.dumps(params))
|
|
|
|
|
2020-02-26 17:19:02 -07:00
|
|
|
pm.send('liveParameters', msg)
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
|
|
main()
|