29 #include "model/comm_can_util.h"
31 static const int kNotFound = -1;
34 static const int kGcThreshold = 100;
37 static const int kGcIntervalSecs = 10;
40 static const int kEntryMaxAgeSecs = 100;
42 typedef struct can_frame CanFrame;
44 bool IsFastMessagePGN(
unsigned pgn) {
45 static const std::vector<unsigned> haystack = {
47 65240u, 126208u, 126464u, 126996u, 126998u, 127233u, 127237u, 127489u,
48 127496u, 127506u, 128275u, 129029u, 129038u, 129039u, 129040u, 129041u,
49 129284u, 129285u, 129540u, 129793u, 129794u, 129795u, 129797u, 129798u,
50 129801u, 129802u, 129808u, 129809u, 129810u, 130065u, 130074u, 130323u,
51 130577u, 130820u, 130822u, 130824u};
53 unsigned needle =
static_cast<unsigned>(pgn);
54 auto found = std::find_if(haystack.begin(), haystack.end(),
55 [needle](
unsigned i) { return i == needle; });
56 return found != haystack.end();
59 unsigned long BuildCanID(
int priority,
int source,
int destination,
int pgn) {
61 unsigned long cid = 0;
62 unsigned char pf = (
unsigned char) (pgn >> 8);
64 cid = ((
unsigned long)(priority & 0x7))<<26 | pgn<<8 | ((
unsigned long)destination)<<8 | (
unsigned long)source;
67 cid = ((
unsigned long)(priority & 0x7))<<26 | pgn<<8 | (
unsigned long)source;
77 buf[0] = frame.can_id & 0xFF;
78 buf[1] = (frame.can_id >> 8) & 0xFF;
79 buf[2] = (frame.can_id >> 16) & 0xFF;
80 buf[3] = (frame.can_id >> 24) & 0xFF;
83 destination = buf[2] < 240 ? buf[1] : 255;
84 pgn = (buf[3] & 0x01) << 16 | (buf[2] << 8) | (buf[2] < 240 ? 0 : buf[1]);
85 priority = (buf[3] & 0x1c) >> 2;
90 return IsFastMessagePGN(
static_cast<unsigned>(pgn));
92 static const std::vector<unsigned> haystack = {
94 65240u, 126208u, 126464u, 126996u, 126998u, 127233u, 127237u, 127489u,
95 127496u, 127506u, 128275u, 129029u, 129038u, 129039u, 129040u, 129041u,
96 129284u, 129285u, 129540u, 129793u, 129794u, 129795u, 129797u, 129798u,
97 129801u, 129802u, 129808u, 129809u, 129810u, 130065u, 130074u, 130323u,
98 130577u, 130820u, 130822u, 130824u};
100 unsigned needle =
static_cast<unsigned>(pgn);
101 auto found = std::find_if(haystack.begin(), haystack.end(),
102 [needle](
unsigned i) { return i == needle; });
103 return found != haystack.end();
109 bool FastMessageMap::IsEntryExpired(
unsigned int i) {
110 return (wxDateTime::Now() - entries[i].time_arrived
111 > wxTimeSpan(0, 0, kEntryMaxAgeSecs));
114 void FastMessageMap::CheckGc() {
115 bool last_run_over_age = (wxDateTime::Now() - last_gc_run) > wxTimeSpan(0, 0, kGcIntervalSecs);
116 if (last_run_over_age || entries.size() > kGcThreshold) {
118 last_gc_run = wxDateTime::Now();
123 const unsigned char sid) {
124 for (
unsigned i = 0; i < entries.size(); i++) {
125 if (((sid & 0xE0) == (entries[i].sid & 0xE0)) &&
126 (entries[i].header.pgn == header.pgn) &&
127 (entries[i].header.source == header.source) &&
128 (entries[i].header.destination == header.destination)) {
136 entries.push_back(
Entry());
137 return entries.size() - 1;
140 int FastMessageMap::GarbageCollector(
void) {
141 std::vector<unsigned> stale_entries;
146 for (
unsigned i = 0; i < entries.size(); i++) {
147 if (IsEntryExpired(i)) {
160 const unsigned char* data,
int index) {
168 if ((data[0] & 0x1F) == 0) {
171 total_data_len =
static_cast<unsigned int>(data[1]);
172 total_data_len += 7 - ((total_data_len - 6) % 7);
174 entries[index].sid =
static_cast<unsigned int>(data[0]);
175 entries[index].expected_length =
static_cast<unsigned int>(data[1]);
176 entries[index].header = header;
177 entries[index].time_arrived = wxDateTime::Now();
179 entries[index].data.resize(total_data_len);
180 memcpy(&entries[index].data[0], &data[2], 6);
183 entries[index].cursor = 6;
186 return entries[index].expected_length <= 6;
194 const unsigned char* data,
int position) {
196 if ((entries[position].sid + 1) == data[0]) {
197 memcpy(&entries[position].data[entries[position].cursor], &data[1], 7);
198 entries[position].sid = data[0];
201 entries[position].cursor += 7;
203 return entries[position].cursor >= entries[position].expected_length;
204 }
else if ((data[0] & 0x1F) == 0) {
210 entries.erase(entries.begin() + position);
220 entries.erase(entries.begin() + position);
222 if (dropped_frames == 0) {
223 dropped_frame_time = wxDateTime::Now();
242 if ((
unsigned int)(pos + 1) >= entries.size())
243 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.