Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : #include "SIM_config.h"
16 :
17 : #if AP_SIM_RF_LIGHTWAR_GRF250_ENABLED
18 :
19 : #include "SIM_RF_LightWare_GRF.h"
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <string.h>
22 :
23 : using namespace SITL;
24 :
25 : #define LIGHTWARE_GRF250_PREAMBLE 0xAA
26 :
27 3432 : uint16_t RF_LightWareGRF::compute_crc(const uint8_t *buf, uint8_t len)
28 : {
29 3432 : uint16_t crc = 0;
30 44558 : for (uint8_t i = 0; i < len; i++) {
31 41126 : crc = crc_xmodem_update(crc, buf[i]);
32 : }
33 3432 : return crc;
34 : }
35 :
36 101112 : void RF_LightWareGRF::process_input()
37 : {
38 101112 : char buffer[50] {};
39 :
40 101112 : ssize_t n = read_from_autopilot(buffer, sizeof(buffer));
41 :
42 101112 : if (n <= 0) {
43 101104 : return;
44 : }
45 :
46 86 : for (ssize_t i = 0; i < n; i++) {
47 78 : if (rxlen < sizeof(rxbuf)) {
48 78 : rxbuf[rxlen++] = (uint8_t)buffer[i];
49 : }
50 : }
51 :
52 : // Try to decode messages in rxbuf[]
53 8 : try_parse_message();
54 : }
55 :
56 15 : void RF_LightWareGRF::try_parse_message()
57 : {
58 : // Need at least preamble + flags + msgid
59 15 : if (rxlen < 6) {
60 0 : return;
61 : }
62 :
63 : // find preamble in buffer
64 15 : uint8_t *p = (uint8_t*)memchr(rxbuf, LIGHTWARE_GRF250_PREAMBLE, rxlen);
65 15 : if (!p) {
66 : // No preamble in the entire buffer, clear buffer
67 0 : rxlen = 0;
68 0 : return;
69 : }
70 :
71 : // If preamble is not at index 0, slide data to front
72 15 : uint8_t offset = p - rxbuf;
73 15 : if (offset > 0) {
74 2 : memmove(rxbuf, p, rxlen - offset);
75 2 : rxlen -= offset;
76 :
77 : // Need at least 6 bytes again
78 2 : if (rxlen < 6) {
79 0 : return;
80 : }
81 : }
82 :
83 : // Extract flags and payload length
84 15 : uint16_t flags = (rxbuf[1] | (rxbuf[2] << 8));
85 15 : uint8_t plen = ((flags >> 6) - 1);
86 :
87 15 : if (plen > 100) {
88 : // invalid payload, drop preamble and retry
89 0 : memmove(rxbuf, rxbuf + 1, --rxlen);
90 0 : return;
91 : }
92 :
93 : // Full message length: preamble + flags(2) + msgid + payload + crc(2)
94 15 : const uint8_t need = 1 + 2 + 1 + plen + 2;
95 :
96 15 : if (rxlen <= need) {
97 : // Not enough bytes yet
98 8 : return;
99 : }
100 :
101 7 : const uint16_t gotcrc = uint16_t(uint8_t(rxbuf[need-2])) |
102 7 : (uint16_t(uint8_t(rxbuf[need-1])) << 8);
103 7 : const uint16_t calcrc = compute_crc(rxbuf, need - 2);
104 :
105 7 : if (gotcrc == calcrc) {
106 :
107 7 : const MsgID msgid = (MsgID)rxbuf[3];
108 7 : switch (msgid) {
109 :
110 2 : case MsgID::PRODUCT_NAME:
111 2 : send_product_name();
112 2 : break;
113 :
114 2 : case MsgID::UPDATE_RATE: {
115 2 : uint8_t rate = rxbuf[4];
116 2 : if (rate == 0) {
117 0 : rate = 1;
118 : }
119 2 : update_period_ms = 1000 / rate;
120 2 : send_ack_u32(MsgID::UPDATE_RATE, (uint32_t)rate);
121 2 : break;
122 : }
123 :
124 2 : case MsgID::DISTANCE_OUTPUT: {
125 : uint32_t desired_fields;
126 2 : memcpy(&desired_fields, &rxbuf[4], 4);
127 2 : send_ack_u32(MsgID::DISTANCE_OUTPUT, desired_fields);
128 2 : break;
129 : }
130 :
131 1 : case MsgID::STREAM: {
132 : uint32_t stream;
133 1 : memcpy(&stream, &rxbuf[4], 4);
134 1 : if (stream == 5) {
135 : // only "5" (distance cm stream) is supported for now
136 1 : stream_enabled = true;
137 : } else {
138 0 : stream_enabled = false;
139 0 : break;
140 : }
141 1 : send_ack_u32(MsgID::STREAM, stream);
142 1 : break;
143 : }
144 :
145 0 : default:
146 0 : break;
147 : }
148 : }
149 :
150 7 : uint8_t remaining = rxlen - need;
151 7 : memmove(rxbuf, rxbuf + need, remaining);
152 7 : rxlen = remaining;
153 :
154 7 : if (rxlen >= 4) {
155 7 : try_parse_message();
156 : }
157 : }
158 :
159 2 : void RF_LightWareGRF::send_product_name()
160 : {
161 2 : uint8_t payload[7] = {'G','R','F','2','5','0',0};
162 : uint8_t msg[32];
163 2 : uint8_t len = 0;
164 :
165 2 : msg[len++] = LIGHTWARE_GRF250_PREAMBLE;
166 :
167 2 : uint16_t flags = 0x1;
168 2 : flags |= (1 + sizeof(payload)) << 6;
169 2 : msg[len++] = flags & 0xFF;
170 2 : msg[len++] = (flags >> 8) & 0xFF;
171 :
172 2 : msg[len++] = (uint8_t)MsgID::PRODUCT_NAME;
173 :
174 2 : memcpy(&msg[len], payload, sizeof(payload));
175 2 : len += sizeof(payload);
176 :
177 2 : uint16_t crc = compute_crc(msg, len);
178 2 : msg[len++] = crc & 0xFF;
179 2 : msg[len++] = crc >> 8;
180 :
181 2 : write_to_autopilot((char*)msg, len);
182 2 : }
183 :
184 0 : void RF_LightWareGRF::send_ack_u8(MsgID id, uint8_t value)
185 : {
186 : uint8_t msg[16];
187 0 : uint8_t len = 0;
188 :
189 0 : msg[len++] = LIGHTWARE_GRF250_PREAMBLE;
190 :
191 : // payload_len = 1 (msgid) + 1 (value) = 2
192 0 : uint16_t payload_len = 2;
193 :
194 0 : uint16_t flags = 0x1;
195 0 : flags |= (payload_len << 6);
196 :
197 0 : msg[len++] = flags & 0xFF; // FLAGS_L
198 0 : msg[len++] = (flags >> 8) & 0xFF; // FLAGS_H
199 :
200 0 : msg[len++] = (uint8_t)id; // MSG_ID
201 0 : msg[len++] = value; // PAYLOAD
202 :
203 0 : uint16_t crc = compute_crc(msg, len);
204 :
205 0 : msg[len++] = crc & 0xFF;
206 0 : msg[len++] = (crc >> 8) & 0xFF;
207 :
208 0 : write_to_autopilot((char*)msg, len);
209 0 : }
210 :
211 5 : void RF_LightWareGRF::send_ack_u32(MsgID id, uint32_t value)
212 : {
213 : uint8_t msg[24];
214 5 : uint8_t len = 0;
215 :
216 5 : msg[len++] = LIGHTWARE_GRF250_PREAMBLE;
217 :
218 5 : uint16_t flags = 0x1;
219 5 : flags |= (1 + 4) << 6; // msgid + 4 byte payload
220 5 : msg[len++] = flags & 0xFF;
221 5 : msg[len++] = flags >> 8;
222 :
223 5 : msg[len++] = (uint8_t)id;
224 :
225 5 : memcpy(&msg[len], &value, 4);
226 5 : len += 4;
227 :
228 5 : uint16_t crc = compute_crc(msg, len);
229 5 : msg[len++] = crc & 0xFF;
230 5 : msg[len++] = crc >> 8;
231 :
232 5 : write_to_autopilot((char*)msg, len);
233 5 : }
234 :
235 3418 : void RF_LightWareGRF::build_distance_packet(float alt_m, uint8_t *out, uint32_t &outlen)
236 : {
237 3418 : float dist_m = alt_m;
238 3418 : uint32_t strength = 100; // no way to get this from the sim
239 :
240 : // simulate out-of-range
241 3418 : if (dist_m < 0.2) {
242 2499 : dist_m = 0.2;
243 2499 : strength = 0;
244 : }
245 3418 : if (dist_m > 500) {
246 0 : dist_m = 500;
247 0 : strength = 0;
248 : }
249 :
250 : /*
251 : For some reason the GRF expects the distance in cm, but sends it in weird units where:
252 : raw = UINT32(...)
253 : distance_cm = raw * 10
254 :
255 : So we must send:
256 : raw = (distance_cm / 10)
257 : */
258 :
259 3418 : const uint32_t cm = uint32_t(dist_m * 100.0f);
260 3418 : uint32_t raw = cm / 10;
261 :
262 3418 : uint8_t len = 0;
263 :
264 3418 : out[len++] = LIGHTWARE_GRF250_PREAMBLE;
265 :
266 3418 : uint16_t flags = 0x1;
267 3418 : flags |= (1 + 8) << 6; // msgid + 8 byte payload
268 3418 : out[len++] = flags & 0xFF;
269 3418 : out[len++] = flags >> 8;
270 :
271 3418 : out[len++] = (uint8_t)MsgID::DISTANCE_DATA_CM;
272 :
273 : // technically this could be filtered/raw and first/last distance, but we just send the raw
274 3418 : memcpy(&out[len], &raw, 4);
275 3418 : len += 4;
276 3418 : memcpy(&out[len], &strength, 4);
277 3418 : len += 4;
278 :
279 3418 : uint16_t crc = compute_crc(out, len);
280 3418 : out[len++] = crc & 0xFF;
281 3418 : out[len++] = crc >> 8;
282 :
283 3418 : outlen = len;
284 3418 : }
285 :
286 101112 : uint32_t RF_LightWareGRF::packet_for_alt(float alt_m,
287 : uint8_t *buffer,
288 : uint8_t buflen)
289 : {
290 : // Always read incoming configuration messages
291 101112 : process_input();
292 :
293 : // Only transmit distance if stream has been configured (this is how the real GRF works)
294 101112 : if (!stream_enabled) {
295 97694 : return 0;
296 : }
297 :
298 3418 : if (buflen < 16) {
299 0 : return 0;
300 : }
301 :
302 3418 : uint32_t outlen = 0;
303 3418 : build_distance_packet(alt_m, buffer, outlen);
304 3418 : return outlen;
305 : }
306 :
307 : #endif // AP_SIM_RF_LIGHTWAR_GRF250_ENABLED
|
