version: dragonpilot development version for EON/C2

date: 2024-02-26T20:43:48
commit: 61592bc9e895361b44b62dd805c59bf41d486183

selfdrive/locationd/helpers.py

@@ -0,0 +1,64 @@
+import numpy as np
+from typing import List, Optional, Tuple, Any
+
+from cereal import log
+
+
+class NPQueue:
+  def __init__(self, maxlen: int, rowsize: int) -> None:
+    self.maxlen = maxlen
+    self.arr = np.empty((0, rowsize))
+
+  def __len__(self) -> int:
+    return len(self.arr)
+
+  def append(self, pt: List[float]) -> None:
+    if len(self.arr) < self.maxlen:
+      self.arr = np.append(self.arr, [pt], axis=0)
+    else:
+      self.arr[:-1] = self.arr[1:]
+      self.arr[-1] = pt
+
+
+class PointBuckets:
+  def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
+    self.x_bounds = x_bounds
+    self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
+    self.buckets_min_points = dict(zip(x_bounds, min_points))
+    self.min_points_total = min_points_total
+
+  def bucket_lengths(self) -> List[int]:
+    return [len(v) for v in self.buckets.values()]
+
+  def __len__(self) -> int:
+    return sum(self.bucket_lengths())
+
+  def is_valid(self) -> bool:
+    individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values()))
+    total_points_valid = self.__len__() >= self.min_points_total
+    return individual_buckets_valid and total_points_valid
+
+  def add_point(self, x: float, y: float, bucket_val: float) -> None:
+    raise NotImplementedError
+
+  def get_points(self, num_points: Optional[int] = None) -> Any:
+    points = np.vstack([x.arr for x in self.buckets.values()])
+    if num_points is None:
+      return points
+    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
+
+  def load_points(self, points: List[List[float]]) -> None:
+    for point in points:
+      self.add_point(*point)
+
+
+class ParameterEstimator:
+  """ Base class for parameter estimators """
+  def reset(self) -> None:
+    raise NotImplementedError
+
+  def handle_log(self, t: int, which: str, msg: log.Event) -> None:
+    raise NotImplementedError
+
+  def get_msg(self, valid: bool, with_points: bool) -> log.Event:
+    raise NotImplementedError

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_7792005023266191321);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_5315267233560935918);
-void car_H_mod_fun(double *state, double *out_7339459761251930020);
-void car_f_fun(double *state, double dt, double *out_2286622615178359403);
-void car_F_fun(double *state, double dt, double *out_8254211230775852992);
-void car_h_25(double *state, double *unused, double *out_6918209566450062964);
-void car_H_25(double *state, double *unused, double *out_4801460618481353345);
-void car_h_24(double *state, double *unused, double *out_1902027086632663468);
-void car_H_24(double *state, double *unused, double *out_3973431789330817549);
-void car_h_30(double *state, double *unused, double *out_3280760038196040614);
-void car_H_30(double *state, double *unused, double *out_4672121671338113275);
-void car_h_26(double *state, double *unused, double *out_6448147115644818676);
-void car_H_26(double *state, double *unused, double *out_1059957299607297121);
-void car_h_27(double *state, double *unused, double *out_1916536298789217319);
-void car_H_27(double *state, double *unused, double *out_2497358359537688364);
-void car_h_29(double *state, double *unused, double *out_2677444153519563097);
-void car_H_29(double *state, double *unused, double *out_783995632668137331);
-void car_h_28(double *state, double *unused, double *out_3149881219929125404);
-void car_H_28(double *state, double *unused, double *out_4298403384401393243);
-void car_h_31(double *state, double *unused, double *out_845055690370609477);
-void car_H_31(double *state, double *unused, double *out_4832106580358313773);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1964954299455970933);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6003963640381126063);
+void car_H_mod_fun(double *state, double *out_1207008555950061675);
+void car_f_fun(double *state, double dt, double *out_2803172193991338453);
+void car_F_fun(double *state, double dt, double *out_4664417582021608014);
+void car_h_25(double *state, double *unused, double *out_8570962910733009665);
+void car_H_25(double *state, double *unused, double *out_1998904563998531603);
+void car_h_24(double *state, double *unused, double *out_2751191583816658395);
+void car_H_24(double *state, double *unused, double *out_6775987843813639291);
+void car_h_30(double *state, double *unused, double *out_4933513382478987315);
+void car_H_30(double *state, double *unused, double *out_1869565616855291533);
+void car_h_26(double *state, double *unused, double *out_2501630051949713498);
+void car_H_26(double *state, double *unused, double *out_1742598754875524621);
+void car_h_27(double *state, double *unused, double *out_8932271310074879516);
+void car_H_27(double *state, double *unused, double *out_305197694945133378);
+void car_h_29(double *state, double *unused, double *out_5877023235380649750);
+void car_H_29(double *state, double *unused, double *out_2379796961169683717);
+void car_h_28(double *state, double *unused, double *out_5895485258630546831);
+void car_H_28(double *state, double *unused, double *out_4343427232735009968);
+void car_h_31(double *state, double *unused, double *out_5381229435042917038);
+void car_H_31(double *state, double *unused, double *out_2029550525875492031);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/gnss.h

