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 : /*
16 : Simulate virtual vessels
17 : ./Tools/autotest/sim_vehicle.py -v Rover -A --serial5=sim:AIS --map
18 :
19 : param set SERIAL5_PROTOCOL 40
20 : param set AIS_TYPE 1
21 : reboot
22 : param set SIM_AIS_COUNT 10
23 : module load ais
24 :
25 : Dump logged AIS data to the serial port
26 : ./Tools/autotest/sim_vehicle.py -v Rover -A --serial5=sim:AISReplay --custom-location 51.58689798356386,-3.9044570193067965,0,0 --map
27 :
28 : param set SERIAL5_PROTOCOL 40
29 : param set AIS_TYPE 1
30 : reboot
31 : module load ais
32 : */
33 :
34 : #include "SIM_config.h"
35 :
36 : #if AP_SIM_AIS_ENABLED
37 :
38 : #include "SIM_AIS.h"
39 :
40 : #include <SITL/SITL.h>
41 :
42 : #define FORCE_VERSION_H_INCLUDE
43 : #include "ap_version.h"
44 :
45 : #include "SIM_Aircraft.h"
46 : #include <AP_Common/NMEA.h>
47 :
48 : extern const AP_HAL::HAL& hal;
49 :
50 : using namespace SITL;
51 :
52 : const AP_Param::GroupInfo AIS::var_info[] = {
53 :
54 : // @Param: COUNT
55 : // @DisplayName: Number of AIS vessels
56 : // @Description: Total number of AIS simulated vessels
57 : AP_GROUPINFO("COUNT", 1, AIS, vessel_count, -1),
58 :
59 : // @Param: RADIUS
60 : // @DisplayName: AIS radius stddev of vessels
61 : // @Description: Simulated standard deviation of radius in AIS of a vessel
62 : // @Units: m
63 : AP_GROUPINFO("RADIUS", 2, AIS, radius_m, 10000),
64 :
65 : AP_GROUPEND
66 : };
67 :
68 0 : AIS_Replay::AIS_Replay() : SerialDevice::SerialDevice()
69 : {
70 : char* file_path;
71 0 : IGNORE_RETURN(asprintf(&file_path, AP_BUILD_ROOT "/libraries/SITL/SIM_AIS_data.txt"));
72 :
73 0 : file = fopen(file_path,"r");
74 :
75 0 : if (file == nullptr) {
76 0 : AP_HAL::panic("AIS could not open data file");
77 : }
78 :
79 : // seek past the header line
80 : char line[100];
81 0 : IGNORE_RETURN(fgets(line, sizeof(line), file));
82 0 : }
83 :
84 0 : void AIS_Replay::update()
85 : {
86 0 : if (file == nullptr) {
87 0 : AP_HAL::panic("AIS lost data file");
88 : }
89 :
90 : // just send a line of data at 1Hz:
91 0 : const uint32_t now = AP_HAL::millis();
92 0 : if (now - last_sent_ms < 1000) {
93 0 : return;
94 : }
95 0 : last_sent_ms = now;
96 :
97 : char line[100];
98 :
99 0 : if (!fgets(line, sizeof(line), file)) {
100 : // got to the end of the file, circle back
101 0 : fseek(file,0,SEEK_SET);
102 0 : if (!fgets(line, sizeof(line), file)) {
103 0 : AP_HAL::panic("AIS lost data file");
104 : }
105 0 : return;
106 : }
107 :
108 : //hal.console->printf("%s",line);
109 0 : write_to_autopilot(line, strlen(line));
110 :
111 : }
112 :
113 3 : AIS::AIS()
114 : {
115 3 : AP::sitl()->models.ais_ptr = this;
116 3 : AP_Param::setup_object_defaults(this, var_info);
117 3 : }
118 :
119 59718 : void AIS::update(const class Aircraft &aircraft)
120 : {
121 59718 : const SIM *sitl = AP::sitl();
122 59718 : if (sitl == nullptr) {
123 0 : return;
124 : }
125 :
126 : // Disabled
127 59718 : if (vessel_count <= 0) {
128 18469 : return;
129 : }
130 :
131 : // Calculate the time step to take
132 41249 : const uint32_t now_ms = AP_HAL::millis();
133 41249 : const float dt = (now_ms - last_sim_update_ms) * 0.001;
134 41249 : last_sim_update_ms = now_ms;
135 :
136 : // Get current location to remove any vessel that is too far away
137 41249 : const Location &aircraft_loc = aircraft.get_location();
138 :
139 : // Update all vessels, marking any that move too far from the aircraft as in-active
140 2103699 : for (uint8_t i=0; i<ARRAY_SIZE(vessels); i++) {
141 2062450 : update_simulated_vessel(vessels[i], dt, aircraft_loc, radius_m, now_ms);
142 : }
143 :
144 : // Add new vessels until we have the correct number
145 2103699 : for (uint8_t i=0; i<ARRAY_SIZE(vessels); i++) {
146 : // Disable any extra vessels over the set count
147 2062450 : if (i >= vessel_count) {
148 1875749 : vessels[i].active = false;
149 1875749 : continue;
150 : }
151 :
152 : // Vessel is already active
153 186701 : if (vessels[i].active) {
154 186665 : continue;
155 : }
156 :
157 : // Generate new vessel
158 36 : init_vessel(vessels[i], aircraft_loc, radius_m);
159 : }
160 :
161 : }
162 :
163 : // Update a active vessel position and heading
164 2062450 : void AIS::update_simulated_vessel(ais_vessel &vessel, const float dt, const Location &vehicle_loc, const float radius, const uint32_t now_ms)
165 : {
166 : // Check if this vessel is active
167 2062450 : if (!vessel.active) {
168 1875785 : return;
169 : }
170 :
171 : // Update heading from turn rate, both turn rate and heading are in cdeg
172 186688 : vessel.info.heading += vessel.info.turn_rate * dt;
173 :
174 : // Extract location
175 186688 : Location loc { vessel.info.lat, vessel.info.lon, 0, Location::AltFrame::ABOVE_ORIGIN };
176 :
177 : // Update location from heading and velocity
178 186688 : loc.offset_bearing(vessel.info.heading * 0.01, vessel.info.velocity * 0.01 * dt);
179 :
180 : // Repopulate location
181 186688 : vessel.info.lat = loc.lat;
182 186688 : vessel.info.lon = loc.lng;
183 :
184 : // Check if location is still valid
185 186688 : if (vehicle_loc.get_distance(loc) > radius) {
186 23 : vessel.active = false;
187 23 : return;
188 : }
189 :
190 : // Report, update rate depends on speed
191 : uint32_t position_interval;
192 :
193 : // Convert speed over ground from 0.1 knots to cm/s
194 186665 : if (vessel.info.velocity > 23.0 * 0.1 * KNOTS_TO_M_PER_SEC * 100.0) {
195 80073 : position_interval = 2000; // every 2 seconds if faster than 23 knots
196 :
197 106592 : } else if (vessel.info.velocity > 14.0 * 0.1 * KNOTS_TO_M_PER_SEC * 100.0) {
198 37665 : position_interval = 6000; // every 6 seconds if faster than 14 knots
199 :
200 68927 : } else if (vessel.info.velocity > 0.0) {
201 1678 : position_interval = 10000; // every 10 seconds if moving
202 :
203 : } else {
204 67249 : position_interval = 3 * 60 * 1000; // every 3 mins if anchored or moored and moving at less than 3 knots
205 :
206 : }
207 :
208 : // Send position report at internal
209 186665 : if ((vessel.last_position_report_ms == 0) || (now_ms - vessel.last_position_report_ms > position_interval)) {
210 45 : send_position_report(vessel.info);
211 45 : vessel.last_position_report_ms = now_ms;
212 : }
213 :
214 : // Send static and voyage at internal
215 186665 : const uint32_t static_and_voyage_interval = 6 * 60 * 1000; // every 6 mins
216 186665 : if ((vessel.last_static_and_voyage_ms == 0) || (now_ms - vessel.last_static_and_voyage_ms > static_and_voyage_interval)) {
217 13 : send_static_and_voyage(vessel.info);
218 13 : vessel.last_static_and_voyage_ms = now_ms;
219 : }
220 :
221 : }
222 :
223 : // Generate a new vessel
224 36 : void AIS::init_vessel(ais_vessel &vessel, const Location &vehicle_loc, const float radius)
225 : {
226 : // Clear any existing data
227 36 : memset(&vessel, 0, sizeof(vessel));
228 :
229 : // Set flags for valid data
230 36 : vessel.info.flags |= AIS_FLAGS_VALID_VELOCITY | AIS_FLAGS_VALID_TURN_RATE;
231 :
232 : // Generate random data for static info, mask to 30 bit
233 36 : vessel.info.MMSI = rand() & 0x3FFFFFFF;
234 :
235 : // Set nav status and type
236 36 : vessel.info.navigational_status = AIS_NAV_STATUS_UNDER_WAY;
237 36 : vessel.info.type = AIS_TYPE_CARGO;
238 :
239 : // 90% chance of having valid dimensions
240 36 : if (rand() > RAND_MAX * 0.1) {
241 32 : vessel.info.flags |= AIS_FLAGS_VALID_DIMENSIONS;
242 :
243 : // Generate each dimension
244 32 : vessel.info.dimension_bow = abs(Aircraft::rand_normal(0, 300.0));
245 32 : if (vessel.info.dimension_bow > 511) {
246 2 : vessel.info.dimension_bow = 511;
247 2 : vessel.info.flags |= AIS_FLAGS_LARGE_BOW_DIMENSION;
248 : }
249 :
250 32 : vessel.info.dimension_stern = abs(Aircraft::rand_normal(0, 300.0));
251 32 : if (vessel.info.dimension_stern > 511) {
252 3 : vessel.info.dimension_stern = 511;
253 3 : vessel.info.flags |= AIS_FLAGS_LARGE_STERN_DIMENSION;
254 : }
255 :
256 32 : vessel.info.dimension_port = abs(Aircraft::rand_normal(0, 30.0));
257 32 : if (vessel.info.dimension_port > 63) {
258 1 : vessel.info.dimension_port = 63;
259 1 : vessel.info.flags |= AIS_FLAGS_LARGE_PORT_DIMENSION;
260 : }
261 :
262 32 : vessel.info.dimension_starboard = abs(Aircraft::rand_normal(0, 30.0));
263 32 : if (vessel.info.dimension_starboard > 63) {
264 4 : vessel.info.dimension_starboard = 63;
265 4 : vessel.info.flags |= AIS_FLAGS_LARGE_STARBOARD_DIMENSION;
266 : }
267 : }
268 :
269 : // 25% chance of turning
270 36 : if (rand() > RAND_MAX * 0.75) {
271 14 : vessel.info.turn_rate = Aircraft::rand_normal(0, 100.0);
272 : }
273 :
274 : // 95% chance of moving
275 36 : if (rand() > RAND_MAX * 0.05) {
276 34 : vessel.info.velocity = abs(Aircraft::rand_normal(0, 500.0));
277 :
278 : // Speed threshold for high velocity flag
279 : // Convert speed over ground from 0.1 knots to cm/s
280 34 : const uint16_t high_velocity = 1022 * 0.1 * KNOTS_TO_M_PER_SEC * 100.0;
281 :
282 34 : if (vessel.info.velocity >= high_velocity) {
283 0 : vessel.info.velocity = high_velocity;
284 0 : vessel.info.flags |= AIS_FLAGS_HIGH_VELOCITY;
285 : }
286 : }
287 :
288 : // Generate a heading
289 36 : vessel.info.heading = wrap_360_cd(rand());
290 :
291 : // Generate a random location
292 36 : Location loc = vehicle_loc;
293 36 : loc.offset( Vector3p {
294 : Aircraft::rand_normal(0, radius),
295 : Aircraft::rand_normal(0, radius),
296 : 0
297 : });
298 36 : vessel.info.lat = loc.lat;
299 36 : vessel.info.lon = loc.lng;
300 :
301 : // mark active
302 36 : vessel.active = true;
303 36 : }
304 :
305 : /*
306 : formatted print of NMEA message, with checksum appended
307 : */
308 71 : void AIS::nmea_printf(const char *fmt, ...)
309 : {
310 : va_list ap;
311 :
312 71 : va_start(ap, fmt);
313 71 : char *s = nmea_vaprintf(fmt, ap);
314 71 : va_end(ap);
315 71 : if (s != nullptr) {
316 71 : write_to_autopilot((const char*)s, strlen(s));
317 71 : free(s);
318 : }
319 71 : }
320 :
321 :
322 : // Convert to NMEA char
323 2183 : uint8_t AIS::encode_char(uint8_t payload) const
324 : {
325 2183 : if (payload + 8 > 40) {
326 524 : return payload + 8 + 48;
327 : }
328 1659 : return payload + 48;
329 : }
330 :
331 2834 : uint8_t AIS::get_bits(uint32_t value, const uint8_t start, const uint8_t len) const
332 : {
333 2834 : value = value >> start;
334 2834 : const uint8_t mask = 0b111111 >> (6 - len);
335 2834 : return value & mask;
336 : }
337 :
338 : // set the specified bits in the payload with 6 bit chars
339 935 : void AIS::set_bits(uint8_t *payload, const uint16_t low, const uint16_t high, uint32_t value)
340 : {
341 : // Bits are inclusive, so add one
342 935 : const uint8_t bit_len = high - low + 1;
343 935 : const uint32_t value_mask = 0xFFFFFFFF >> (32 - bit_len);
344 :
345 935 : if ((value & ~value_mask) != 0) {
346 0 : AP_HAL::panic("AIS value passed too big to fit (%u > %u bits)", value, bit_len);
347 : }
348 :
349 935 : const uint8_t char_low = low / 6;
350 935 : const uint8_t bit_low = low % 6;
351 :
352 935 : const uint8_t char_high = high / 6;
353 935 : const uint8_t bit_high = (high % 6) + 1;
354 :
355 935 : const uint8_t char_range = char_high - char_low;
356 935 : uint8_t bits = bit_len;
357 3769 : for (uint8_t index = 0; index <= char_range; index++) {
358 2834 : if (bits == 0) {
359 0 : AP_HAL::panic("AIS packing fail A");
360 : }
361 2834 : if (index == char_range) {
362 : // last char uses high bit
363 935 : const uint8_t bits_needed = MIN(bit_high, bit_len);
364 935 : bits -= bits_needed;
365 935 : payload[char_low + index] |= get_bits(value, bits, bits_needed) << (6 - bit_high);
366 :
367 1899 : } else if (index == 0) {
368 : // First char uses low bit
369 593 : const uint8_t bits_needed = MIN(6 - bit_low, bit_len);
370 593 : bits -= bits_needed;
371 593 : payload[char_low + index] |= get_bits(value, bits, bits_needed);
372 :
373 : } else {
374 : // Others need full length
375 1306 : bits -= 6;
376 1306 : payload[char_low + index] |= get_bits(value, bits, 6);
377 :
378 : }
379 : }
380 :
381 935 : if (bits != 0) {
382 0 : AP_HAL::panic("AIS packing fail B");
383 : }
384 :
385 935 : }
386 :
387 135 : void AIS::set_bits_signed(uint8_t *payload, const uint16_t low, const uint16_t high, int32_t value)
388 : {
389 135 : if (value >= 0) {
390 : // Positive number is the same as unsigned
391 81 : set_bits(payload, low, high, value);
392 81 : return;
393 : }
394 :
395 : // Convert to unsigned
396 54 : uint32_t val = value;
397 :
398 : // Mask to correct length
399 54 : const uint8_t bit_len = high - low + 1;
400 54 : val &= 0xFFFFFFFF >> (32 - bit_len);
401 :
402 : // Set sign bit
403 54 : val |= 1 << (bit_len - 1);
404 :
405 54 : set_bits(payload, low, high, val);
406 : }
407 :
408 45 : void AIS::send_position_report(const mavlink_ais_vessel_t &info) {
409 :
410 : // Turn rate, default to not available
411 45 : int8_t rot = -128;
412 45 : if ((info.flags & AIS_FLAGS_VALID_TURN_RATE) != 0) {
413 45 : if ((info.flags & AIS_FLAGS_TURN_RATE_SIGN_ONLY) != 0) {
414 : // Only sign available
415 0 : rot = info.turn_rate >= 0 ? 127 : -127;
416 :
417 : } else {
418 : // cdeg/s to deg/min, take abs to allow sqrt
419 45 : const float turn_rate_deg_per_min = abs(info.turn_rate) * 60.0f * 0.01f;
420 :
421 : // Apply scaling, recovering sign
422 45 : rot = 4.733 * sqrtf(turn_rate_deg_per_min) * info.turn_rate >= 0 ? 1.0 : -1.0;
423 : }
424 : }
425 :
426 : // Corse over ground, default to not available
427 45 : uint16_t cog = 3600;
428 45 : if ((info.flags & AIS_FLAGS_VALID_COG) != 0) {
429 : // cdeg to 0.1 deg
430 0 : cog = info.COG * 0.1;
431 : }
432 :
433 : // Accuracy bit from flag
434 45 : const bool accuracy = (info.flags & AIS_FLAGS_POSITION_ACCURACY) != 0;
435 :
436 : // Speed over ground, default to not available
437 45 : uint16_t sog = 1023;
438 45 : if ((info.flags & AIS_FLAGS_VALID_VELOCITY) != 0) {
439 45 : if ((info.flags & AIS_FLAGS_HIGH_VELOCITY) != 0) {
440 0 : sog = 1022;
441 : } else {
442 45 : sog = MIN((float(info.velocity) * 0.01 * M_PER_SEC_TO_KNOTS * 10), 1022);
443 : }
444 : }
445 :
446 : // storage for payload
447 45 : uint8_t payload[28] {};
448 45 : set_bits(payload, 0, 5, 1); // type: Position Report Class A
449 45 : set_bits(payload, 6, 7, 3); // repeat: Do not repeat
450 45 : set_bits(payload, 8, 37, info.MMSI); // mmsi
451 45 : set_bits(payload, 38, 41, info.navigational_status); // status
452 45 : set_bits_signed(payload, 42, 49, rot); // rate of turn
453 45 : set_bits(payload, 50, 59, sog); // speed
454 45 : set_bits(payload, 60, 60, accuracy); // accuracy
455 45 : set_bits_signed(payload, 61, 88, int32_t(double(info.lon) * 0.06)); // lon
456 45 : set_bits_signed(payload, 89, 115, int32_t(double(info.lat) * 0.06)); // lat
457 45 : set_bits(payload, 116, 127, cog); // course over ground
458 45 : set_bits(payload, 128, 136, uint32_t(info.heading * 0.01)); // heading convert from deg to centi-deg
459 45 : set_bits(payload, 137, 142, 60); // second: not available
460 45 : set_bits(payload, 143, 144, 0); // maneuver: not available
461 : // 145 - 147: spare
462 45 : set_bits(payload, 148, 148, 0); // raim not used
463 45 : set_bits(payload, 149, 167, 0); // Radio status
464 :
465 : // Convert to chars
466 45 : uint8_t encoded[sizeof(payload) + 1] {};
467 1305 : for (uint8_t i = 0; i < sizeof(payload); i++) {
468 1260 : encoded[i] = encode_char(payload[i]);
469 : }
470 :
471 45 : const uint8_t total_fragments = 1;
472 45 : const uint8_t fragment = 1;
473 : // sequence ID is not used for a single fragment message
474 45 : const char channel_code = 'A';
475 45 : const uint8_t fill_bits = 0;
476 :
477 : // Send
478 45 : nmea_printf("!AIVDM,%u,%u,,%c,%s,%u", total_fragments, fragment, channel_code, encoded, fill_bits);
479 :
480 45 : }
481 :
482 13 : void AIS::send_static_and_voyage(const mavlink_ais_vessel_t &info) {
483 :
484 : // Fill in dimensions if valid
485 13 : uint16_t bow_dim = 0;
486 13 : uint16_t stern_dim = 0;
487 13 : uint8_t port_dim = 0;
488 13 : uint8_t star_dim = 0;
489 :
490 13 : if ((info.flags & AIS_FLAGS_VALID_DIMENSIONS) != 0) {
491 12 : if ((info.flags & AIS_FLAGS_LARGE_BOW_DIMENSION) != 0) {
492 1 : bow_dim = 511;
493 : } else {
494 11 : bow_dim = MIN(info.dimension_bow, 511);
495 : }
496 12 : if ((info.flags & AIS_FLAGS_LARGE_STERN_DIMENSION) != 0) {
497 2 : stern_dim = 511;
498 : } else {
499 10 : stern_dim = MIN(info.dimension_stern, 511);
500 : }
501 12 : if ((info.flags & AIS_FLAGS_LARGE_PORT_DIMENSION) != 0) {
502 0 : port_dim = 63;
503 : } else {
504 12 : port_dim = MIN(info.dimension_port, 63);
505 : }
506 12 : if ((info.flags & AIS_FLAGS_LARGE_STARBOARD_DIMENSION) != 0) {
507 2 : star_dim = 63;
508 : } else {
509 10 : star_dim = MIN(info.dimension_starboard, 63);
510 : }
511 : }
512 :
513 : // storage for payload
514 13 : uint8_t payload[71] {};
515 13 : set_bits(payload, 0, 5, 5); // type: Static and Voyage Related Data
516 13 : set_bits(payload, 6, 7, 3); // repeat: Do not repeat
517 13 : set_bits(payload, 8, 37, info.MMSI); // mmsi
518 13 : set_bits(payload, 38, 39, 1); // AIS version
519 13 : set_bits(payload, 40, 69, 0); // imo number
520 13 : set_bits(payload, 70, 111, '@'); // call_sign: empty
521 13 : set_bits(payload, 112, 231, '@'); // name: empty
522 13 : set_bits(payload, 232, 239, info.type);
523 13 : set_bits(payload, 240, 248, bow_dim); // bow_dim
524 13 : set_bits(payload, 249, 257, stern_dim); // stern_dim
525 13 : set_bits(payload, 258, 263, port_dim); // port_dim
526 13 : set_bits(payload, 264, 269, star_dim); // star_dim
527 13 : set_bits(payload, 270, 273, 1); // fix: GPS
528 13 : set_bits(payload, 274, 277, 0); // ETA month: not available
529 13 : set_bits(payload, 278, 282, 0); // ETA day: not available
530 13 : set_bits(payload, 283, 287, 0); // ETA hour: not available
531 13 : set_bits(payload, 288, 293, 0); // ETA minute: not available
532 13 : set_bits(payload, 294, 301, 0); // draught: 0 decimeters
533 13 : set_bits(payload, 302, 421, '@'); // destination: empty
534 13 : set_bits(payload, 422, 422, 1); // dte: Data terminal not ready
535 : // 423: not used
536 :
537 : // Split and encode
538 13 : const uint8_t msg1_len = 50;
539 13 : const uint8_t msg2_len = sizeof(payload) - msg1_len;
540 :
541 13 : uint8_t msg1_encoded[msg1_len + 1] {};
542 663 : for (uint8_t i = 0; i < msg1_len; i++) {
543 650 : msg1_encoded[i] = encode_char(payload[i]);
544 : }
545 :
546 13 : uint8_t msg2_encoded[msg2_len + 1] {};
547 286 : for (uint8_t i = 0; i < msg2_len; i++) {
548 273 : msg2_encoded[i] = encode_char(payload[msg1_len + i]);
549 : }
550 :
551 13 : const uint8_t total_fragments = 2;
552 13 : uint8_t fragment = 1;
553 13 : const char channel_code = 'A';
554 13 : const uint8_t fill_bits = 0;
555 :
556 : // Send
557 13 : nmea_printf("!AIVDM,%u,%u,%u,%c,%s,%u", total_fragments, fragment, sequence_ID, channel_code, msg1_encoded, fill_bits);
558 13 : fragment++;
559 :
560 13 : nmea_printf("!AIVDM,%u,%u,%u,%c,%s,%u", total_fragments, fragment, sequence_ID, channel_code, msg2_encoded, fill_bits);
561 :
562 13 : sequence_ID++;
563 13 : }
564 :
565 : #endif // AP_SIM_AIS_ENABLED
|
