parser.cc 7.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  1. #include <algorithm>
  2. #include <cassert>
  3. #include <cstring>
  4. #include <limits>
  5. #include <stdexcept>
  6. #include <sstream>
  7. #include <unistd.h>
  8. #include <fcntl.h>
  9. #include <sys/stat.h>
  10. #include <sys/mman.h>
  11. #include "candbc.h"
  12. int64_t get_raw_value(const std::vector<uint8_t> &msg, const Signal &sig) {
  13. int64_t ret = 0;
  14. int i = sig.msb / 8;
  15. int bits = sig.size;
  16. while (i >= 0 && i < msg.size() && bits > 0) {
  17. int lsb = (int)(sig.lsb / 8) == i ? sig.lsb : i*8;
  18. int msb = (int)(sig.msb / 8) == i ? sig.msb : (i+1)*8 - 1;
  19. int size = msb - lsb + 1;
  20. uint64_t d = (msg[i] >> (lsb - (i*8))) & ((1ULL << size) - 1);
  21. ret |= d << (bits - size);
  22. bits -= size;
  23. i = sig.is_little_endian ? i-1 : i+1;
  24. }
  25. return ret;
  26. }
  27. bool MessageState::parse(uint64_t nanos, const std::vector<uint8_t> &dat) {
  28. std::vector<double> tmp_vals(parse_sigs.size());
  29. bool checksum_failed = false;
  30. bool counter_failed = false;
  31. for (int i = 0; i < parse_sigs.size(); i++) {
  32. const auto &sig = parse_sigs[i];
  33. int64_t tmp = get_raw_value(dat, sig);
  34. if (sig.is_signed) {
  35. tmp -= ((tmp >> (sig.size-1)) & 0x1) ? (1ULL << sig.size) : 0;
  36. }
  37. //DEBUG("parse 0x%X %s -> %ld\n", address, sig.name, tmp);
  38. if (!ignore_checksum) {
  39. if (sig.calc_checksum != nullptr && sig.calc_checksum(address, sig, dat) != tmp) {
  40. checksum_failed = true;
  41. }
  42. }
  43. if (!ignore_counter) {
  44. if (sig.type == SignalType::COUNTER && !update_counter_generic(tmp, sig.size)) {
  45. counter_failed = true;
  46. }
  47. }
  48. tmp_vals[i] = tmp * sig.factor + sig.offset;
  49. }
  50. // only update values if both checksum and counter are valid
  51. if (checksum_failed || counter_failed) {
  52. LOGE_100("0x%X message checks failed, checksum failed %d, counter failed %d", address, checksum_failed, counter_failed);
  53. return false;
  54. }
  55. for (int i = 0; i < parse_sigs.size(); i++) {
  56. vals[i] = tmp_vals[i];
  57. all_vals[i].push_back(vals[i]);
  58. }
  59. last_seen_nanos = nanos;
  60. return true;
  61. }
  62. bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
  63. if (((counter + 1) & ((1 << cnt_size) -1)) != v) {
  64. counter_fail = std::min(counter_fail + 1, MAX_BAD_COUNTER);
  65. if (counter_fail > 1) {
  66. INFO("0x%X COUNTER FAIL #%d -- %d -> %d\n", address, counter_fail, counter, (int)v);
  67. }
  68. } else if (counter_fail > 0) {
  69. counter_fail--;
  70. }
  71. counter = v;
  72. return counter_fail < MAX_BAD_COUNTER;
  73. }
  74. CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector<std::pair<uint32_t, int>> &messages)
  75. : bus(abus) {
  76. dbc = dbc_lookup(dbc_name);
  77. assert(dbc);
  78. bus_timeout_threshold = std::numeric_limits<uint64_t>::max();
  79. for (const auto& [address, frequency] : messages) {
  80. // disallow duplicate message checks
  81. if (message_states.find(address) != message_states.end()) {
  82. std::stringstream is;
  83. is << "Duplicate Message Check: " << address;
  84. throw std::runtime_error(is.str());
  85. }
  86. MessageState &state = message_states[address];
  87. state.address = address;
  88. // state.check_frequency = op.check_frequency,
  89. // msg is not valid if a message isn't received for 10 consecutive steps
  90. if (frequency > 0) {
  91. state.check_threshold = (1000000000ULL / frequency) * 10;
  92. // bus timeout threshold should be 10x the fastest msg
  93. bus_timeout_threshold = std::min(bus_timeout_threshold, state.check_threshold);
  94. }
  95. const Msg *msg = dbc->addr_to_msg.at(address);
  96. state.name = msg->name;
  97. state.size = msg->size;
  98. assert(state.size <= 64); // max signal size is 64 bytes
  99. // track all signals for this message
  100. state.parse_sigs = msg->sigs;
  101. state.vals.resize(msg->sigs.size());
  102. state.all_vals.resize(msg->sigs.size());
  103. }
  104. }
  105. CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum, bool ignore_counter)
  106. : bus(abus) {
  107. // Add all messages and signals
  108. dbc = dbc_lookup(dbc_name);
  109. assert(dbc);
  110. for (const auto& msg : dbc->msgs) {
  111. MessageState state = {
  112. .name = msg.name,
  113. .address = msg.address,
  114. .size = msg.size,
  115. .ignore_checksum = ignore_checksum,
  116. .ignore_counter = ignore_counter,
  117. };
  118. for (const auto& sig : msg.sigs) {
  119. state.parse_sigs.push_back(sig);
  120. state.vals.push_back(0);
  121. state.all_vals.push_back({});
  122. }
  123. message_states[state.address] = state;
  124. }
  125. }
  126. void CANParser::update(const std::vector<CanData> &can_data, std::vector<SignalValue> &vals) {
  127. uint64_t current_nanos = 0;
  128. for (const auto &c : can_data) {
  129. if (first_nanos == 0) {
  130. first_nanos = c.nanos;
  131. }
  132. if (current_nanos == 0) {
  133. current_nanos = c.nanos;
  134. }
  135. last_nanos = c.nanos;
  136. UpdateCans(c);
  137. UpdateValid(last_nanos);
  138. }
  139. query_latest(vals, current_nanos);
  140. }
  141. void CANParser::UpdateCans(const CanData &can) {
  142. //DEBUG("got %zu messages\n", can.frames.size());
  143. bool bus_empty = true;
  144. for (const auto &frame : can.frames) {
  145. if (frame.src != bus) {
  146. // DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
  147. continue;
  148. }
  149. bus_empty = false;
  150. auto state_it = message_states.find(frame.address);
  151. if (state_it == message_states.end()) {
  152. // DEBUG("skip %d: not specified\n", cmsg.getAddress());
  153. continue;
  154. }
  155. if (frame.dat.size() > 64) {
  156. DEBUG("got message longer than 64 bytes: 0x%X %zu\n", frame.address, frame.dat.size());
  157. continue;
  158. }
  159. // TODO: this actually triggers for some cars. fix and enable this
  160. //if (dat.size() != state_it->second.size) {
  161. // DEBUG("got message with unexpected length: expected %d, got %zu for %d", state_it->second.size, dat.size(), cmsg.getAddress());
  162. // continue;
  163. //}
  164. state_it->second.parse(can.nanos, frame.dat);
  165. }
  166. // update bus timeout
  167. if (!bus_empty) {
  168. last_nonempty_nanos = can.nanos;
  169. }
  170. bus_timeout = (can.nanos - last_nonempty_nanos) > bus_timeout_threshold;
  171. }
  172. void CANParser::UpdateValid(uint64_t nanos) {
  173. const bool show_missing = (nanos - first_nanos) > 8e9;
  174. bool _valid = true;
  175. bool _counters_valid = true;
  176. for (const auto& kv : message_states) {
  177. const auto& state = kv.second;
  178. if (state.counter_fail >= MAX_BAD_COUNTER) {
  179. _counters_valid = false;
  180. }
  181. const bool missing = state.last_seen_nanos == 0;
  182. const bool timed_out = (nanos - state.last_seen_nanos) > state.check_threshold;
  183. if (state.check_threshold > 0 && (missing || timed_out)) {
  184. if (show_missing && !bus_timeout) {
  185. if (missing) {
  186. LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str());
  187. } else if (timed_out) {
  188. LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str());
  189. }
  190. }
  191. _valid = false;
  192. }
  193. }
  194. can_invalid_cnt = _valid ? 0 : (can_invalid_cnt + 1);
  195. can_valid = (can_invalid_cnt < CAN_INVALID_CNT) && _counters_valid;
  196. }
  197. void CANParser::query_latest(std::vector<SignalValue> &vals, uint64_t last_ts) {
  198. if (last_ts == 0) {
  199. last_ts = last_nanos;
  200. }
  201. for (auto& kv : message_states) {
  202. auto& state = kv.second;
  203. if (last_ts != 0 && state.last_seen_nanos < last_ts) {
  204. continue;
  205. }
  206. for (int i = 0; i < state.parse_sigs.size(); i++) {
  207. const Signal &sig = state.parse_sigs[i];
  208. SignalValue &v = vals.emplace_back();
  209. v.address = state.address;
  210. v.ts_nanos = state.last_seen_nanos;
  211. v.name = sig.name;
  212. v.value = state.vals[i];
  213. v.all_values = state.all_vals[i];
  214. state.all_vals[i].clear();
  215. }
  216. }
  217. }