|
|
|
@ -1,9 +1,6 @@ |
|
|
|
|
#!/usr/bin/env python3 |
|
|
|
|
import os |
|
|
|
|
import signal |
|
|
|
|
import numpy as np |
|
|
|
|
from collections import deque, defaultdict |
|
|
|
|
from functools import partial |
|
|
|
|
|
|
|
|
|
import cereal.messaging as messaging |
|
|
|
|
from cereal import car, log |
|
|
|
@ -12,7 +9,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL |
|
|
|
|
from openpilot.common.filter_simple import FirstOrderFilter |
|
|
|
|
from openpilot.common.swaglog import cloudlog |
|
|
|
|
from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY |
|
|
|
|
from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, cache_points_onexit |
|
|
|
|
from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, cache_points |
|
|
|
|
|
|
|
|
|
HISTORY = 5 # secs |
|
|
|
|
POINTS_PER_BUCKET = 1500 |
|
|
|
@ -227,9 +224,6 @@ def main(): |
|
|
|
|
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: |
|
|
|
|
estimator = TorqueEstimator(CP) |
|
|
|
|
|
|
|
|
|
if "REPLAY" not in os.environ: |
|
|
|
|
signal.signal(signal.SIGINT, partial(cache_points_onexit, "LiveTorqueParameters", estimator)) |
|
|
|
|
|
|
|
|
|
while True: |
|
|
|
|
sm.update() |
|
|
|
|
if sm.all_checks(): |
|
|
|
@ -242,6 +236,10 @@ def main(): |
|
|
|
|
if sm.frame % 5 == 0: |
|
|
|
|
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) |
|
|
|
|
|
|
|
|
|
# Cache points every 60 seconds while onroad |
|
|
|
|
if sm.frame % 240 == 0: |
|
|
|
|
cache_points("LiveTorqueParameters", estimator, sm.all_checks()) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__": |
|
|
|
|
main() |
|
|
|
|