retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Use new locationd packet in paramsd

albatross
Willem Melching 2020-03-09 12:45:04 -07:00
parent 2b43df0d6e
commit 01ac9d4722
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
selfdrive/locationd/paramsd.py
View File

@ -2,7 +2,6 @@
import math
import cereal.messaging as messaging
import common.transformations.orientation as orient
from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States
CARSTATE_DECIMATION = 5
@ -22,14 +21,18 @@ class ParamsLearner:
self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5
def handle_log(self, t, which, msg):
if which == 'liveLocation':
roll, pitch, yaw = math.radians(msg.roll), math.radians(msg.pitch), math.radians(msg.heading)
v_device = orient.rot_from_euler([roll, pitch, yaw]).T.dot(msg.vNED)
if which == 'liveLocationKalman':
v_device = msg.velocityDevice.value
# v_device_std = msg.velocityDevice.std
self.speed = v_device[0]
yaw_rate = msg.angularVelocityDevice.value[2]
# yaw_rate_std = msg.angularVelocityDevice.std[2]
self.update_active()
if self.active:
self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-msg.gyro[2]])
self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-yaw_rate])
self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]])
# Clamp values
@ -59,7 +62,7 @@ class ParamsLearner:
def main(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['liveLocation', 'carState'])
sm = messaging.SubMaster(['liveLocationKalman', 'carState'])
if pm is None:
pm = messaging.PubMaster(['liveParameters'])