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 : ADSB simulator class for MAVLink ADSB peripheral
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_ADSB_ENABLED
22 :
23 : #include "SIM_ADSB.h"
24 :
25 : #include "SITL.h"
26 :
27 : #include <stdio.h>
28 :
29 : #include "SIM_Aircraft.h"
30 : #include <AP_HAL_SITL/SITL_State.h>
31 : #include <AP_AHRS/AP_AHRS.h>
32 :
33 : namespace SITL {
34 :
35 : /*
36 : update a simulated vehicle
37 : */
38 80556 : void ADSB_Vehicle::update(const class Aircraft &aircraft, float delta_t)
39 : {
40 80556 : const SIM *_sitl = AP::sitl();
41 80556 : if (_sitl == nullptr) {
42 0 : return;
43 : }
44 :
45 80556 : const Location &origin { aircraft.get_origin() };
46 :
47 80556 : if (!initialised) {
48 : // spawn another aircraft
49 12 : initialised = true;
50 12 : ICAO_address = (uint32_t)(rand() % 10000);
51 12 : snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
52 12 : Location aircraft_location = aircraft.get_location();
53 12 : const Vector2f aircraft_offset_ne = aircraft_location.get_distance_NE(origin);
54 12 : position.x = aircraft_offset_ne[1];
55 12 : position.y = aircraft_offset_ne[0];
56 12 : position.x += Aircraft::rand_normal(0, _sitl->adsb_radius_m.get());
57 12 : position.y += Aircraft::rand_normal(0, _sitl->adsb_radius_m.get());
58 12 : position.z = -fabsf(_sitl->adsb_altitude_m);
59 :
60 12 : double vel_min = 5, vel_max = 20;
61 12 : if (position.length() > 500) {
62 12 : vel_min *= 3;
63 12 : vel_max *= 3;
64 0 : } else if (position.length() > 10000) {
65 0 : vel_min *= 10;
66 0 : vel_max *= 10;
67 : }
68 12 : type = (ADSB_EMITTER_TYPE)(rand() % (ADSB_EMITTER_TYPE_POINT_OBSTACLE + 1));
69 : // don't allow surface emitters to move
70 12 : if (type == ADSB_EMITTER_TYPE_POINT_OBSTACLE) {
71 1 : stationary_object_created_ms = AP_HAL::millis64();
72 1 : velocity_ef.zero();
73 : } else {
74 11 : stationary_object_created_ms = 0;
75 11 : velocity_ef.x = Aircraft::rand_normal(vel_min, vel_max);
76 11 : velocity_ef.y = Aircraft::rand_normal(vel_min, vel_max);
77 11 : if (type < ADSB_EMITTER_TYPE_EMERGENCY_SURFACE) {
78 10 : velocity_ef.z = Aircraft::rand_normal(-3, 3);
79 : }
80 : }
81 80544 : } else if (stationary_object_created_ms > 0 && AP_HAL::millis64() - stationary_object_created_ms > AP_MSEC_PER_HOUR) {
82 : // regenerate stationary objects so we don't randomly fill up the screen with them over time
83 0 : initialised = false;
84 : }
85 :
86 80556 : position += velocity_ef * delta_t;
87 80556 : if (position.z > 0) {
88 : // it has crashed! reset
89 0 : initialised = false;
90 : }
91 :
92 80556 : Location ret = origin;
93 80556 : ret.offset(position.x, position.y);
94 :
95 80556 : location = ret;
96 : }
97 :
98 147691 : const Location &ADSB_Vehicle::get_location() const
99 : {
100 147691 : return location;
101 : }
102 :
103 : /*
104 : update the ADSB peripheral state
105 : */
106 29376 : void ADSB::update_simulated_vehicles(const class Aircraft &aircraft)
107 : {
108 29376 : if (_sitl == nullptr) {
109 2 : _sitl = AP::sitl();
110 2 : return;
111 29374 : } else if (_sitl->adsb_plane_count <= 0) {
112 2522 : return;
113 26852 : } else if (_sitl->adsb_plane_count >= num_vehicles_MAX) {
114 0 : _sitl->adsb_plane_count.set_and_save(0);
115 0 : num_vehicles = 0;
116 0 : return;
117 26852 : } else if (num_vehicles != _sitl->adsb_plane_count) {
118 2 : num_vehicles = _sitl->adsb_plane_count;
119 402 : for (uint8_t i=0; i<num_vehicles_MAX; i++) {
120 400 : vehicles[i].initialised = false;
121 : }
122 : }
123 :
124 : // calculate delta time in seconds
125 26852 : uint32_t now_us = AP_HAL::micros();
126 :
127 26852 : float delta_t = (now_us - last_update_us) * 1.0e-6f;
128 26852 : last_update_us = now_us;
129 :
130 : // prune any aircraft which get too far away from our simulated vehicle:
131 26852 : const Location &aircraft_loc = aircraft.get_location();
132 :
133 107408 : for (uint8_t i=0; i<num_vehicles; i++) {
134 80556 : auto &vehicle = vehicles[i];
135 80556 : vehicle.update(aircraft, delta_t);
136 :
137 : // re-init when exceeding radius range
138 80556 : if (aircraft_loc.get_distance(vehicle.get_location()) > _sitl->adsb_radius_m) {
139 6 : vehicle.initialised = false;
140 : }
141 : }
142 : }
143 :
144 29376 : void ADSB::update(const class Aircraft &aircraft)
145 : {
146 29376 : update_simulated_vehicles(aircraft);
147 :
148 : // see if we should do a report.
149 29376 : if ((_sitl->adsb_types & (1U << (uint8_t)SIM::ADSBType::Shortcut)) == 0) {
150 : // some other simulated device is in use (e.g. MXS)
151 13427 : return;
152 : }
153 :
154 : // bakwards compatability; the parameters specify ADSB simulation,
155 : // but we are not configured to use a simulated ADSB driver.
156 : // Pretend to be a uAvionix mavlink device:
157 15949 : send_report(aircraft);
158 : }
159 :
160 : /*
161 : send a report to the vehicle control code over MAVLink
162 : */
163 15949 : void ADSB::send_report(const class Aircraft &aircraft)
164 : {
165 15949 : if (AP_HAL::millis() < 10000) {
166 : // simulated aircraft don't appear until 10s after startup. This avoids a windows
167 : // threading issue with non-blocking sockets and the initial wait on SERIAL0
168 12005 : return;
169 : }
170 :
171 : // check for incoming MAVLink messages
172 : uint8_t buf[100];
173 : ssize_t ret;
174 :
175 4619 : while ((ret=read_from_autopilot((char*)buf, sizeof(buf))) > 0) {
176 56720 : for (uint8_t i=0; i<ret; i++) {
177 : mavlink_message_t msg;
178 : mavlink_status_t status;
179 112090 : if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status,
180 56045 : buf[i],
181 56045 : &msg, &status) == MAVLINK_FRAMING_OK) {
182 1643 : switch (msg.msgid) {
183 45 : case MAVLINK_MSG_ID_HEARTBEAT: {
184 45 : if (!seen_heartbeat) {
185 2 : seen_heartbeat = true;
186 2 : vehicle_component_id = msg.compid;
187 2 : vehicle_system_id = msg.sysid;
188 2 : ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id);
189 : }
190 45 : break;
191 : }
192 : }
193 : }
194 : }
195 : }
196 :
197 3944 : if (!seen_heartbeat) {
198 0 : return;
199 : }
200 :
201 3944 : uint32_t now = AP_HAL::millis();
202 : mavlink_message_t msg;
203 : uint16_t len;
204 :
205 3944 : if (now - last_heartbeat_ms >= 1000) {
206 : mavlink_heartbeat_t heartbeat;
207 4 : heartbeat.type = MAV_TYPE_ADSB;
208 4 : heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA;
209 4 : heartbeat.base_mode = 0;
210 4 : heartbeat.system_status = 0;
211 4 : heartbeat.mavlink_version = 0;
212 4 : heartbeat.custom_mode = 0;
213 :
214 4 : len = mavlink_msg_heartbeat_encode_status(vehicle_system_id,
215 4 : vehicle_component_id,
216 : &mavlink.status,
217 : &msg, &heartbeat);
218 :
219 4 : write_to_autopilot((char*)&msg.magic, len);
220 :
221 4 : last_heartbeat_ms = now;
222 : }
223 :
224 :
225 : /*
226 : send a ADSB_VEHICLE messages
227 : */
228 3944 : uint32_t now_us = AP_HAL::micros();
229 3944 : if (now_us - last_report_us >= reporting_period_ms*1000UL) {
230 12 : for (uint8_t i=0; i<num_vehicles; i++) {
231 8 : const ADSB_Vehicle &vehicle = vehicles[i];
232 8 : if (!vehicle.initialised) {
233 0 : continue;
234 : }
235 :
236 8 : const Location &loc { vehicle.get_location() };
237 :
238 8 : mavlink_adsb_vehicle_t adsb_vehicle {};
239 8 : last_report_us = now_us;
240 :
241 8 : adsb_vehicle.ICAO_address = vehicle.ICAO_address;
242 8 : adsb_vehicle.lat = loc.lat;
243 8 : adsb_vehicle.lon = loc.lng;
244 8 : adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
245 8 : adsb_vehicle.altitude = -vehicle.position.z * 1000;
246 8 : adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x)));
247 8 : adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100;
248 8 : adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100;
249 8 : memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign));
250 8 : adsb_vehicle.emitter_type = vehicle.type;
251 8 : adsb_vehicle.tslc = 1;
252 8 : adsb_vehicle.flags =
253 : ADSB_FLAGS_VALID_COORDS |
254 : ADSB_FLAGS_VALID_ALTITUDE |
255 : ADSB_FLAGS_VALID_HEADING |
256 : ADSB_FLAGS_VALID_VELOCITY |
257 : ADSB_FLAGS_VALID_CALLSIGN |
258 : ADSB_FLAGS_VALID_SQUAWK |
259 : ADSB_FLAGS_SIMULATED |
260 : ADSB_FLAGS_VERTICAL_VELOCITY_VALID |
261 : ADSB_FLAGS_BARO_VALID;
262 : // all flags set except ADSB_FLAGS_SOURCE_UAT
263 :
264 8 : adsb_vehicle.squawk = 1200;
265 :
266 8 : len = mavlink_msg_adsb_vehicle_encode_status(vehicle_system_id,
267 : MAV_COMP_ID_ADSB,
268 : &mavlink.status,
269 : &msg, &adsb_vehicle);
270 :
271 8 : uint8_t msgbuf[len];
272 8 : len = mavlink_msg_to_send_buffer(msgbuf, &msg);
273 8 : if (len > 0) {
274 8 : write_to_autopilot((char*)msgbuf, len);
275 : }
276 8 : }
277 : }
278 :
279 : // ADSB_transceiever is enabled, send the status report.
280 3944 : if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) {
281 0 : last_tx_report_ms = now;
282 :
283 0 : const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK};
284 0 : len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode_status(vehicle_system_id,
285 : MAV_COMP_ID_ADSB,
286 : &mavlink.status,
287 : &msg, &health_report);
288 :
289 0 : uint8_t msgbuf[len];
290 0 : len = mavlink_msg_to_send_buffer(msgbuf, &msg);
291 0 : if (len > 0) {
292 0 : write_to_autopilot((char*)msgbuf, len);
293 0 : ::printf("ADSBsim send tx health packet\n");
294 : }
295 0 : }
296 :
297 : }
298 :
299 : } // namespace SITL
300 :
301 : #endif // AP_SIM_ADSB_ENABLED
|