@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_769020749321527910);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6434868672514128410);
-void gnss_H_mod_fun(double *state, double *out_5816879217043291130);
-void gnss_f_fun(double *state, double dt, double *out_8075609735883072475);
-void gnss_F_fun(double *state, double dt, double *out_7333316748729345884);
-void gnss_h_6(double *state, double *sat_pos, double *out_6468678908706064352);
-void gnss_H_6(double *state, double *sat_pos, double *out_3868951147833506795);
-void gnss_h_20(double *state, double *sat_pos, double *out_7441535645826448778);
-void gnss_H_20(double *state, double *sat_pos, double *out_8090373605569156258);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_3881194323588387136);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_3194084289009820845);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_3881194323588387136);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_3194084289009820845);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_8519908807701444381);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7494726988393394594);
+void gnss_H_mod_fun(double *state, double *out_6213794081998936506);
+void gnss_f_fun(double *state, double dt, double *out_7557732014251175831);
+void gnss_F_fun(double *state, double dt, double *out_3784107475037224440);
+void gnss_h_6(double *state, double *sat_pos, double *out_1045706946388579730);
+void gnss_H_6(double *state, double *sat_pos, double *out_6469468708647551695);
+void gnss_h_20(double *state, double *sat_pos, double *out_4390939550384841311);
+void gnss_H_20(double *state, double *sat_pos, double *out_4150453882875260635);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_882947065038288714);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_4769832076722140500);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_882947065038288714);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_4769832076722140500);
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/locationd/models/generated/live.h

