angle later (#97)

* Auto Update. (commaai#770) * auto update. Co-Authored-By: Shane Smiskol <shane@smiskol.com> * test auto update. * skip E1101 * Revert "08 e2e (#94)" This reverts commit 677c020. Co-authored-by: Shane Smiskol <shane@smiskol.com> Co-authored-by: sebastian4k <69202924+sebastian4k@users.noreply.github.com> * add LatControl * add latControl * add latControl * send latControl * Add , 'latControl' for planner * make latControl available in carstate Co-authored-by: Shane Smiskol <shane@smiskol.com> Co-authored-by: sebastian4k <69202924+sebastian4k@users.noreply.github.com> Co-authored-by: Arne Schwarck <arneschwarck@gmail.com>
rav4kumar · Jan 15, 2021 · c181e0b · c181e0b
1 parent 4cd38fb
commit c181e0b
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 3 deletions.
diff --git a/cereal/log.capnp b/cereal/log.capnp
@@ -2066,6 +2066,10 @@ struct Sentinel {
   type @0 :SentinelType;
 }
 
+struct LatControl {
+  anglelater @0 :Float32;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -2150,6 +2154,7 @@ struct Event {
     wideEncodeIdx @77 :EncodeIndex;
     dragonConf @78 :DragonConf;
     liveTrafficData @79:LiveTrafficData;
+    latControl @80: LatControl;
   }
 }
 

diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml
@@ -88,6 +88,7 @@ testLiveLocation: [8045, false, 0.]
 testJoystick: [8056, false, 0.]
 
 liveTrafficData: [8208, false, 100.]
+latControl: [8210, false, 20.]
 
 
 # 8080 is reserved for slave testing daemon

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
@@ -36,7 +36,7 @@ def __init__(self, CP):
     #self.read_distance_lines = 0
     if not travis:
       self.pm = messaging.PubMaster(['liveTrafficData'])
-      self.sm = messaging.SubMaster(['liveMapData'])#',latControl',])
+      self.sm = messaging.SubMaster(['liveMapData','latControl'])
     # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
     # is zeroed to where the steering angle is at start.
     # Need to apply an offset as soon as the steering angle measurements are both received

diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
@@ -280,3 +280,7 @@ def update(self, sm, pm, CP, VM):
       dat.liveMpc.delta = list(self.mpc_solution[0].delta)
       dat.liveMpc.cost = self.mpc_solution[0].cost
       pm.send('liveMpc', dat)
+
+    msg = messaging.new_message('latControl')
+    msg.latControl.anglelater = math.degrees(list(self.mpc_solution[0].delta)[-1])
+    pm.send('latControl', msg)
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
@@ -23,11 +23,11 @@ def plannerd_thread(sm=None, pm=None):
   VM = VehicleModel(CP)
 
   if sm is None:
-    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'],
+    sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData', 'latControl'],
                              poll=['radarState', 'model'])
 
   if pm is None:
-    pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc'])
+    pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc', 'latControl'])
 
   sm['liveParameters'].valid = True
   sm['liveParameters'].sensorValid = True