Line data Source code
1 : #pragma once
2 :
3 : /*
4 :
5 : ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:sagetech_mxs --no-configure --map
6 :
7 : param set SERIAL5_PROTOCOL 35 # ADSB
8 : param set ADSB_TYPE 4 # Sagetech MX Series
9 : param set SIM_ADSB_TYPES 8 # Sagetech MX Series
10 : param set SIM_ADSB_COUNT 5
11 : param set SIM_ADSB_RADIUS 2000 # same as ADSB_LIST_RADIUS
12 :
13 : */
14 :
15 :
16 : #include "SIM_config.h"
17 :
18 : #if AP_SIM_ADSB_SAGETECH_MXS_ENABLED
19 :
20 : #include "SIM_ADSB_Device.h"
21 :
22 : #include <AP_Common/Location.h>
23 : #include <AP_InternalError/AP_InternalError.h>
24 :
25 : namespace SITL {
26 :
27 : class ADSB_Sagetech_MXS : public ADSB_Device {
28 : public:
29 : using ADSB_Device::ADSB_Device;
30 :
31 : void update(const class Aircraft *sitl_model);
32 :
33 : private:
34 : void update_serial_input();
35 : void update_serial_output(const Aircraft *sitl_model);
36 : void update_serial_output_vehicles(const Aircraft *sitl_model);
37 :
38 : // pack a floating-point latitude or longitude into three bytes
39 : // according to Sagetech definitions:
40 : void pack_scaled_geocoord(uint8_t buf[3], float coord);
41 : void pack_scaled_alt(uint8_t buf[3], float alt_m);
42 : uint8_t scaled_groundspeed(float speed_m_s) const;
43 : void pack_scaled_groundspeed(uint8_t dest[1], float speed_m_s) const;
44 : void pack_scaled_airspeed(uint8_t dest[2], float speed_m_s) const;
45 : void pack_scaled_vertical_rate(uint8_t dest[2], float speed_m_s) const;
46 : // pack the three lower bytes of source into dest:
47 : void pack_int32_into_uint8_ts(int32_t source, uint8_t dest[3]);
48 :
49 : void send_vehicle_message(const class ADSB_Vehicle &vehicle);
50 : void send_vehicle_message_state_vector(const class ADSB_Vehicle &vehicle);
51 : void send_vehicle_message_status_report(const class ADSB_Vehicle &vehicle);
52 :
53 : void update_rf_output();
54 : void update_rf_input();
55 :
56 : enum class MsgType : uint8_t {
57 : INSTALLATION = 0x01,
58 : FLIGHTID = 0x02,
59 : OPMSG = 0x03,
60 : GPS = 0x04,
61 : DATAREQ = 0x05,
62 : TARGETREQUEST = 0x0B,
63 :
64 : ACK = 0x80,
65 : STATEVECTORREPORT = 0x91,
66 : MODESTATUSREPORT = 0x92,
67 : };
68 :
69 : template <typename T>
70 : class PACKED PackedMessage {
71 : public:
72 134276 : PackedMessage(T _msg, MsgType _msgtype, uint8_t _msgid) :
73 134276 : msgtype{_msgtype},
74 134276 : msgid{_msgid},
75 134276 : msg{_msg}
76 : {
77 134276 : update_checksum();
78 134276 : }
79 : uint8_t preamble { 0xAA };
80 : MsgType msgtype;
81 : uint8_t msgid;
82 : uint8_t payloadlen = sizeof(T);
83 : T msg;
84 : uint8_t checksum;
85 :
86 134276 : uint16_t calculate_checksum(uint16_t len) const WARN_IF_UNUSED {
87 134276 : uint8_t ret = 0;
88 3759354 : for (uint8_t i=0; i<len; i++) {
89 3625078 : ret += ((const char*)this)[i];
90 :
91 : }
92 134276 : return ret;
93 : }
94 134276 : uint16_t calculate_checksum() const WARN_IF_UNUSED {
95 134276 : return calculate_checksum(4+sizeof(T));
96 : }
97 :
98 134276 : void update_checksum() {
99 134276 : checksum = calculate_checksum();
100 134276 : }
101 : };
102 :
103 : enum class OperatingMode {
104 : OFF,
105 : MAINTENANCE,
106 : } operating_mode;
107 :
108 : // 0x01 - Installation Message
109 : class PACKED Installation {
110 : public:
111 : Installation()
112 : { }
113 : uint8_t ICAO_address[3];
114 : uint8_t aircraft_registration[7];
115 : uint8_t reserved1[2];
116 : uint8_t com_port_0;
117 : uint8_t com_port_1;
118 : uint8_t ip_adress[4];
119 : uint8_t net_mask[4];
120 : uint8_t port_number[2];
121 : uint8_t gps_integrity;
122 : uint8_t emitter_category_set;
123 : uint8_t emitter_category;
124 : uint8_t aircraft_size;
125 : uint8_t max_airspeed;
126 : uint8_t altitude_encoder_offset[2];
127 : uint8_t reserved2[2];
128 : uint8_t install_configuration;
129 : uint8_t reserved3[2];
130 : };
131 :
132 : // 0x01 - Flight ID Message
133 : class PACKED FlightIDMessage {
134 : public:
135 : FlightIDMessage()
136 : { }
137 : uint8_t flight_id[8];
138 : uint8_t reserved[4];
139 : };
140 :
141 : // 0x03 - Operating Message
142 : class PACKED Operating {
143 : public:
144 : Operating()
145 : { }
146 :
147 : uint16_t squawk;
148 : uint8_t config;
149 : uint8_t emergency_flag;
150 : uint16_t alt;
151 : uint16_t climb_rate;
152 : uint16_t heading;
153 : uint16_t airspeed;
154 : };
155 :
156 : // 0x04 - GPS Navigation data message
157 : class PACKED GPS {
158 : public:
159 : GPS()
160 : { }
161 : uint8_t longitude[11];
162 : uint8_t latitude[10];
163 : uint8_t speed[6];
164 : uint8_t ground_track[8];
165 : uint8_t hemisphere;
166 : uint8_t time_of_fix[10];
167 : float height;
168 : float hpl;
169 : float hfom;
170 : float vfom;
171 : uint8_t nac;
172 : };
173 :
174 : // 0x05 - Data Request Message
175 : class PACKED DataRequest {
176 : public:
177 : DataRequest(uint8_t _reqtype) :
178 : reqtype(_reqtype)
179 : { }
180 : uint8_t reqtype;
181 : uint8_t reserved[3];
182 :
183 : enum class RequestDataType {
184 : HEALTH = 0x8D,
185 : };
186 : };
187 :
188 : // 0x0B - Target Request
189 : class PACKED TargetRequest {
190 : public:
191 : TargetRequest()
192 : { }
193 : uint8_t reqtype;
194 : uint16_t number_of_participants;
195 : uint8_t participant_id[3];
196 : uint8_t requested_reports;
197 : };
198 :
199 : // 0x80 - Acknowledgement Message
200 : class PACKED Ack {
201 : public:
202 22 : Ack(MsgType _ackType, uint8_t _ackId, uint8_t _state, uint8_t _alt[3]) :
203 22 : ackType{_ackType},
204 22 : ackId{_ackId},
205 22 : state{_state}
206 : {
207 22 : memcpy(alt, _alt, sizeof(alt));
208 22 : }
209 : MsgType ackType;
210 : uint8_t ackId;
211 : uint8_t state;
212 : uint8_t alt[3];
213 : };
214 :
215 : // 0x91 - ADSB State Vector Report. Note that this structure is
216 : // full-sized, assuming that the report structure field is indicating
217 : // all fields present. Actual parsing of this packet would
218 : // require looking at the report structure field.
219 : union PACKED StateVectorReport_ReportStructure {
220 : struct {
221 : struct PACKED {
222 : uint8_t time_of_applicability_for_estimated_positon : 1;
223 : uint8_t position_time_of_applicability : 1;
224 : uint8_t velocity_time_of_applicability : 1;
225 : uint8_t lat_and_lng : 1;
226 : uint8_t msn : 4;
227 : } rs0;
228 : struct PACKED {
229 : uint8_t estimated_latitude : 1;
230 : uint8_t navigation_integrity_category : 1;
231 : uint8_t vertical_rate_geometric_barometric : 1;
232 : uint8_t altimiter_barometric : 1;
233 : uint8_t heading_on_ground : 1;
234 : uint8_t ground_speed_on_ground : 1;
235 : uint8_t north_south_east_west_vel : 1;
236 : uint8_t altitude_geometric : 1;
237 : } rs1;
238 : struct PACKED {
239 : uint8_t reserved1 : 3;
240 : uint8_t report_mode : 1;
241 : uint8_t surveillance_status : 1;
242 : uint8_t estimated_east_west_velocity : 1;
243 : uint8_t estimated_north_south_velocity : 1;
244 : uint8_t estimated_longitude : 1;
245 : } rs2;
246 : };
247 : uint8_t raw[3];
248 : };
249 :
250 : class PACKED StateVectorReport {
251 : public:
252 : StateVectorReport_ReportStructure report_structure;
253 : uint16_t validity_flags;
254 : uint8_t participant_address[3];
255 : uint8_t address_qualifier;
256 : uint8_t report_times_of_applicability[6];
257 : uint8_t latitude[3];
258 : uint8_t longitude[3];
259 : uint8_t geometric_altitude[3];
260 : uint16_t vel_n;
261 : uint16_t vel_e;
262 : uint8_t groundspeed_on_surface;
263 : uint8_t heading_on_surface;
264 : uint8_t barometric_alt[3];
265 : uint8_t vertical_rate[2];
266 : uint8_t NIC;
267 : uint8_t estimated_latitude[3];
268 : uint8_t estimated_longitude[3];
269 : uint16_t estimated_vel_n;
270 : uint16_t estimated_vel_e;
271 : uint8_t surveillance_status;
272 : uint8_t report_mode;
273 : };
274 : template <uint8_t SIZE>
275 : class PACKED StateVectorReportBuffer {
276 : public:
277 : uint8_t buffer[SIZE];
278 : };
279 :
280 :
281 : // 0x92 - ADSB Status Report. Note that this structure is
282 : // full-sized, assuming that the report structure field is indicating
283 : // all fields present. Actual parsing of this packet would
284 : // require looking at the report structure.
285 : class PACKED ModeStatusReport {
286 : public:
287 : uint32_t report_structure : 24;
288 : uint16_t validity_flags;
289 : uint8_t participant_address[3];
290 : uint8_t address_qualifier;
291 : uint8_t report_times_of_applicability[6];
292 : uint8_t callsign[8];
293 : uint8_t emitter_category;
294 : uint8_t av_length_and_width_code;
295 : uint8_t emergency_priority_status;
296 : uint32_t capability_class_codes : 24;
297 : uint16_t operational_mode;
298 : uint8_t sv_quality_nacp;
299 : uint8_t sv_quality_nacv;
300 : uint8_t sv_quality_sil_and_sda;
301 : uint8_t sv_quality_gva;
302 : uint8_t sv_quality_nic;
303 : uint8_t sv_quality_track_heading_and_horizontal_reference_direction;
304 : uint8_t vertical_rate_type;
305 : uint8_t reserved[2];
306 : };
307 : template <uint8_t SIZE>
308 : class PACKED ModeStatusReportBuffer {
309 : public:
310 : uint8_t buffer[SIZE];
311 : };
312 :
313 : union u {
314 1 : u() {}
315 : char buffer[280]; // from-autopilot
316 : PackedMessage<Installation> packed_installation;
317 : PackedMessage<FlightIDMessage> packed_flightidmessage;
318 : PackedMessage<Operating> packed_operating;
319 : PackedMessage<GPS> packed_gps;
320 : PackedMessage<DataRequest> packed_data_req;
321 : PackedMessage<TargetRequest> packed_target_req;
322 : PackedMessage<Ack> packed_ack; // we probably don't get ACKs from the autopilot....
323 : PackedMessage<StateVectorReport> packed_state_vector_report;
324 : PackedMessage<ModeStatusReport> packed_status_report;
325 :
326 : struct PACKED {
327 : uint8_t start_byte;
328 : MsgType msgtype;
329 : uint8_t msgid;
330 : uint8_t payload_length;
331 : } preamble;
332 : } msg;
333 : uint8_t buflen;
334 :
335 : // called when there is a message in the buffer which should be
336 : // handled. If they return true then an ACK is sent.
337 : bool handle_message(const SITL::ADSB_Sagetech_MXS::Installation&);
338 : bool handle_message(const SITL::ADSB_Sagetech_MXS::FlightIDMessage&);
339 : bool handle_message(const SITL::ADSB_Sagetech_MXS::Operating&);
340 : bool handle_message(const SITL::ADSB_Sagetech_MXS::GPS&);
341 : bool handle_message(const SITL::ADSB_Sagetech_MXS::DataRequest&);
342 : bool handle_message(const SITL::ADSB_Sagetech_MXS::TargetRequest&);
343 : // bool handle_message(const SITL::ADSB_Sagetech_MXS::StateVectorReport&);
344 :
345 : bool _handle_message();
346 : void handle_message();
347 :
348 : double lon_string_to_double(const uint8_t *lat);
349 : double lat_string_to_double(const uint8_t *lng);
350 :
351 : /*
352 : * Simulated device state and methods
353 : */
354 : struct {
355 : bool send;
356 : MsgType ackType;
357 : uint8_t ackId;
358 : } ack_info;
359 :
360 : uint8_t msgid;
361 :
362 : struct {
363 : struct {
364 : double lat;
365 : double lng;
366 : } gps;
367 : } info_from_vehicle;
368 :
369 : void assert_good_flight_id();
370 : char flight_id[8];
371 : uint32_t flight_id_set_time_ms;
372 :
373 : /*
374 : serial parsing methods and data
375 : */
376 :
377 : // packet structure:
378 : // 1-byte start-byte
379 : // 1-byte msgtype
380 : // 1-byte msgid (sequence number)
381 : // 1-byte payloadlen
382 : // n-bytes payload
383 : // 1-byte checksum (sum of all previous bytes in packet % 256
384 :
385 : void move_preamble_in_buffer(uint8_t search_start_pos=0);
386 :
387 : enum class InputState {
388 : WANT_START_BYTE = 56,
389 : WANT_PREAMBLE = 57,
390 : WANT_PAYLOAD = 60,
391 : WANT_CHECKSUM = 61,
392 : } input_state = InputState::WANT_START_BYTE;
393 67 : void set_input_state(InputState newstate) {
394 : // ::fprintf(stderr, "Moving from inputstate (%u) to (%u)\n", (uint8_t)_inputstate, (uint8_t)newstate);
395 67 : input_state = newstate;
396 67 : }
397 :
398 : };
399 :
400 : } // namespace SITL
401 :
402 : #endif // AP_SIM_ADSB_SAGETECH_MXS_ENABLED
|
