Line data Source code
1 : /*
2 : Copyright (C) 2020 Kraus Hamdani Aerospace Inc. All rights reserved.
3 :
4 : This program is free software: you can redistribute it and/or modify
5 : it under the terms of the GNU General Public License as published by
6 : the Free Software Foundation, either version 3 of the License, or
7 : (at your option) any later version.
8 :
9 : This program is distributed in the hope that it will be useful,
10 : but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : GNU General Public License for more details.
13 :
14 : You should have received a copy of the GNU General Public License
15 : along with this program. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : #include "AP_ADSB_Sagetech.h"
19 :
20 : #if HAL_ADSB_SAGETECH_ENABLED
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <AP_SerialManager/AP_SerialManager.h>
23 : #include <AP_HAL/utility/sparse-endian.h>
24 : #include <stdio.h>
25 : #include <time.h>
26 : #include <string.h>
27 : #include <math.h>
28 :
29 : #define SAGETECH_SCALER_LATLNG (1.0f/2.145767E-5f) // 180/(2^23)
30 : #define SAGETECH_SCALER_KNOTS_TO_CMS ((KNOTS_TO_M_PER_SEC/0.125f) * 100.0f)
31 : #define SAGETECH_SCALER_ALTITUDE (1.0f/0.015625f)
32 : #define SAGETECH_SCALER_HEADING_CM ((360.0f/256.0f) * 100.0f)
33 :
34 : #define SAGETECH_VALIDFLAG_LATLNG (1U<<0)
35 : #define SAGETECH_VALIDFLAG_ALTITUDE (1U<<1)
36 : #define SAGETECH_VALIDFLAG_VELOCITY (1U<<2)
37 : #define SAGETECH_VALIDFLAG_GND_SPEED (1U<<3)
38 : #define SAGETECH_VALIDFLAG_HEADING (1U<<4)
39 : #define SAGETECH_VALIDFLAG_V_RATE_GEO (1U<<5)
40 : #define SAGETECH_VALIDFLAG_V_RATE_BARO (1U<<6)
41 : #define SAGETECH_VALIDFLAG_EST_LATLNG (1U<<7)
42 : #define SAGETECH_VALIDFLAG_EST_VELOCITY (1U<<8)
43 :
44 : // detect if any port is configured as Sagetech
45 0 : bool AP_ADSB_Sagetech::detect()
46 : {
47 0 : return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
48 : }
49 :
50 : // Init, called once after class is constructed
51 0 : bool AP_ADSB_Sagetech::init()
52 : {
53 0 : _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
54 :
55 0 : return (_port != nullptr);
56 : }
57 :
58 0 : void AP_ADSB_Sagetech::update()
59 : {
60 0 : if (_port == nullptr) {
61 0 : return;
62 : }
63 :
64 0 : const uint32_t now_ms = AP_HAL::millis();
65 :
66 : // -----------------------------
67 : // read any available data on serial port
68 : // -----------------------------
69 0 : uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_XP_MAX_SIZE);
70 0 : while (nbytes-- > 0) {
71 : uint8_t data;
72 0 : if (!_port->read(data)) {
73 0 : break;
74 : }
75 0 : if (parse_byte_XP(data)) {
76 0 : handle_packet_XP(message_in.packet);
77 : }
78 : } // while nbytes
79 :
80 :
81 : // -----------------------------
82 : // handle timers for generating data
83 : // -----------------------------
84 0 : if (!last_packet_initialize_ms || (now_ms - last_packet_initialize_ms >= 5000)) {
85 0 : last_packet_initialize_ms = now_ms;
86 0 : send_packet(MsgType_XP::Installation_Set);
87 :
88 0 : } else if (!last_packet_PreFlight_ms || (now_ms - last_packet_PreFlight_ms >= 8200)) {
89 0 : last_packet_PreFlight_ms = now_ms;
90 : // TODO: allow callsign to not require a reboot
91 0 : send_packet(MsgType_XP::Preflight_Set);
92 :
93 0 : } else if (now_ms - last_packet_Operating_ms >= 1000 && (
94 0 : last_packet_Operating_ms == 0 || // send once at boot
95 : // send as data changes
96 0 : last_operating_squawk != _frontend.out_state.cfg.squawk_octal ||
97 0 : abs(last_operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit
98 0 : last_operating_rf_select != _frontend.out_state.cfg.rfSelect))
99 : {
100 0 : last_packet_Operating_ms = now_ms;
101 0 : last_operating_squawk = _frontend.out_state.cfg.squawk_octal;
102 0 : last_operating_alt = _frontend._my_loc.alt;
103 0 : last_operating_rf_select = _frontend.out_state.cfg.rfSelect;
104 0 : send_packet(MsgType_XP::Operating_Set);
105 :
106 0 : } else if (now_ms - last_packet_GPS_ms >= (_frontend.out_state.is_flying ? 200 : 1000)) {
107 : // 1Hz when not flying, 5Hz when flying
108 0 : last_packet_GPS_ms = now_ms;
109 0 : send_packet(MsgType_XP::GPS_Set);
110 : }
111 : }
112 :
113 0 : void AP_ADSB_Sagetech::send_packet(const MsgType_XP type)
114 : {
115 0 : switch (type) {
116 0 : case MsgType_XP::Installation_Set:
117 0 : send_msg_Installation();
118 0 : break;
119 0 : case MsgType_XP::Preflight_Set:
120 0 : send_msg_PreFlight();
121 0 : break;
122 0 : case MsgType_XP::Operating_Set:
123 0 : send_msg_Operating();
124 0 : break;
125 0 : case MsgType_XP::GPS_Set:
126 0 : send_msg_GPS();
127 0 : break;
128 0 : default:
129 0 : break;
130 : }
131 0 : }
132 :
133 0 : void AP_ADSB_Sagetech::request_packet(const MsgType_XP type)
134 : {
135 : // set all bytes in packet to 0 via {} so we only need to set the ones we need to
136 0 : Packet_XP pkt {};
137 :
138 0 : pkt.type = MsgType_XP::Request;
139 0 : pkt.id = 0;
140 0 : pkt.payload_length = 4;
141 :
142 0 : pkt.payload[0] = static_cast<uint8_t>(type);
143 :
144 0 : send_msg(pkt);
145 0 : }
146 :
147 :
148 0 : void AP_ADSB_Sagetech::handle_packet_XP(const Packet_XP &msg)
149 : {
150 0 : switch (msg.type) {
151 0 : case MsgType_XP::ACK:
152 0 : handle_ack(msg);
153 0 : break;
154 :
155 0 : case MsgType_XP::Installation_Response:
156 : case MsgType_XP::Preflight_Response:
157 : case MsgType_XP::Status_Response:
158 : // TODO add support for these
159 0 : break;
160 :
161 0 : case MsgType_XP::ADSB_StateVector_Report:
162 : case MsgType_XP::ADSB_ModeStatus_Report:
163 : case MsgType_XP::TISB_StateVector_Report:
164 : case MsgType_XP::TISB_ModeStatus_Report:
165 : case MsgType_XP::TISB_CorasePos_Report:
166 : case MsgType_XP::TISB_ADSB_Mgr_Report:
167 0 : handle_adsb_in_msg(msg);
168 0 : break;
169 :
170 0 : case MsgType_XP::Installation_Set:
171 : case MsgType_XP::Preflight_Set:
172 : case MsgType_XP::Operating_Set:
173 : case MsgType_XP::GPS_Set:
174 : case MsgType_XP::Request:
175 : // these are out-bound only and are not expected to be received
176 : case MsgType_XP::INVALID:
177 0 : break;
178 : }
179 0 : }
180 :
181 0 : void AP_ADSB_Sagetech::handle_ack(const Packet_XP &msg)
182 : {
183 : // ACK received!
184 0 : const uint8_t system_state = msg.payload[2];
185 0 : transponder_type = (Transponder_Type)msg.payload[6];
186 :
187 0 : const uint8_t prev_transponder_mode = last_ack_transponder_mode;
188 0 : last_ack_transponder_mode = (system_state >> 6) & 0x03;
189 0 : if (prev_transponder_mode != last_ack_transponder_mode) {
190 : static const char *mode_names[] = {"OFF", "STBY", "ON", "ON-ALT"};
191 0 : if (last_ack_transponder_mode < ARRAY_SIZE(mode_names)) {
192 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ADSB: RF Mode: %s", mode_names[last_ack_transponder_mode]);
193 : }
194 : }
195 0 : }
196 :
197 0 : void AP_ADSB_Sagetech::handle_adsb_in_msg(const Packet_XP &msg)
198 : {
199 0 : AP_ADSB::adsb_vehicle_t vehicle {};
200 :
201 0 : vehicle.last_update_ms = AP_HAL::millis();
202 :
203 0 : switch (msg.type) {
204 0 : case MsgType_XP::ADSB_StateVector_Report: { // 0x91
205 0 : const uint16_t validFlags = le16toh_ptr(&msg.payload[8]);
206 0 : vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[10]);
207 :
208 0 : if (validFlags & SAGETECH_VALIDFLAG_LATLNG) {
209 0 : vehicle.info.lat = ((int32_t)le24toh_ptr(&msg.payload[20])) * SAGETECH_SCALER_LATLNG;
210 0 : vehicle.info.lon = ((int32_t)le24toh_ptr(&msg.payload[23])) * SAGETECH_SCALER_LATLNG;
211 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
212 : }
213 :
214 0 : if (validFlags & SAGETECH_VALIDFLAG_ALTITUDE) {
215 0 : vehicle.info.altitude = (int32_t)le24toh_ptr(&msg.payload[26]);
216 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
217 : }
218 :
219 0 : if (validFlags & SAGETECH_VALIDFLAG_VELOCITY) {
220 0 : const float velNS = ((int32_t)le16toh_ptr(&msg.payload[29])) * SAGETECH_SCALER_KNOTS_TO_CMS;
221 0 : const float velEW = ((int32_t)le16toh_ptr(&msg.payload[31])) * SAGETECH_SCALER_KNOTS_TO_CMS;
222 0 : vehicle.info.hor_velocity = Vector2f(velEW, velNS).length();
223 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
224 : }
225 :
226 0 : if (validFlags & SAGETECH_VALIDFLAG_HEADING) {
227 0 : vehicle.info.heading = ((float)msg.payload[29]) * SAGETECH_SCALER_HEADING_CM;
228 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
229 : }
230 :
231 0 : if ((validFlags & SAGETECH_VALIDFLAG_V_RATE_GEO) || (validFlags & SAGETECH_VALIDFLAG_V_RATE_BARO)) {
232 0 : vehicle.info.ver_velocity = (int16_t)le16toh_ptr(&msg.payload[38]);
233 0 : vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
234 : }
235 :
236 0 : _frontend.handle_adsb_vehicle(vehicle);
237 0 : break;
238 : }
239 0 : case MsgType_XP::ADSB_ModeStatus_Report: // 0x92
240 0 : vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[9]);
241 :
242 0 : if (msg.payload[16] != 0) {
243 : // if string is non-null, consider it valid
244 : // "The callsign, 8+null" (from the mavlink spec) means we subtract one here
245 0 : memcpy(&vehicle.info.callsign, &msg.payload[16], MIN(ARRAY_SIZE(vehicle.info.callsign)-1, 8U));
246 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;
247 : }
248 :
249 0 : _frontend.handle_adsb_vehicle(vehicle);
250 0 : break;
251 0 : case MsgType_XP::TISB_StateVector_Report:
252 : case MsgType_XP::TISB_ModeStatus_Report:
253 : case MsgType_XP::TISB_CorasePos_Report:
254 : case MsgType_XP::TISB_ADSB_Mgr_Report:
255 : // TODO
256 0 : return;
257 :
258 0 : default:
259 0 : return;
260 : }
261 :
262 : }
263 :
264 : // handling inbound byte and process it in the state machine
265 : // return true when a full packet has been received
266 0 : bool AP_ADSB_Sagetech::parse_byte_XP(const uint8_t data)
267 : {
268 0 : switch (message_in.state) {
269 0 : default:
270 : case ParseState::WaitingFor_Start:
271 0 : if (data == 0xA5) {
272 0 : message_in.state = ParseState::WaitingFor_AssmAddr;
273 : }
274 0 : break;
275 0 : case ParseState::WaitingFor_AssmAddr:
276 0 : message_in.state = (data == 0x01) ? ParseState::WaitingFor_MsgType : ParseState::WaitingFor_Start;
277 0 : break;
278 0 : case ParseState::WaitingFor_MsgType:
279 0 : message_in.packet.type = static_cast<MsgType_XP>(data);
280 0 : message_in.state = ParseState::WaitingFor_MsgId;
281 0 : break;
282 0 : case ParseState::WaitingFor_MsgId:
283 0 : message_in.packet.id = data;
284 0 : message_in.state = ParseState::WaitingFor_PayloadLen;
285 0 : break;
286 0 : case ParseState::WaitingFor_PayloadLen:
287 0 : message_in.packet.payload_length = data;
288 0 : message_in.index = 0;
289 0 : message_in.state = (data == 0) ? ParseState::WaitingFor_ChecksumFletcher : ParseState::WaitingFor_PayloadContents;
290 0 : break;
291 :
292 0 : case ParseState::WaitingFor_PayloadContents:
293 0 : message_in.packet.payload[message_in.index++] = data;
294 0 : if (message_in.index >= message_in.packet.payload_length) {
295 0 : message_in.state = ParseState::WaitingFor_ChecksumFletcher;
296 0 : message_in.index = 0;
297 : }
298 0 : break;
299 :
300 0 : case ParseState::WaitingFor_ChecksumFletcher:
301 0 : message_in.packet.checksumFletcher = data;
302 0 : message_in.state = ParseState::WaitingFor_Checksum;
303 0 : break;
304 0 : case ParseState::WaitingFor_Checksum:
305 0 : message_in.packet.checksum = data;
306 0 : message_in.state = ParseState::WaitingFor_End;
307 0 : if (checksum_verify_XP(message_in.packet)) {
308 0 : handle_packet_XP(message_in.packet);
309 : }
310 0 : break;
311 :
312 0 : case ParseState::WaitingFor_End:
313 : // we don't care if the end value matches
314 0 : message_in.state = ParseState::WaitingFor_Start;
315 0 : break;
316 : }
317 0 : return false;
318 : }
319 :
320 : // compute Sum and FletcherSum values into a single value
321 : // returns uint16_t with MSByte as Sum and LSByte FletcherSum
322 0 : uint16_t AP_ADSB_Sagetech::checksum_generate_XP(Packet_XP &msg) const
323 : {
324 0 : uint8_t sum = 0;
325 0 : uint8_t sumFletcher = 0;
326 :
327 0 : const uint8_t header_message_format[5] {
328 : 0xA5, // start
329 : 0x01, // assembly address
330 0 : static_cast<uint8_t>(msg.type),
331 0 : msg.id,
332 0 : msg.payload_length
333 0 : };
334 :
335 0 : for (uint8_t i=0; i<5; i++) {
336 0 : sum += header_message_format[i];
337 0 : sumFletcher += sum;
338 : }
339 0 : for (uint8_t i=0; i<msg.payload_length; i++) {
340 0 : sum += msg.payload[i];
341 0 : sumFletcher += sum;
342 : }
343 :
344 0 : return UINT16_VALUE(sum, sumFletcher);
345 : }
346 :
347 : // computes checksum and returns true if it matches msg checksum
348 0 : bool AP_ADSB_Sagetech::checksum_verify_XP(Packet_XP &msg) const
349 : {
350 0 : const uint16_t checksum = checksum_generate_XP(msg);
351 0 : return (HIGHBYTE(checksum) == msg.checksum) && (LOWBYTE(checksum) == msg.checksumFletcher);
352 : }
353 :
354 : // computes checksum and assigns checksum values to msg
355 0 : void AP_ADSB_Sagetech::checksum_assign_XP(Packet_XP &msg)
356 : {
357 0 : const uint16_t checksum = checksum_generate_XP(msg);
358 0 : msg.checksum = HIGHBYTE(checksum);
359 0 : msg.checksumFletcher = LOWBYTE(checksum);
360 0 : }
361 :
362 : // send message to serial port
363 0 : void AP_ADSB_Sagetech::send_msg(Packet_XP &msg)
364 : {
365 : // generate and populate checksums.
366 0 : checksum_assign_XP(msg);
367 :
368 0 : const uint8_t message_format_header[5] {
369 : 0xA5, // start
370 : 0x01, // assembly address
371 0 : static_cast<uint8_t>(msg.type),
372 0 : msg.id,
373 0 : msg.payload_length
374 0 : };
375 0 : const uint8_t message_format_tail[3] {
376 0 : msg.checksumFletcher,
377 0 : msg.checksum,
378 : 0x5A // end
379 0 : };
380 :
381 0 : if (_port != nullptr) {
382 0 : _port->write(message_format_header, sizeof(message_format_header));
383 0 : _port->write(msg.payload, msg.payload_length);
384 0 : _port->write(message_format_tail, sizeof(message_format_tail));
385 : }
386 0 : }
387 :
388 0 : void AP_ADSB_Sagetech::send_msg_Installation()
389 : {
390 0 : Packet_XP pkt {};
391 :
392 0 : pkt.type = MsgType_XP::Installation_Set;
393 0 : pkt.payload_length = 28; // 28== 0x1C
394 :
395 : // Mode C = 3, Mode S = 0
396 0 : pkt.id = (transponder_type == Transponder_Type::Mode_C) ? 3 : 0;
397 :
398 : // // convert a decimal 123456 to 0x123456
399 : // TODO: do a proper conversion. The param contains "131313" but what gets transmitted over the air is 0x200F1.
400 0 : const uint32_t icao_hex = AP_ADSB::convert_base_to_decimal(16, _frontend.out_state.cfg.ICAO_id_param);
401 0 : put_le24_ptr(&pkt.payload[0], icao_hex);
402 :
403 0 : memcpy(&pkt.payload[3], &_frontend.out_state.cfg.callsign, 8);
404 :
405 0 : pkt.payload[11] = 0; // airspeed MAX
406 :
407 0 : pkt.payload[12] = 0; // COM Port 0 baud, fixed at 57600
408 0 : pkt.payload[13] = 0; // COM Port 1 baud, fixed at 57600
409 0 : pkt.payload[14] = 0; // COM Port 2 baud, fixed at 57600
410 :
411 0 : pkt.payload[15] = 1; // GPS from COM port 0 (this port)
412 0 : pkt.payload[16] = 1; // GPS Integrity
413 :
414 0 : pkt.payload[17] = _frontend.out_state.cfg.emitterType / 8; // Emitter Set
415 0 : pkt.payload[18] = _frontend.out_state.cfg.emitterType & 0x0F; // Emitter Type
416 :
417 0 : pkt.payload[19] = _frontend.out_state.cfg.lengthWidth; // Aircraft Size
418 :
419 0 : pkt.payload[20] = 0; // Altitude Encoder Offset
420 0 : pkt.payload[21] = 0; // Altitude Encoder Offset
421 :
422 0 : pkt.payload[22] = 0x07; // ADSB In Control, enable reading everything
423 0 : pkt.payload[23] = 30; // ADSB In Report max length COM Port 0 (this one)
424 0 : pkt.payload[24] = 0; // ADSB In Report max length COM Port 1
425 :
426 0 : send_msg(pkt);
427 0 : }
428 :
429 0 : void AP_ADSB_Sagetech::send_msg_PreFlight()
430 : {
431 0 : Packet_XP pkt {};
432 :
433 0 : pkt.type = MsgType_XP::Preflight_Set;
434 0 : pkt.id = 0;
435 0 : pkt.payload_length = 10;
436 :
437 0 : memcpy(&pkt.payload[0], &_frontend.out_state.cfg.callsign, 8);
438 :
439 0 : memset(&pkt.payload[8], 0, 2);
440 :
441 0 : send_msg(pkt);
442 0 : }
443 :
444 0 : void AP_ADSB_Sagetech::send_msg_Operating()
445 : {
446 0 : Packet_XP pkt {};
447 :
448 0 : pkt.type = MsgType_XP::Operating_Set;
449 0 : pkt.id = 0;
450 0 : pkt.payload_length = 8;
451 :
452 : // squawk
453 : // param is saved as native octal so we need convert back to
454 : // decimal because Sagetech will convert it back to octal
455 0 : const uint16_t squawk = AP_ADSB::convert_base_to_decimal(8, last_operating_squawk);
456 0 : put_le16_ptr(&pkt.payload[0], squawk);
457 :
458 : // altitude
459 0 : if (_frontend.out_state.cfg.rf_capable & 0x01) {
460 0 : const float alt_meters = last_operating_alt * 0.01f;
461 0 : const int32_t alt_feet = (int32_t)(alt_meters * FEET_TO_METERS);
462 0 : const int16_t alt_feet_adj = (alt_feet + 50) / 100; // 1 = 100 feet, 1 = 149 feet, 5 = 500 feet
463 0 : put_le16_ptr(&pkt.payload[2], alt_feet_adj);
464 :
465 : } else {
466 : // use integrated altitude - recommend by sagetech
467 0 : pkt.payload[2] = 0x80;
468 0 : pkt.payload[3] = 0x00;
469 : }
470 :
471 : // RF mode
472 0 : pkt.payload[4] = last_operating_rf_select;
473 0 : send_msg(pkt);
474 0 : }
475 :
476 0 : void AP_ADSB_Sagetech::send_msg_GPS()
477 : {
478 0 : Packet_XP pkt {};
479 :
480 0 : pkt.type = MsgType_XP::GPS_Set;
481 0 : pkt.payload_length = 52;
482 0 : pkt.id = 0;
483 :
484 0 : const auto &loc = _frontend._my_loc;
485 :
486 0 : const int32_t longitude = loc.lng;
487 0 : const int32_t latitude = loc.lat;
488 :
489 : // longitude and latitude
490 : // NOTE: these MUST be done in double or else we get roundoff in the maths
491 0 : const double lon_deg = longitude * (double)1.0e-7 * (longitude < 0 ? -1 : 1);
492 0 : const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
493 0 : snprintf((char*)&pkt.payload[0], 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, unsigned((lon_minutes - (int)lon_minutes) * 1.0E5));
494 :
495 0 : const double lat_deg = latitude * (double)1.0e-7 * (latitude < 0 ? -1 : 1);
496 0 : const double lat_minutes = (lat_deg - int(lat_deg)) * 60;
497 0 : snprintf((char*)&pkt.payload[11], 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
498 :
499 : // ground speed
500 0 : const Vector2f speed = loc.groundspeed_vector();
501 0 : float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
502 0 : snprintf((char*)&pkt.payload[21], 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
503 :
504 : // heading
505 0 : float heading = wrap_360(degrees(speed.angle()));
506 0 : snprintf((char*)&pkt.payload[27], 10, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * 1.0E4));
507 :
508 : // hemisphere
509 0 : uint8_t hemisphere = 0;
510 0 : hemisphere |= (latitude >= 0) ? 0x01 : 0; // isNorth
511 0 : hemisphere |= (longitude >= 0) ? 0x02 : 0; // isEast
512 0 : hemisphere |= (loc.status() < AP_GPS_FixType::FIX_2D) ? 0x80 : 0; // isInvalid
513 0 : pkt.payload[35] = hemisphere;
514 :
515 : // time
516 0 : uint64_t time_usec = loc.epoch_from_rtc_us;
517 0 : if (loc.have_epoch_from_rtc_us) {
518 : // not completely accurate, our time includes leap seconds and time_t should be without
519 0 : const time_t time_sec = time_usec / 1000000;
520 0 : struct tm tmd {};
521 0 : struct tm* tm = gmtime_r(&time_sec, &tmd);
522 :
523 : // format time string
524 0 : snprintf((char*)&pkt.payload[36], 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
525 : } else {
526 0 : memset(&pkt.payload[36],' ', 10);
527 : }
528 :
529 0 : send_msg(pkt);
530 0 : }
531 :
532 : #endif // HAL_ADSB_SAGETECH_ENABLED
533 :
|