@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_7429008421233965090);
-void live_err_fun(double *nom_x, double *delta_x, double *out_7284268987952680115);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_4617513455921064816);
-void live_H_mod_fun(double *state, double *out_9076499371074019083);
-void live_f_fun(double *state, double dt, double *out_7863759626582993905);
-void live_F_fun(double *state, double dt, double *out_4436932215835623194);
-void live_h_4(double *state, double *unused, double *out_3414164203987698111);
-void live_H_4(double *state, double *unused, double *out_1320474316052717902);
-void live_h_9(double *state, double *unused, double *out_3803073597728245259);
-void live_H_9(double *state, double *unused, double *out_5477642052407495385);
-void live_h_10(double *state, double *unused, double *out_5079492584863434208);
-void live_H_10(double *state, double *unused, double *out_2479322990341632036);
-void live_h_12(double *state, double *unused, double *out_6224366416023586503);
-void live_H_12(double *state, double *unused, double *out_699375291005124235);
-void live_h_35(double *state, double *unused, double *out_8146514266443503093);
-void live_H_35(double *state, double *unused, double *out_2046187741319889474);
-void live_h_32(double *state, double *unused, double *out_3901636462027137299);
-void live_H_32(double *state, double *unused, double *out_1733566046432444446);
-void live_h_13(double *state, double *unused, double *out_6669897253458653498);
-void live_H_13(double *state, double *unused, double *out_1446272565186157079);
-void live_h_14(double *state, double *unused, double *out_3803073597728245259);
-void live_H_14(double *state, double *unused, double *out_5477642052407495385);
-void live_h_33(double *state, double *unused, double *out_3006773689665239248);
-void live_H_33(double *state, double *unused, double *out_5196744745958747078);
+void live_H(double *in_vec, double *out_2427080946257414198);
+void live_err_fun(double *nom_x, double *delta_x, double *out_4499017012837728402);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_4568239191987130813);
+void live_H_mod_fun(double *state, double *out_7466442051496209266);
+void live_f_fun(double *state, double dt, double *out_7616316395018437217);
+void live_F_fun(double *state, double dt, double *out_3738746660903124007);
+void live_h_4(double *state, double *unused, double *out_5102933337023237333);
+void live_H_4(double *state, double *unused, double *out_7235994504100796541);
+void live_h_9(double *state, double *unused, double *out_6101034663185489335);
+void live_H_9(double *state, double *unused, double *out_51224431163650929);
+void live_h_10(double *state, double *unused, double *out_1900712798164773370);
+void live_H_10(double *state, double *unused, double *out_4820155672142958661);
+void live_h_12(double *state, double *unused, double *out_8879026316650247416);
+void live_H_12(double *state, double *unused, double *out_6614895479053202874);
+void live_h_35(double *state, double *unused, double *out_1924620734251358832);
+void live_H_35(double *state, double *unused, double *out_3869332446728189165);
+void live_h_32(double *state, double *unused, double *out_1566775458117550308);
+void live_H_32(double *state, double *unused, double *out_8625923032509862606);
+void live_h_13(double *state, double *unused, double *out_5786152235345498787);
+void live_H_13(double *state, double *unused, double *out_1965575430518687731);
+void live_h_14(double *state, double *unused, double *out_6101034663185489335);
+void live_H_14(double *state, double *unused, double *out_51224431163650929);
+void live_h_33(double *state, double *unused, double *out_1672775015514148260);
+void live_H_33(double *state, double *unused, double *out_718775442089331561);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/locationd/torqued.py

@@ -1,7 +1,4 @@
 #!/usr/bin/env python3
-import os
-import sys
-import signal
 import numpy as np
 from collections import deque, defaultdict
 
@@ -12,6 +9,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.system.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -43,56 +41,15 @@ def slope2rot(slope):
   return np.array([[cos, -sin], [sin, cos]])
 
 
-class NPQueue:
-  def __init__(self, maxlen, rowsize):
-    self.maxlen = maxlen
-    self.arr = np.empty((0, rowsize))
-
-  def __len__(self):
-    return len(self.arr)
-
-  def append(self, pt):
-    if len(self.arr) < self.maxlen:
-      self.arr = np.append(self.arr, [pt], axis=0)
-    else:
-      self.arr[:-1] = self.arr[1:]
-      self.arr[-1] = pt
-
-
-class PointBuckets:
-  def __init__(self, x_bounds, min_points, min_points_total):
-    self.x_bounds = x_bounds
-    self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
-    self.buckets_min_points = dict(zip(x_bounds, min_points))
-    self.min_points_total = min_points_total
-
-  def bucket_lengths(self):
-    return [len(v) for v in self.buckets.values()]
-
-  def __len__(self):
-    return sum(self.bucket_lengths())
-
-  def is_valid(self):
-    return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total)
-
+class TorqueBuckets(PointBuckets):
   def add_point(self, x, y):
     for bound_min, bound_max in self.x_bounds:
       if (x >= bound_min) and (x < bound_max):
         self.buckets[(bound_min, bound_max)].append([x, 1.0, y])
         break
 
