CANParser: add field for all values from a cycle (#558)

* could work

* should be light-speed

* clean up

* clean up

* make sure updated is empty if nothing is updated

updating without a msg is undefined behavior

* should be fast enough

* more test

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

can/common.h

@@ -32,6 +32,7 @@ public:
 
   std::vector<Signal> parse_sigs;
   std::vector<double> vals;
+  std::vector<std::vector<double>> all_vals;
 
   uint64_t seen;
   uint64_t check_threshold;

can/common.pxd

@@ -62,6 +62,7 @@ cdef extern from "common_dbc.h":
     uint32_t address
     const char* name
     double value
+    vector[double] all_values
 
   cdef struct SignalPackValue:
     string name

can/common_dbc.h

@@ -25,7 +25,8 @@ struct MessageParseOptions {
 struct SignalValue {
   uint32_t address;
   const char* name;
-  double value;
+  double value;  // latest value
+  std::vector<double> all_values;  // all values from this cycle
 };
 
 enum SignalType {

can/parser.cc

@@ -83,6 +83,7 @@ bool MessageState::parse(uint64_t sec, uint8_t * dat) {
     }
 
     vals[i] = tmp * sig.factor + sig.offset;
+    all_vals[i].push_back(vals[i]);
   }
   seen = sec;
 
@@ -147,6 +148,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
       if (sig->type != SignalType::DEFAULT) {
         state.parse_sigs.push_back(*sig);
         state.vals.push_back(0);
+        state.all_vals.push_back({});
       }
     }
 
@@ -160,6 +162,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name,
             && sig->type == SignalType::DEFAULT) {
           state.parse_sigs.push_back(*sig);
           state.vals.push_back(0);
+          state.all_vals.push_back({});
           break;
         }
       }
@@ -188,6 +191,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum
       const Signal *sig = &msg->sigs[j];
       state.parse_sigs.push_back(*sig);
       state.vals.push_back(0);
+      state.all_vals.push_back({});
     }
 
     message_states[state.address] = state;
