OpenCPN Partial API docs
Loading...
Searching...
No Matches
comm_can_util.cpp
Go to the documentation of this file.
1/***************************************************************************
2 * Copyright (C) 2024 by David Register *
3 * Copyright (C) 2024 Alec Leamas *
4 * *
5 * This program is free software; you can redistribute it and/or modify *
6 * it under the terms of the GNU General Public License as published by *
7 * the Free Software Foundation; either version 2 of the License, or *
8 * (at your option) any later version. *
9 * *
10 * This program is distributed in the hope that it will be useful, *
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13 * GNU General Public License for more details. *
14 * *
15 * You should have received a copy of the GNU General Public License *
16 * along with this program; if not, see <https://www.gnu.org/licenses/>. *
17 **************************************************************************/
18
25#include <algorithm>
26#include <vector>
27
28#include "model/comm_can_util.h"
29
30static const int kNotFound = -1;
31
33static const int kGcThreshold = 100;
34
36static const int kGcIntervalSecs = 10;
37
39static const int kEntryMaxAgeSecs = 100;
40
41typedef struct can_frame CanFrame;
42
43bool IsFastMessagePGN(unsigned pgn) {
44 static const std::vector<unsigned> haystack = {
45 // All known multiframe fast messages
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};
51
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();
56}
57
58unsigned long BuildCanID(int priority, int source, int destination, int pgn) {
59 // build CanID
60 unsigned long cid = 0;
61 unsigned char pf = (unsigned char)(pgn >> 8);
62 if (pf < 240) {
63 cid = ((unsigned long)(priority & 0x7)) << 26 | pgn << 8 |
64 ((unsigned long)destination) << 8 | (unsigned long)source;
65 } else {
66 cid = ((unsigned long)(priority & 0x7)) << 26 | pgn << 8 |
67 (unsigned long)source;
68 }
69 return cid;
70}
71
72// CanHeader implementation
73
74CanHeader::CanHeader(const CanFrame frame) {
75 unsigned char buf[4];
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;
80
81 source = buf[0];
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;
85}
86
88 return IsFastMessagePGN(static_cast<unsigned>(pgn));
89#if 0
90 static const std::vector<unsigned> haystack = {
91 // All known multiframe fast messages
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};
97
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();
102#endif
103}
104
105// FastMessage implementation
106
107bool FastMessageMap::IsEntryExpired(unsigned int i) {
108 return (wxDateTime::Now() - entries[i].time_arrived >
109 wxTimeSpan(0, 0, kEntryMaxAgeSecs));
110}
111
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) {
116 GarbageCollector();
117 last_gc_run = wxDateTime::Now();
118 }
119}
120
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)) {
128 return i;
129 }
130 }
131 return kNotFound;
132}
133
135 entries.push_back(Entry());
136 return entries.size() - 1;
137}
138
139int FastMessageMap::GarbageCollector() {
140 std::vector<unsigned> stale_entries;
141 bool bremoved;
142 int nremoved = 0;
143 do {
144 bremoved = false;
145 for (unsigned i = 0; i < entries.size(); i++) {
146 if (IsEntryExpired(i)) {
147 Remove(i);
148 nremoved++;
149 bremoved = true;
150 break;
151 }
152 }
153 } while (bremoved);
154
155 return nremoved;
156}
157
159 const unsigned char* data, int index) {
160 // first message of fast packet
161 // data[0] Sequence Identifier (sid)
162 // data[1] Length of data bytes
163 // data[2..7] 6 data bytes
164
165 CheckGc();
166 // Ensure that this is indeed the first frame of a fast message
167 if ((data[0] & 0x1F) == 0) {
168 int total_data_len; // will also include padding as we memcpy all of the
169 // frame, because I'm lazy
170 total_data_len = static_cast<unsigned int>(data[1]);
171 total_data_len += 7 - ((total_data_len - 6) % 7);
172
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();
177
178 entries[index].data.resize(total_data_len);
179 memcpy(&entries[index].data[0], &data[2], 6);
180 // First frame of a multi-frame Fast Message contains six data bytes.
181 // Position the cursor ready for next message
182 entries[index].cursor = 6;
183
184 // Fusion, using fast messages to sends frames less than eight bytes
185 return entries[index].expected_length <= 6;
186 }
187 return false;
188 // No further processing is performed if this is not a start frame.
189 // A start frame may have been dropped and we received a subsequent frame
190}
191
193 const unsigned char* data, int position) {
194 // Check that this is the next message in the sequence
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];
198 // Subsequent messages contains seven data bytes (last message may be padded
199 // with 0xFF)
200 entries[position].cursor += 7;
201 // Is this the last message ?
202 return entries[position].cursor >= entries[position].expected_length;
203 } else if ((data[0] & 0x1F) == 0) {
204 // We've found a matching entry, however this is a start frame, therefore
205 // we've missed an end frame, and now we have a start frame with the same id
206 // (top 3 bits). The id has obviously rolled over. Should really double
207 // check that (data[0] & 0xE0) Clear the entry as we don't want to leak
208 // memory, prior to inserting a start frame
209 entries.erase(entries.begin() + position);
210 position = AddNewEntry();
211 // And now insert it
212 InsertEntry(header, data, position);
213 // FIXME (dave) Should update the dropped frame stats
214 return false;
215 } else {
216 // This is not the next frame in the sequence and not a start frame
217 // We've dropped an intermedite frame, so free the slot and do no further
218 // processing
219 entries.erase(entries.begin() + position);
220 // Dropped Frame Statistics
221 if (dropped_frames == 0) {
222 dropped_frame_time = wxDateTime::Now();
223 dropped_frames += 1;
224 } else {
225 dropped_frames += 1;
226 }
227 // FIXME (dave)
228 // if ((dropped_frames > CONST_DROPPEDFRAME_THRESHOLD) &&
229 // (wxDateTime::Now() < (dropped_frame_time +
230 // wxTimeSpan::Seconds(CONST_DROPPEDFRAME_PERIOD) ) ) ) {
231 // wxLogError("TwoCan Device, Dropped Frames rate exceeded");
232 // wxLogError(wxString::Format(_T("Frame: Source: %d Destination: %d
233 // Priority: %d PGN: %d"),header.source, header.destination,
234 // header.priority, header.pgn)); dropped_frames = 0;
235 // }
236 return false;
237 }
238}
239
241 if ((unsigned int)(pos + 1) <= entries.size())
242 entries.erase(entries.begin() + pos);
243}
CAN v2.0 29 bit header as used by NMEA 2000.
CanHeader()
CAN v2.0 29 bit header as used by NMEA 2000.
bool IsFastMessage() const
Return true if header reflects a multipart fast message.
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.