-  def get_points(self, num_points=None):
-    points = np.vstack([x.arr for x in self.buckets.values()])
-    if num_points is None:
-      return points
-    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
 
-  def load_points(self, points):
-    for x, y in points:
-      self.add_point(x, y)
-
-
-class TorqueEstimator:
+class TorqueEstimator(ParameterEstimator):
   def __init__(self, CP, decimated=False):
     self.hist_len = int(HISTORY / DT_MDL)
     self.lag = CP.steerActuatorDelay + .2   # from controlsd
@@ -113,7 +70,7 @@ class TorqueEstimator:
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
-    self.use_params = CP.carName in ALLOWED_CARS
+    self.use_params = CP.carName in ALLOWED_CARS and CP.lateralTuning.which() == 'torque'
 
     if CP.lateralTuning.which() == 'torque':
       self.offline_friction = CP.lateralTuning.torque.friction
@@ -135,7 +92,7 @@ class TorqueEstimator:
 
     # try to restore cached params
     params = Params()
-    params_cache = params.get("LiveTorqueCarParams")
+    params_cache = params.get("CarParamsPrevRoute")
     torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:
       try:
@@ -158,7 +115,6 @@ class TorqueEstimator:
           cloudlog.info("restored torque params from cache")
       except Exception:
         cloudlog.exception("failed to restore cached torque params")
-        params.remove("LiveTorqueCarParams")
         params.remove("LiveTorqueParameters")
 
     self.filtered_params = {}
@@ -176,7 +132,11 @@ class TorqueEstimator:
     self.resets += 1.0
     self.decay = MIN_FILTER_DECAY
     self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
-    self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total)
+    self.filtered_points = TorqueBuckets(x_bounds=STEER_BUCKET_BOUNDS,
+                                         min_points=self.min_bucket_points,
+                                         min_points_total=self.min_points_total,
+                                         points_per_bucket=POINTS_PER_BUCKET,
+                                         rowsize=3)
 
   def estimate_params(self):
     points = self.filtered_points.get_points(self.fit_points)
@@ -256,34 +216,17 @@ class TorqueEstimator:
     return msg
 
 
-def main(sm=None, pm=None):
+def main(demo=False):
   config_realtime_process([0, 1, 2, 3], 5)
 
-  if sm is None:
-    sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])
-
-  if pm is None:
-    pm = messaging.PubMaster(['liveTorqueParameters'])
+  pm = messaging.PubMaster(['liveTorqueParameters'])
+  sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll=['liveLocationKalman'])
 
   params = Params()
   CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
   # with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
   estimator = TorqueEstimator(CP)
 
-  def cache_params(sig, frame):
-    signal.signal(sig, signal.SIG_DFL)
-    cloudlog.warning("caching torque params")
-
-    params = Params()
-    params.put("LiveTorqueCarParams", CP.as_builder().to_bytes())
-
-    msg = estimator.get_msg(with_points=True)
-    params.put("LiveTorqueParameters", msg.to_bytes())
-
-    sys.exit(0)
-  if "REPLAY" not in os.environ:
-    signal.signal(signal.SIGINT, cache_params)
-
   while True:
     sm.update()
     if sm.all_checks():
@@ -296,11 +239,14 @@ def main(sm=None, pm=None):
     if sm.frame % 5 == 0:
       pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
 
-    # dp - auto save every 3 mins: 4 hz * 60 * 3 = 720 (3 mins)
-    if sm.frame % 720 == 0:
-      put_nonblocking("LiveTorqueCarParams", CP.as_builder().to_bytes())
-      msg = estimator.get_msg(with_points=True)
+    # Cache points every 60 seconds while onroad
+    if sm.frame % 240 == 0:
+      msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
       put_nonblocking("LiveTorqueParameters", msg.to_bytes())
 
 if __name__ == "__main__":
-  main()
+  import argparse
+  parser = argparse.ArgumentParser(description='Process the --demo argument.')
+  parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
+  args = parser.parse_args()
+  main(demo=args.demo)