@@ -209,7 +213,7 @@ void CANParser::update_string(const std::string &data, bool sendcan) {
 
   last_sec = event.getLogMonoTime();
 
-  auto cans = sendcan? event.getSendcan() : event.getCan();
+  auto cans = sendcan ? event.getSendcan() : event.getCan();
   UpdateCans(last_sec, cans);
 
   UpdateValid(last_sec);
@@ -282,17 +286,19 @@ void CANParser::UpdateValid(uint64_t sec) {
 std::vector<SignalValue> CANParser::query_latest() {
   std::vector<SignalValue> ret;
 
-  for (const auto& kv : message_states) {
-    const auto& state = kv.second;
+  for (auto& kv : message_states) {
+    auto& state = kv.second;
     if (last_sec != 0 && state.seen != last_sec) continue;
 
-    for (int i=0; i<state.parse_sigs.size(); i++) {
+    for (int i = 0; i < state.parse_sigs.size(); i++) {
       const Signal &sig = state.parse_sigs[i];
       ret.push_back((SignalValue){
         .address = state.address,
         .name = sig.name,
         .value = state.vals[i],
+        .all_values = state.all_vals[i],
       });
+      state.all_vals[i].clear();
     }
   }
 

can/parser_pyx.pyx

@@ -5,8 +5,8 @@ from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp.unordered_set cimport unordered_set
 from libc.stdint cimport uint32_t, uint64_t, uint16_t
-from libcpp.map cimport map
 from libcpp cimport bool
+from libcpp.map cimport map
 
 from .common cimport CANParser as cpp_CANParser
 from .common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC
@@ -17,6 +17,7 @@ from collections import defaultdict
 
 cdef int CAN_INVALID_CNT = 5
 
+
 cdef class CANParser:
   cdef:
     cpp_CANParser *can
@@ -27,6 +28,7 @@ cdef class CANParser:
 
   cdef readonly:
     dict vl
+    dict vl_all
     bool can_valid
     string dbc_name
     int can_invalid_cnt
@@ -41,6 +43,7 @@ cdef class CANParser:
       raise RuntimeError(f"Can't find DBC: {dbc_name}")
 
     self.vl = {}
+    self.vl_all = {}
     self.can_valid = False
     self.can_invalid_cnt = CAN_INVALID_CNT
 
@@ -54,6 +57,8 @@ cdef class CANParser:
       self.address_to_msg_name[msg.address] = name
       self.vl[msg.address] = {}
       self.vl[name] = self.vl[msg.address]
+      self.vl_all[msg.address] = defaultdict(list)
+      self.vl_all[name] = self.vl_all[msg.address]
 
     # Convert message names into addresses
     for i in range(len(signals)):
@@ -99,38 +104,41 @@ cdef class CANParser:
     self.update_vl()
 
   cdef unordered_set[uint32_t] update_vl(self):
-    cdef string sig_name
-    cdef unordered_set[uint32_t] updated_val
-
-    can_values = self.can.query_latest()
-    valid = self.can.can_valid
+    cdef unordered_set[uint32_t] updated_addrs
 
     # Update invalid flag
     self.can_invalid_cnt += 1
-    if valid:
-        self.can_invalid_cnt = 0
+    if self.can.can_valid:
+      self.can_invalid_cnt = 0
     self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
 
-    for cv in can_values:
+    new_vals = self.can.query_latest()
+    for cv in new_vals:
       # Cast char * directly to unicode
       cv_name = <unicode>cv.name
       self.vl[cv.address][cv_name] = cv.value
-      updated_val.insert(cv.address)
+      self.vl_all[cv.address][cv_name].extend(cv.all_values)
+      updated_addrs.insert(cv.address)
 
-    return updated_val
+    return updated_addrs
 
   def update_string(self, dat, sendcan=False):
+    for v in self.vl_all.values():
+      v.clear()
+
     self.can.update_string(dat, sendcan)
     return self.update_vl()
 
   def update_strings(self, strings, sendcan=False):
-    updated_vals = set()
+    for v in self.vl_all.values():
+      v.clear()
 
+    updated_addrs = set()
     for s in strings:
-      updated_val = self.update_string(s, sendcan)
-      updated_vals.update(updated_val)
+      updated_addrs = self.update_string(s, sendcan)
+      updated_addrs.update(updated_addrs)
+    return updated_addrs
 
-    return updated_vals
 
 cdef class CANDefine():
   cdef:
@@ -172,7 +180,6 @@ cdef class CANDefine():
       values = [int(v) for v in def_val[::2]]
       defs = def_val[1::2]
 
-
       # two ways to lookup: address or msg name
       dv[address][sgname] = dict(zip(values, defs))
       dv[msgname][sgname] = dv[address][sgname]

can/tests/test_packer_parser.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 import unittest
+import random
 
 import cereal.messaging as messaging
 from opendbc.can.parser import CANParser
@@ -122,6 +123,36 @@ class TestCanParserPacker(unittest.TestCase):
 
         idx += 1
 
+  def test_updated(self):
+    """Test updated value dict"""
+    dbc_file = "honda_civic_touring_2016_can_generated"
+
+    signals = [("USER_BRAKE", "VSA_STATUS")]
+    checks = [("VSA_STATUS", 50)]
+
+    parser = CANParser(dbc_file, signals, checks, 0)
+    packer = CANPacker(dbc_file)
+
+    # Make sure nothing is updated
+    self.assertEqual(len(parser.vl_all["VSA_STATUS"]["USER_BRAKE"]), 0)
+
+    idx = 0
+    for _ in range(10):
+      # Ensure CANParser holds the values of any duplicate messages
+      user_brake_vals = [random.randrange(100) for _ in range(random.randrange(10))]
+      msgs = []
+      for user_brake in user_brake_vals:
+        values = {"USER_BRAKE": user_brake}
+        msgs.append(packer.make_can_msg("VSA_STATUS", 0, values, idx))
+        idx += 1
+
+      parser.update_strings((can_list_to_can_capnp(msgs), ))
+      vl_all = parser.vl_all["VSA_STATUS"]["USER_BRAKE"]
+
+      self.assertEqual(vl_all, user_brake_vals)
+      if len(user_brake_vals):
+        self.assertEqual(vl_all[-1], parser.vl["VSA_STATUS"]["USER_BRAKE"])
+
 
 if __name__ == "__main__":
   unittest.main()