Line data Source code
1 : /*
2 : * Copyright (C) 2022 Sagetech Avionics Inc. All rights reserved.
3 : *
4 : * This file is free software: you can redistribute it and/or modify it
5 : * under the terms of the GNU General Public License as published by the
6 : * Free Software Foundation, either version 3 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * This file is distributed in the hope that it will be useful, but
10 : * WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 : * See the GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License along
15 : * with this program. If not, see <http://www.gnu.org/licenses/>.
16 : *
17 : * SDK specification
18 : * https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf
19 : *
20 : * Authors: Chuck Faber, Tom Pittenger
21 : */
22 :
23 :
24 : #include "AP_ADSB_Sagetech_MXS.h"
25 :
26 : #if HAL_ADSB_SAGETECH_MXS_ENABLED
27 : #include <GCS_MAVLink/GCS.h>
28 : #include <AP_SerialManager/AP_SerialManager.h>
29 : #include <stdio.h>
30 : #include <time.h>
31 : #include <AP_Vehicle/AP_Vehicle_Type.h>
32 :
33 : #define SAGETECH_USE_MXS_SDK !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
34 :
35 : #define MXS_INIT_TIMEOUT 20000
36 :
37 : #define SAGETECH_SCALE_CM_TO_FEET (0.0328084f)
38 : #define SAGETECH_SCALE_FEET_TO_MM (304.8f)
39 : #define SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC (51.4444f)
40 : #define SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC (0.508f)
41 : #define SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN (196.85f)
42 : #define CLIMB_RATE_LIMIT 16448
43 :
44 : #define SAGETECH_INSTALL_MSG_RATE 5000
45 : #define SAGETECH_OPERATING_MSG_RATE 1000
46 : #define SAGETECH_FLIGHT_ID_MSG_RATE 8200
47 : #define SAGETECH_GPS_MSG_RATE_FLYING 200
48 : #define SAGETECH_GPS_MSG_RATE_GROUNDED 1000
49 : #define SAGETECH_TARGETREQ_MSG_RATE 1000
50 : #define SAGETECH_HFOM_UNKNOWN (19000.0f)
51 : #define SAGETECH_VFOM_UNKNOWN (151.0f)
52 : #define SAGETECH_HPL_UNKNOWN (38000.0f)
53 :
54 2 : bool AP_ADSB_Sagetech_MXS::detect()
55 : {
56 2 : return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
57 : }
58 :
59 :
60 2 : bool AP_ADSB_Sagetech_MXS::init()
61 : {
62 2 : _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
63 2 : if (_port == nullptr) {
64 0 : return false;
65 : }
66 2 : return true;
67 : }
68 :
69 96 : void AP_ADSB_Sagetech_MXS::update()
70 : {
71 96 : if (_port == nullptr) {
72 0 : return;
73 : }
74 :
75 : // -----------------------------
76 : // read any available data on serial port
77 : // -----------------------------
78 96 : uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_MXS_MAX_SIZE);
79 239796 : while (nbytes-- > 0) {
80 : uint8_t data;
81 239700 : if (!_port->read(data)) {
82 0 : break;
83 : }
84 239700 : parse_byte(data);
85 : }
86 :
87 96 : const uint32_t now_ms = AP_HAL::millis();
88 :
89 : // -----------------------------
90 : // handle timers for generating data
91 : // -----------------------------
92 96 : if (!mxs_state.init) {
93 2 : if (!last.packet_initialize_ms || (now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE)) {
94 2 : last.packet_initialize_ms = now_ms;
95 :
96 2 : if (_frontend._options & uint32_t(AP_ADSB::AdsbOption::SagteTech_MXS_External_Config)) {
97 : // request the device's configuration
98 0 : send_data_req(dataInstall);
99 :
100 : } else {
101 : // auto configure based on autopilot's saved parameters
102 2 : auto_config_operating();
103 2 : auto_config_installation();
104 2 : auto_config_flightid();
105 2 : send_targetreq_msg();
106 2 : _frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to 1090ES TX and RX
107 2 : mxs_state.init = true;
108 : }
109 :
110 0 : } else if (last.packet_initialize_ms > MXS_INIT_TIMEOUT && !mxs_state.init_failed) {
111 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Sagetech MXS: Initialization Timeout. Failed to initialize.");
112 0 : mxs_state.init_failed = true;
113 : }
114 :
115 : // before init is done, don't run any other update() tasks
116 2 : return;
117 : }
118 :
119 95 : if ((now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE) &&
120 1 : (mxs_state.inst.icao != (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get() ||
121 1 : mxs_state.inst.emitter != convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()) ||
122 1 : mxs_state.inst.size != _frontend.out_state.cfg.lengthWidth.get() ||
123 1 : mxs_state.inst.maxSpeed != (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots)
124 : )) {
125 1 : last.packet_initialize_ms = now_ms;
126 1 : send_install_msg();
127 :
128 93 : } else if (!last.packet_PreFlight_ms || (now_ms - last.packet_PreFlight_ms >= SAGETECH_FLIGHT_ID_MSG_RATE)) {
129 3 : last.packet_PreFlight_ms = now_ms;
130 3 : send_flight_id_msg();
131 :
132 269 : } else if (!last.packet_Operating_ms || // Send once at boot
133 89 : now_ms - last.packet_Operating_ms >= SAGETECH_OPERATING_MSG_RATE || // Send Operating Message every second
134 82 : last.operating_squawk != _frontend.out_state.ctrl.squawkCode || // Or anytime Operating data changes
135 82 : last.operating_squawk != _frontend.out_state.cfg.squawk_octal ||
136 82 : abs(last.operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit
137 81 : last.operating_rf_select != _frontend.out_state.cfg.rfSelect || // The following booleans control the MXS OpMode
138 81 : last.modeAEnabled != _frontend.out_state.ctrl.modeAEnabled ||
139 260 : last.modeCEnabled != _frontend.out_state.ctrl.modeCEnabled ||
140 81 : last.modeSEnabled != _frontend.out_state.ctrl.modeSEnabled
141 : ) {
142 :
143 9 : if (last.operating_squawk != _frontend.out_state.ctrl.squawkCode) {
144 0 : last.operating_squawk = _frontend.out_state.ctrl.squawkCode;
145 0 : _frontend.out_state.cfg.squawk_octal_param.set_and_notify(last.operating_squawk);
146 9 : } else if (last.operating_squawk != _frontend.out_state.cfg.squawk_octal) {
147 1 : last.operating_squawk = _frontend.out_state.cfg.squawk_octal;
148 1 : _frontend.out_state.ctrl.squawkCode = last.operating_squawk;
149 : }
150 9 : last.operating_rf_select = _frontend.out_state.cfg.rfSelect;
151 9 : last.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
152 9 : last.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
153 9 : last.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;
154 :
155 9 : last.operating_alt = _frontend._my_loc.alt;
156 9 : last.packet_Operating_ms = now_ms;
157 9 : send_operating_msg();
158 :
159 81 : } else if (now_ms - last.packet_GPS_ms >= (_frontend.out_state.is_flying ? SAGETECH_GPS_MSG_RATE_FLYING : SAGETECH_GPS_MSG_RATE_GROUNDED)) {
160 9 : last.packet_GPS_ms = now_ms;
161 9 : send_gps_msg();
162 :
163 144 : } else if ((now_ms - last.packet_targetReq >= SAGETECH_TARGETREQ_MSG_RATE) &&
164 72 : ((mxs_state.treq.icao != (uint32_t)_frontend._special_ICAO_target) || (mxs_state.treq.maxTargets != (uint16_t)_frontend.in_state.list_size_param))) {
165 0 : send_targetreq_msg();
166 : }
167 : }
168 :
169 8386 : void AP_ADSB_Sagetech_MXS::handle_packet(const Packet &msg)
170 : {
171 : #if SAGETECH_USE_MXS_SDK
172 8386 : switch (msg.type) {
173 3 : case MsgType::ACK:
174 3 : if(sgDecodeAck((uint8_t*) &msg, &mxs_state.ack)) {
175 3 : handle_ack(mxs_state.ack);
176 : }
177 3 : break;
178 :
179 0 : case MsgType::Installation_Response:
180 0 : if (!mxs_state.init && sgDecodeInstall((uint8_t*)&msg, &mxs_state.inst)) {
181 : // If not doing auto-config, get the current configuration of the MXS
182 : // Fill out configuration parameters with preconfigured values
183 0 : if (mxs_state.ack.opMode == modeOff) { // If the last ACK showed an OFF state, turn off all rfSelect bits.
184 0 : _frontend.out_state.cfg.rfSelect.set_and_notify(0);
185 0 : } else if (mxs_state.ack.opMode == modeStby) {
186 0 : _frontend.out_state.ctrl.modeAEnabled = false;
187 0 : _frontend.out_state.ctrl.modeCEnabled = false;
188 0 : _frontend.out_state.ctrl.modeSEnabled = false;
189 0 : _frontend.out_state.ctrl.es1090TxEnabled = false;
190 0 : } else if (mxs_state.ack.opMode == modeOn) {
191 0 : _frontend.out_state.ctrl.modeAEnabled = true;
192 0 : _frontend.out_state.ctrl.modeCEnabled = false;
193 0 : _frontend.out_state.ctrl.modeSEnabled = true;
194 0 : _frontend.out_state.ctrl.es1090TxEnabled = true;
195 0 : } else if (mxs_state.ack.opMode == modeAlt) {
196 0 : _frontend.out_state.ctrl.modeAEnabled = true;
197 0 : _frontend.out_state.ctrl.modeCEnabled = true;
198 0 : _frontend.out_state.ctrl.modeSEnabled = true;
199 0 : _frontend.out_state.ctrl.es1090TxEnabled = true;
200 : }
201 0 : _frontend.out_state.cfg.ICAO_id_param.set_and_notify(mxs_state.inst.icao);
202 0 : _frontend.out_state.cfg.lengthWidth.set_and_notify(mxs_state.inst.size);
203 0 : _frontend.out_state.cfg.emitterType.set_and_notify(convert_sg_emitter_type_to_adsb(mxs_state.inst.emitter));
204 :
205 0 : _frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to UAT and 1090ES TX and RX
206 :
207 0 : mxs_state.init = true;
208 : }
209 0 : break;
210 :
211 0 : case MsgType::FlightID_Response: {
212 0 : sg_flightid_t flightId {};
213 0 : if (sgDecodeFlightId((uint8_t*) &msg, &flightId)) {
214 0 : _frontend.set_callsign(flightId.flightId, false);
215 : }
216 0 : break;
217 : }
218 :
219 : // ADSB Messages
220 4155 : case MsgType::ADSB_StateVector_Report: {
221 4155 : sg_svr_t svr {};
222 4155 : if (sgDecodeSVR((uint8_t*) &msg, &svr)) {
223 4155 : handle_svr(svr);
224 : }
225 4155 : break;
226 : }
227 :
228 4228 : case MsgType::ADSB_ModeStatus_Report: {
229 4228 : sg_msr_t msr {};
230 4228 : if (sgDecodeMSR((uint8_t*) &msg, &msr)) {
231 4228 : handle_msr(msr);
232 : }
233 4228 : break;
234 : }
235 :
236 0 : case MsgType::Data_Request:
237 : case MsgType::Target_Request:
238 : case MsgType::Mode:
239 : case MsgType::Installation:
240 : case MsgType::FlightID:
241 : case MsgType::Operating:
242 : case MsgType::GPS_Data:
243 : case MsgType::Status_Response:
244 : case MsgType::Version_Response:
245 : case MsgType::Serial_Number_Response:
246 : case MsgType::Mode_Settings:
247 : case MsgType::Target_Summary_Report:
248 : case MsgType::RESERVED_0x84:
249 : case MsgType::RESERVED_0x85:
250 : case MsgType::RESERVED_0x8D:
251 : case MsgType::ADSB_Target_State_Report:
252 : case MsgType::ADSB_Air_Ref_Vel_Report:
253 : // unhandled or intended for out-bound only
254 0 : break;
255 : }
256 : #endif // SAGETECH_USE_MXS_SDK
257 8386 : }
258 :
259 239700 : bool AP_ADSB_Sagetech_MXS::parse_byte(const uint8_t data)
260 : {
261 239700 : switch (message_in.state) {
262 10743 : default:
263 : case ParseState::WaitingFor_Start:
264 10743 : if (data == START_BYTE) {
265 8474 : message_in.checksum = data; // initialize checksum here
266 8474 : message_in.state = ParseState::WaitingFor_MsgType;
267 : }
268 10743 : break;
269 8474 : case ParseState::WaitingFor_MsgType:
270 8474 : message_in.checksum += data;
271 8474 : message_in.packet.type = static_cast<MsgType>(data);
272 8474 : message_in.state = ParseState::WaitingFor_MsgId;
273 8474 : break;
274 8474 : case ParseState::WaitingFor_MsgId:
275 8474 : message_in.checksum += data;
276 8474 : message_in.packet.id = data;
277 8474 : message_in.state = ParseState::WaitingFor_PayloadLen;
278 8474 : break;
279 8474 : case ParseState::WaitingFor_PayloadLen:
280 8474 : message_in.checksum += data;
281 8474 : message_in.packet.payload_length = data;
282 : // the checksum is also appended to the payload array, so
283 : // we only allow a 254 byte payload here:
284 8474 : if (message_in.packet.payload_length >= ARRAY_SIZE(message_in.packet.payload)) {
285 0 : message_in.state = ParseState::WaitingFor_Start;
286 0 : break;
287 : }
288 8474 : message_in.index = 0;
289 8474 : message_in.state = (data == 0) ? ParseState::WaitingFor_Checksum : ParseState::WaitingFor_PayloadContents;
290 8474 : break;
291 195062 : case ParseState::WaitingFor_PayloadContents:
292 195062 : message_in.checksum += data;
293 195062 : message_in.packet.payload[message_in.index++] = data;
294 195062 : if (message_in.index >= message_in.packet.payload_length) {
295 8473 : message_in.state = ParseState::WaitingFor_Checksum;
296 : }
297 195062 : break;
298 8473 : case ParseState::WaitingFor_Checksum:
299 8473 : message_in.state = ParseState::WaitingFor_Start;
300 8473 : if (message_in.checksum == data) {
301 : // append the checksum to the payload and zero out the payload index
302 8386 : message_in.packet.payload[message_in.index] = data;
303 8386 : message_in.index = 0;
304 8386 : handle_packet(message_in.packet);
305 : }
306 8473 : break;
307 : }
308 239700 : return false;
309 : }
310 :
311 26 : void AP_ADSB_Sagetech_MXS::msg_write(const uint8_t *data, const uint16_t len) const
312 : {
313 26 : if (_port != nullptr) {
314 26 : _port->write(data, len);
315 : }
316 26 : }
317 :
318 2 : void AP_ADSB_Sagetech_MXS::auto_config_operating()
319 : {
320 : // Configure the Default Operation Message Data
321 2 : mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, _frontend.out_state.cfg.squawk_octal);
322 2 : mxs_state.op.opMode = sg_op_mode_t::modeOff; // MXS needs to start in OFF mode to accept installation message
323 2 : mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile
324 2 : mxs_state.op.enableSqt = true; // Enable extended squitters
325 2 : mxs_state.op.enableXBit = false; // Enable the x-bit
326 2 : mxs_state.op.milEmergency = false; // Broadcast a military emergency
327 2 : mxs_state.op.emergcType = (sg_emergc_t)_frontend.out_state.ctrl.emergencyState; // Enumerated civilian emergency type
328 :
329 2 : mxs_state.op.altUseIntrnl = true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field)
330 2 : mxs_state.op.altHostAvlbl = false;
331 2 : mxs_state.op.altRes25 = !mxs_state.inst.altRes100; // Host Altitude Resolution from install
332 :
333 2 : mxs_state.op.identOn = false;
334 :
335 2 : const auto &my_loc = _frontend._my_loc;
336 :
337 2 : populate_op_altitude(my_loc);
338 2 : populate_op_climbrate(my_loc);
339 2 : populate_op_airspeed_and_heading(my_loc);
340 :
341 2 : last.msg.type = SG_MSG_TYPE_HOST_OPMSG;
342 :
343 : #if SAGETECH_USE_MXS_SDK
344 2 : uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {};
345 2 : sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id);
346 2 : msg_write(txComBuffer, SG_MSG_LEN_OPMSG);
347 : #endif
348 2 : }
349 :
350 2 : void AP_ADSB_Sagetech_MXS::auto_config_installation()
351 : {
352 : // Configure the Default Installation Message Data
353 2 : mxs_state.inst.icao = (uint32_t) _frontend.out_state.cfg.ICAO_id_param;
354 2 : snprintf(mxs_state.inst.reg, 8, "%-7s", "TEST01Z");
355 :
356 2 : mxs_state.inst.com0 = sg_baud_t::baud230400;
357 2 : mxs_state.inst.com1 = sg_baud_t::baud57600;
358 :
359 2 : mxs_state.inst.eth.ipAddress = 0;
360 2 : mxs_state.inst.eth.subnetMask = 0;
361 2 : mxs_state.inst.eth.portNumber = 0;
362 :
363 2 : mxs_state.inst.sil = sg_sil_t::silUnknown;
364 2 : mxs_state.inst.sda = sg_sda_t::sdaUnknown;
365 2 : mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());
366 2 : mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
367 2 : mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
368 2 : mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0.
369 2 : mxs_state.inst.antenna = sg_antenna_t::antBottom;
370 :
371 2 : mxs_state.inst.altRes100 = true;
372 2 : mxs_state.inst.hdgTrueNorth = false;
373 2 : mxs_state.inst.airspeedTrue = false;
374 2 : mxs_state.inst.heater = true; // Heater should always be connected.
375 2 : mxs_state.inst.wowConnected = true;
376 :
377 2 : last.msg.type = SG_MSG_TYPE_HOST_INSTALL;
378 :
379 : #if SAGETECH_USE_MXS_SDK
380 2 : uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {};
381 2 : sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id);
382 2 : msg_write(txComBuffer, SG_MSG_LEN_INSTALL);
383 : #endif
384 2 : }
385 :
386 2 : void AP_ADSB_Sagetech_MXS::auto_config_flightid()
387 : {
388 2 : if (!strlen(_frontend.out_state.cfg.callsign)) {
389 2 : snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", "TESTMXS0");
390 : } else {
391 0 : snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", _frontend.out_state.cfg.callsign);
392 : }
393 2 : last.msg.type = SG_MSG_TYPE_HOST_FLIGHT;
394 :
395 : #if SAGETECH_USE_MXS_SDK
396 2 : uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {};
397 2 : sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id);
398 2 : msg_write(txComBuffer, SG_MSG_LEN_FLIGHT);
399 : #endif
400 2 : }
401 :
402 3 : void AP_ADSB_Sagetech_MXS::handle_ack(const sg_ack_t ack)
403 : {
404 3 : if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) {
405 : // GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: ACK: Message %d of type %02x not acknowledged.", last.msg.id, last.msg.type);
406 : }
407 : // System health
408 3 : if (ack.failXpdr && !last.failXpdr) {
409 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: Transponder Failure");
410 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
411 : }
412 3 : if (ack.failSystem && !last.failSystem) {
413 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: System Failure");
414 0 : _frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
415 : }
416 3 : last.failXpdr = ack.failXpdr;
417 3 : last.failSystem = ack.failSystem;
418 3 : }
419 :
420 4155 : void AP_ADSB_Sagetech_MXS::handle_svr(const sg_svr_t svr)
421 : {
422 4155 : if (svr.addrType != svrAdrIcaoUnknown && svr.addrType != svrAdrIcao && svr.addrType != svrAdrIcaoSurface) {
423 : // invalid icao
424 0 : return;
425 : }
426 :
427 : AP_ADSB::adsb_vehicle_t vehicle;
428 4155 : if (!_frontend.get_vehicle_by_ICAO(svr.addr, vehicle)) {
429 : // new vehicle
430 6 : memset(&vehicle, 0, sizeof(vehicle));
431 6 : vehicle.info.ICAO_address = svr.addr;
432 : }
433 :
434 4155 : vehicle.info.flags &= ~ADSB_FLAGS_VALID_SQUAWK;
435 :
436 4155 : if (svr.validity.position) {
437 4155 : vehicle.info.lat = (int32_t) (svr.lat * 1e7);
438 4155 : vehicle.info.lon = (int32_t) (svr.lon * 1e7);
439 4155 : vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
440 : }
441 4155 : if (svr.validity.geoAlt) {
442 4155 : vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
443 4155 : vehicle.info.altitude = svr.airborne.geoAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm
444 4155 : vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
445 : }
446 4155 : if (svr.validity.baroAlt) {
447 0 : vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
448 0 : vehicle.info.altitude = svr.airborne.baroAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm
449 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
450 : }
451 4155 : if (svr.validity.surfSpeed) {
452 0 : vehicle.info.hor_velocity = svr.surface.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s
453 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
454 : }
455 4155 : if (svr.validity.surfHeading) {
456 0 : vehicle.info.heading = svr.surface.heading * 100;
457 0 : vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
458 : }
459 4155 : if (svr.validity.airSpeed) {
460 4155 : vehicle.info.hor_velocity = svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s
461 4155 : vehicle.info.heading = svr.airborne.heading * 100;
462 4155 : vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
463 4155 : vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
464 : }
465 4155 : if (svr.validity.geoVRate || svr.validity.baroVRate) {
466 4155 : vehicle.info.ver_velocity = svr.airborne.vrate * SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC; // Convert from ft/min to cm/s
467 4155 : vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
468 : }
469 :
470 4155 : vehicle.last_update_ms = AP_HAL::millis();
471 4155 : _frontend.handle_adsb_vehicle(vehicle);
472 : }
473 :
474 4228 : void AP_ADSB_Sagetech_MXS::handle_msr(const sg_msr_t msr)
475 : {
476 : AP_ADSB::adsb_vehicle_t vehicle;
477 4228 : if (!_frontend.get_vehicle_by_ICAO(msr.addr, vehicle)) {
478 : // new vehicle is not allowed here because we don't know the lat/lng
479 : // yet and we don't allow lat/lng of (0,0) so it will get rejected anyway
480 0 : return;
481 : }
482 :
483 4228 : if (strlen(msr.callsign)) {
484 4228 : snprintf(vehicle.info.callsign, sizeof(vehicle.info.callsign), "%-8s", msr.callsign);
485 4228 : vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;
486 : } else {
487 0 : vehicle.info.flags &= ~ADSB_FLAGS_VALID_CALLSIGN;
488 : }
489 :
490 4228 : vehicle.last_update_ms = AP_HAL::millis();
491 4228 : _frontend.handle_adsb_vehicle(vehicle);
492 : }
493 :
494 0 : void AP_ADSB_Sagetech_MXS::send_data_req(const sg_datatype_t dataReqType)
495 : {
496 0 : sg_datareq_t dataReq {};
497 0 : dataReq.reqType = dataReqType;
498 0 : last.msg.type = SG_MSG_TYPE_HOST_DATAREQ;
499 :
500 : #if SAGETECH_USE_MXS_SDK
501 0 : uint8_t txComBuffer[SG_MSG_LEN_DATAREQ] {};
502 0 : sgEncodeDataReq(txComBuffer, &dataReq, ++last.msg.id);
503 0 : msg_write(txComBuffer, SG_MSG_LEN_DATAREQ);
504 : #else
505 : (void)dataReq;
506 : #endif
507 0 : }
508 :
509 1 : void AP_ADSB_Sagetech_MXS::send_install_msg()
510 : {
511 : // MXS must be in OFF mode to change ICAO or Registration
512 1 : if (mxs_state.op.opMode != modeOff) {
513 : // GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: unable to send installation data while not in OFF mode.");
514 1 : return;
515 : }
516 :
517 0 : mxs_state.inst.icao = (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get();
518 0 : mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());
519 0 : mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
520 0 : mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);
521 0 : mxs_state.inst.antenna = sg_antenna_t::antBottom;
522 :
523 0 : last.msg.type = SG_MSG_TYPE_HOST_INSTALL;
524 :
525 : #if SAGETECH_USE_MXS_SDK
526 0 : uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {};
527 0 : sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id);
528 0 : msg_write(txComBuffer, SG_MSG_LEN_INSTALL);
529 : #endif
530 : }
531 :
532 3 : void AP_ADSB_Sagetech_MXS::send_flight_id_msg()
533 : {
534 3 : if (!strlen((char*) _frontend.out_state.ctrl.callsign)) {
535 3 : return;
536 : }
537 0 : snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", (char*) _frontend.out_state.ctrl.callsign);
538 :
539 0 : last.msg.type = SG_MSG_TYPE_HOST_FLIGHT;
540 :
541 : #if SAGETECH_USE_MXS_SDK
542 0 : uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {};
543 0 : sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id);
544 0 : msg_write(txComBuffer, SG_MSG_LEN_FLIGHT);
545 : #endif
546 : }
547 :
548 9 : void AP_ADSB_Sagetech_MXS::send_operating_msg()
549 : {
550 9 : if (!_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&
551 9 : !_frontend.out_state.ctrl.modeSEnabled && !_frontend.out_state.ctrl.es1090TxEnabled) {
552 9 : mxs_state.op.opMode = modeStby;
553 : }
554 9 : if (_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&
555 0 : _frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {
556 0 : mxs_state.op.opMode = modeOn;
557 : }
558 9 : if (_frontend.out_state.ctrl.modeAEnabled && _frontend.out_state.ctrl.modeCEnabled &&
559 0 : _frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {
560 0 : mxs_state.op.opMode = modeAlt;
561 : }
562 9 : if ((_frontend.out_state.cfg.rfSelect & 1) == 0) {
563 0 : mxs_state.op.opMode = modeOff;
564 : }
565 :
566 9 : mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, last.operating_squawk);
567 9 : mxs_state.op.emergcType = (sg_emergc_t) _frontend.out_state.ctrl.emergencyState;
568 :
569 9 : const auto &my_loc = _frontend._my_loc;
570 :
571 9 : populate_op_altitude(my_loc);
572 9 : populate_op_climbrate(my_loc);
573 9 : populate_op_airspeed_and_heading(my_loc);
574 :
575 9 : mxs_state.op.identOn = _frontend.out_state.ctrl.identActive;
576 9 : _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request
577 :
578 9 : last.msg.type = SG_MSG_TYPE_HOST_OPMSG;
579 :
580 : #if SAGETECH_USE_MXS_SDK
581 9 : uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {};
582 9 : sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id);
583 9 : msg_write(txComBuffer, SG_MSG_LEN_OPMSG);
584 : #endif
585 9 : }
586 :
587 9 : void AP_ADSB_Sagetech_MXS::send_gps_msg()
588 : {
589 9 : sg_gps_t gps {};
590 9 : const AP_ADSB::Loc &ap_gps { _frontend._my_loc };
591 : float hAcc, vAcc, velAcc;
592 :
593 9 : gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown
594 9 : gps.hfom = ap_gps.horizontal_accuracy(hAcc) ? hAcc : SAGETECH_HFOM_UNKNOWN; // HFOM over 18,520 specifies unknown
595 9 : gps.vfom = ap_gps.vertical_accuracy(vAcc) ? vAcc : SAGETECH_VFOM_UNKNOWN; // VFOM over 150 specifies unknown
596 9 : gps.nacv = sg_nacv_t::nacvUnknown;
597 9 : if (ap_gps.speed_accuracy(velAcc)) {
598 2 : if (velAcc >= 10.0 || velAcc < 0) {
599 0 : gps.nacv = sg_nacv_t::nacvUnknown;
600 : }
601 2 : else if (velAcc >= 3.0) {
602 0 : gps.nacv = sg_nacv_t::nacv10dot0;
603 : }
604 2 : else if (velAcc >= 1.0) {
605 0 : gps.nacv = sg_nacv_t::nacv3dot0;
606 : }
607 2 : else if (velAcc >= 0.3) {
608 0 : gps.nacv = sg_nacv_t::nacv1dot0;
609 : }
610 : else { //if (velAcc >= 0.0)
611 2 : gps.nacv = sg_nacv_t::nacv0dot3;
612 : }
613 : }
614 :
615 : // Get Vehicle Longitude and Latitude and Convert to string
616 9 : const int32_t longitude = _frontend._my_loc.lng;
617 9 : const int32_t latitude = _frontend._my_loc.lat;
618 9 : const double lon_deg = longitude * (double)1.0e-7 * (longitude < 0 ? -1 : 1);
619 9 : const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
620 9 : snprintf((char*)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, unsigned((lon_minutes - (int)lon_minutes) * 1.0E5));
621 :
622 9 : const double lat_deg = latitude * (double)1.0e-7 * (latitude < 0 ? -1 : 1);
623 9 : const double lat_minutes = (lat_deg - int(lat_deg)) * 60;
624 9 : snprintf((char*)&gps.latitude, 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
625 :
626 9 : const Vector2f speed = _frontend._my_loc.groundspeed_vector();
627 9 : const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
628 9 : snprintf((char*)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
629 :
630 9 : if (!is_zero(speed_knots)) {
631 0 : cog = wrap_360(degrees(speed.angle()));
632 : }
633 9 : snprintf((char*)&gps.grdTrack, 9, "%03u.%04u", unsigned(cog), unsigned((cog - (int)cog) * 1.0E4));
634 :
635 :
636 9 : gps.latNorth = (latitude >= 0 ? true: false);
637 9 : gps.lngEast = (longitude >= 0 ? true: false);
638 :
639 9 : gps.gpsValid = ap_gps.status() >= AP_GPS_FixType::FIX_2D;
640 :
641 9 : uint64_t time_usec = ap_gps.epoch_from_rtc_us;
642 9 : if (ap_gps.have_epoch_from_rtc_us) {
643 2 : const time_t time_sec = time_usec * 1E-6;
644 2 : struct tm tmd {};
645 2 : struct tm* tm = gmtime_r(&time_sec, &tmd);
646 :
647 2 : snprintf((char*)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
648 : } else {
649 7 : strncpy(gps.timeOfFix, " . ", 11);
650 : }
651 :
652 : int32_t height;
653 9 : if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {
654 2 : gps.height = height * 0.01;
655 : } else {
656 7 : gps.height = 0.0;
657 : }
658 :
659 9 : last.msg.type = SG_MSG_TYPE_HOST_GPS;
660 :
661 : #if SAGETECH_USE_MXS_SDK
662 9 : uint8_t txComBuffer[SG_MSG_LEN_GPS] {};
663 9 : sgEncodeGPS(txComBuffer, &gps, ++last.msg.id);
664 9 : msg_write(txComBuffer, SG_MSG_LEN_GPS);
665 : #else
666 : (void)gps;
667 : #endif
668 9 : }
669 :
670 2 : void AP_ADSB_Sagetech_MXS::send_targetreq_msg()
671 : {
672 2 : mxs_state.treq.reqType = sg_reporttype_t::reportAuto;
673 2 : mxs_state.treq.transmitPort = sg_transmitport_t::transmitCom1;
674 2 : mxs_state.treq.maxTargets = _frontend.in_state.list_size_param;
675 2 : mxs_state.treq.icao = _frontend._special_ICAO_target.get();
676 2 : mxs_state.treq.stateVector = true;
677 2 : mxs_state.treq.modeStatus = true;
678 2 : mxs_state.treq.targetState = false;
679 2 : mxs_state.treq.airRefVel = false;
680 2 : mxs_state.treq.tisb = false;
681 2 : mxs_state.treq.military = false;
682 2 : mxs_state.treq.commA = false;
683 2 : mxs_state.treq.ownship = true;
684 :
685 2 : last.msg.type = SG_MSG_TYPE_HOST_TARGETREQ;
686 :
687 : #if SAGETECH_USE_MXS_SDK
688 2 : uint8_t txComBuffer[SG_MSG_LEN_TARGETREQ] {};
689 2 : sgEncodeTargetReq(txComBuffer, &mxs_state.treq, ++last.msg.id);
690 2 : msg_write(txComBuffer, SG_MSG_LEN_TARGETREQ);
691 : #endif
692 2 : }
693 :
694 3 : sg_emitter_t AP_ADSB_Sagetech_MXS::convert_emitter_type_to_sg(const uint8_t emitterType) const
695 : {
696 3 : uint8_t result = SG_EMIT_OFFSET_D;
697 :
698 3 : if (emitterType < 8) {
699 0 : result = emitterType;
700 3 : } else if (emitterType < 13 && emitterType >= 8) {
701 0 : result = (emitterType - 8) + SG_EMIT_OFFSET_B;
702 3 : } else if (emitterType < 16 && emitterType >= 14) {
703 3 : result = (emitterType - 14) + (SG_EMIT_OFFSET_B + 6); // Subtracting 14 because SG emitter types don't include the reserved state at value 13.
704 0 : } else if (emitterType < 21 && emitterType >= 16) {
705 0 : result = (emitterType - 16) + SG_EMIT_OFFSET_C;
706 : }
707 3 : return (sg_emitter_t)result;
708 : }
709 :
710 0 : uint8_t AP_ADSB_Sagetech_MXS::convert_sg_emitter_type_to_adsb(const sg_emitter_t sgEmitterType) const
711 : {
712 0 : if (sgEmitterType < SG_EMIT_OFFSET_B) {
713 0 : return sgEmitterType;
714 0 : } else if ((sgEmitterType < (SG_EMIT_OFFSET_B + 6)) && (sgEmitterType >= SG_EMIT_OFFSET_B)) {
715 0 : return (sgEmitterType - SG_EMIT_OFFSET_B) + 8;
716 0 : } else if ((sgEmitterType < SG_EMIT_OFFSET_C) && (sgEmitterType >= SG_EMIT_OFFSET_B + 6)) {
717 0 : return (sgEmitterType - (SG_EMIT_OFFSET_B + 6)) + 14; // Starts at UAV = 14
718 0 : } else if ((sgEmitterType < SG_EMIT_OFFSET_D) && (sgEmitterType >= SG_EMIT_OFFSET_C)) {
719 0 : return (sgEmitterType - SG_EMIT_OFFSET_C) + 16;
720 : } else {
721 0 : return 0;
722 : }
723 : }
724 :
725 0 : sg_airspeed_t AP_ADSB_Sagetech_MXS::convert_airspeed_knots_to_sg(const float maxAirSpeed) const
726 : {
727 0 : const int32_t airspeed = (int) maxAirSpeed;
728 :
729 0 : if (airspeed < 0) {
730 0 : return sg_airspeed_t::speedUnknown;
731 0 : } else if (airspeed < 75) {
732 0 : return sg_airspeed_t::speed75kt;
733 0 : } else if (airspeed < 150) {
734 0 : return sg_airspeed_t::speed150kt;
735 0 : } else if (airspeed < 300) {
736 0 : return sg_airspeed_t::speed300kt;
737 0 : } else if (airspeed < 600) {
738 0 : return sg_airspeed_t::speed600kt;
739 0 : } else if (airspeed < 1200) {
740 0 : return sg_airspeed_t::speed1200kt;
741 : } else { //if (airspeed >= 1200)
742 0 : return sg_airspeed_t::speedGreater;
743 : }
744 : }
745 :
746 11 : void AP_ADSB_Sagetech_MXS::populate_op_altitude(const AP_ADSB::Loc &loc)
747 : {
748 : int32_t height;
749 11 : if (loc.initialised() && loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {
750 3 : mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet
751 : } else {
752 8 : mxs_state.op.altitude = 0;
753 : }
754 11 : }
755 :
756 11 : void AP_ADSB_Sagetech_MXS::populate_op_climbrate(const AP_ADSB::Loc &my_loc)
757 : {
758 : float vertRateD;
759 11 : if (my_loc.get_vert_pos_rate_D(vertRateD)) {
760 : // convert from down to up, and scale appropriately:
761 11 : mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;
762 11 : mxs_state.op.climbValid = true;
763 : } else {
764 0 : mxs_state.op.climbValid = false;
765 0 : mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;
766 : }
767 11 : }
768 :
769 11 : void AP_ADSB_Sagetech_MXS::populate_op_airspeed_and_heading(const AP_ADSB::Loc &my_loc)
770 : {
771 11 : const Vector2f speed = my_loc.groundspeed_vector();
772 11 : if (!speed.is_nan() && !speed.is_zero()) {
773 1 : mxs_state.op.headingValid = true;
774 1 : mxs_state.op.airspdValid = true;
775 : } else {
776 10 : mxs_state.op.headingValid = false;
777 10 : mxs_state.op.airspdValid = false;
778 : }
779 11 : const uint16_t speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
780 11 : double heading = wrap_360(degrees(speed.angle()));
781 11 : mxs_state.op.airspd = speed_knots;
782 11 : mxs_state.op.heading = heading;
783 11 : }
784 :
785 : #endif // HAL_ADSB_SAGETECH_MXS_ENABLED
786 :
|
