30static const int kNotFound = -1;
33static const int kGcThreshold = 100;
36static const int kGcIntervalSecs = 10;
39static const int kEntryMaxAgeSecs = 100;
41typedef struct can_frame CanFrame;
43bool IsFastMessagePGN(
unsigned pgn) {
44 static const std::vector<unsigned> haystack = {
46 65240u, 126208u, 126464u, 126996u, 126998u, 127233u, 127237u, 127489u,
47 127496u, 127506u, 128275u, 129029u, 129038u, 129039u, 129040u, 129041u,
48 129284u, 129285u, 129540u, 129793u, 129794u, 129795u, 129797u, 129798u,
49 129801u, 129802u, 129808u, 129809u, 129810u, 130065u, 130074u, 130323u,
50 130577u, 130820u, 130822u, 130824u};
52 unsigned needle =
static_cast<unsigned>(pgn);
53 auto found = std::find_if(haystack.begin(), haystack.end(),
54 [needle](
unsigned i) { return i == needle; });
55 return found != haystack.end();
58unsigned long BuildCanID(
int priority,
int source,
int destination,
int pgn) {
60 unsigned long cid = 0;
61 unsigned char pf = (
unsigned char)(pgn >> 8);
63 cid = ((
unsigned long)(priority & 0x7)) << 26 | pgn << 8 |
64 ((
unsigned long)destination) << 8 | (
unsigned long)source;
66 cid = ((
unsigned long)(priority & 0x7)) << 26 | pgn << 8 |
67 (
unsigned long)source;
76 buf[0] = frame.can_id & 0xFF;
77 buf[1] = (frame.can_id >> 8) & 0xFF;
78 buf[2] = (frame.can_id >> 16) & 0xFF;
79 buf[3] = (frame.can_id >> 24) & 0xFF;
82 destination = buf[2] < 240 ? buf[1] : 255;
83 pgn = (buf[3] & 0x01) << 16 | (buf[2] << 8) | (buf[2] < 240 ? 0 : buf[1]);
84 priority = (buf[3] & 0x1c) >> 2;
88 return IsFastMessagePGN(
static_cast<unsigned>(pgn));
90 static const std::vector<unsigned> haystack = {
92 65240u, 126208u, 126464u, 126996u, 126998u, 127233u, 127237u, 127489u,
93 127496u, 127506u, 128275u, 129029u, 129038u, 129039u, 129040u, 129041u,
94 129284u, 129285u, 129540u, 129793u, 129794u, 129795u, 129797u, 129798u,
95 129801u, 129802u, 129808u, 129809u, 129810u, 130065u, 130074u, 130323u,
96 130577u, 130820u, 130822u, 130824u};
98 unsigned needle =
static_cast<unsigned>(pgn);
99 auto found = std::find_if(haystack.begin(), haystack.end(),
100 [needle](
unsigned i) { return i == needle; });
101 return found != haystack.end();
107bool FastMessageMap::IsEntryExpired(
unsigned int i) {
108 return (wxDateTime::Now() - entries[i].time_arrived >
109 wxTimeSpan(0, 0, kEntryMaxAgeSecs));
112void FastMessageMap::CheckGc() {
113 bool last_run_over_age =
114 (wxDateTime::Now() - last_gc_run) > wxTimeSpan(0, 0, kGcIntervalSecs);
115 if (last_run_over_age || entries.size() > kGcThreshold) {
117 last_gc_run = wxDateTime::Now();
122 const unsigned char sid) {
123 for (
unsigned i = 0; i < entries.size(); i++) {
124 if (((sid & 0xE0) == (entries[i].sid & 0xE0)) &&
125 (entries[i].header.pgn == header.pgn) &&
126 (entries[i].header.source == header.source) &&
127 (entries[i].header.destination == header.destination)) {
135 entries.push_back(
Entry());
136 return entries.size() - 1;
139int FastMessageMap::GarbageCollector() {
140 std::vector<unsigned> stale_entries;
145 for (
unsigned i = 0; i < entries.size(); i++) {
146 if (IsEntryExpired(i)) {
159 const unsigned char* data,
int index) {
167 if ((data[0] & 0x1F) == 0) {
170 total_data_len =
static_cast<unsigned int>(data[1]);
171 total_data_len += 7 - ((total_data_len - 6) % 7);
173 entries[index].sid =
static_cast<unsigned int>(data[0]);
174 entries[index].expected_length =
static_cast<unsigned int>(data[1]);
175 entries[index].header = header;
176 entries[index].time_arrived = wxDateTime::Now();
178 entries[index].data.resize(total_data_len);
179 memcpy(&entries[index].data[0], &data[2], 6);
182 entries[index].cursor = 6;
185 return entries[index].expected_length <= 6;
193 const unsigned char* data,
int position) {
195 if ((entries[position].sid + 1) == data[0]) {
196 memcpy(&entries[position].data[entries[position].cursor], &data[1], 7);
197 entries[position].sid = data[0];
200 entries[position].cursor += 7;
202 return entries[position].cursor >= entries[position].expected_length;
203 }
else if ((data[0] & 0x1F) == 0) {
209 entries.erase(entries.begin() + position);
219 entries.erase(entries.begin() + position);
221 if (dropped_frames == 0) {
222 dropped_frame_time = wxDateTime::Now();
241 if ((
unsigned int)(pos + 1) <= entries.size())
242 entries.erase(entries.begin() + pos);
int AddNewEntry(void)
Allocate a new, fresh entry and return index to it.
void Remove(int pos)
Remove entry at pos.
bool AppendEntry(const CanHeader hdr, const unsigned char *data, int index)
Append fragment to existing multipart message.
int FindMatchingEntry(const CanHeader header, const unsigned char sid)
Setter.
bool InsertEntry(const CanHeader header, const unsigned char *data, int index)
Insert a new entry, first part of a multipart message.
Low-level socketcan utility functions.