mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 17:23:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF global variable support (#1230)

Browse Source 
* Add global variables to kalman filter code

* fix linter
old-commit-hash: e28832b359a2981b6ebeb9f1f3d5bbe72248993c
This commit is contained in:
Willem Melching
2020-03-12 12:52:31 -07:00
committed by GitHub
parent 6c6df5486a
commit ea1c97cab7
4 changed files with 52 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/paramsd.py
+15 -2
View File
@@ -8,8 +8,16 @@ CARSTATE_DECIMATION = 5
class ParamsLearner:
def __init__(self):
def __init__(self, CP):
self.kf = CarKalman()
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
self.active = False
self.speed = 0
@@ -66,7 +74,12 @@ def main(sm=None, pm=None):
if pm is None:
pm = messaging.PubMaster(['liveParameters'])
learner = ParamsLearner()
# TODO: Read from car params at runtime
from selfdrive.car.toyota.interface import CarInterface
from selfdrive.car.toyota.values import CAR
CP = CarInterface.get_params(CAR.COROLLA_TSS2)
learner = ParamsLearner(CP)
while True:
sm.update()