From 32544452f0f91e8063197a369506d147dd459143 Mon Sep 17 00:00:00 2001 From: Chuck <31755523+faberc@users.noreply.github.com> Date: Mon, 20 Jun 2022 15:16:07 -0700 Subject: [PATCH] drivers: Sagetech MXS transponder support Co-authored-by: Megan McCormick Co-authored-by: Chuck Faber --- Tools/px4moduledoc/srcparser.py | 2 +- boards/cuav/x7pro/default.px4board | 1 + boards/cubepilot/cubeorange/default.px4board | 4 +- msg/templates/module.yaml | 6 + src/drivers/drv_sensor.h | 2 + src/drivers/transponder/CMakeLists.txt | 34 + src/drivers/transponder/Kconfig | 10 + .../transponder/sagetech_mxs/CMakeLists.txt | 82 + src/drivers/transponder/sagetech_mxs/Kconfig | 5 + .../transponder/sagetech_mxs/SagetechMXS.cpp | 1380 +++++++++++++++++ .../transponder/sagetech_mxs/SagetechMXS.hpp | 279 ++++ .../transponder/sagetech_mxs/module.yaml | 8 + .../transponder/sagetech_mxs/parameters.c | 269 ++++ .../transponder/sagetech_mxs/sg_sdk/LICENSE | 201 +++ .../transponder/sagetech_mxs/sg_sdk/README.md | 6 + .../sagetech_mxs/sg_sdk/appendChecksum.c | 19 + .../sagetech_mxs/sg_sdk/calcChecksum.c | 26 + .../sagetech_mxs/sg_sdk/charArray2Buf.c | 22 + .../sagetech_mxs/sg_sdk/float2Buf.c | 30 + .../sagetech_mxs/sg_sdk/icao2Buf.c | 21 + .../sagetech_mxs/sg_sdk/sagetech_mxs.h | 13 + .../transponder/sagetech_mxs/sg_sdk/sg.h | 899 +++++++++++ .../sagetech_mxs/sg_sdk/sgDecodeAck.c | 72 + .../sagetech_mxs/sg_sdk/sgDecodeFlightId.c | 41 + .../sagetech_mxs/sg_sdk/sgDecodeInstall.c | 84 + .../sagetech_mxs/sg_sdk/sgDecodeMSR.c | 183 +++ .../sagetech_mxs/sg_sdk/sgDecodeSVR.c | 209 +++ .../sagetech_mxs/sg_sdk/sgEncodeDataReq.c | 51 + .../sagetech_mxs/sg_sdk/sgEncodeFlightId.c | 47 + .../sagetech_mxs/sg_sdk/sgEncodeGPS.c | 92 ++ .../sagetech_mxs/sg_sdk/sgEncodeInstall.c | 132 ++ .../sagetech_mxs/sg_sdk/sgEncodeOperating.c | 107 ++ .../sagetech_mxs/sg_sdk/sgEncodeTargetReq.c | 63 + .../transponder/sagetech_mxs/sg_sdk/sgUtil.h | 325 ++++ .../transponder/sagetech_mxs/sg_sdk/target.c | 127 ++ .../transponder/sagetech_mxs/sg_sdk/target.h | 128 ++ .../transponder/sagetech_mxs/sg_sdk/toAlt.c | 24 + .../transponder/sagetech_mxs/sg_sdk/toGS.c | 50 + .../sagetech_mxs/sg_sdk/toHeading.c | 24 + .../sagetech_mxs/sg_sdk/toHeading2.c | 33 + .../transponder/sagetech_mxs/sg_sdk/toIcao.c | 21 + .../transponder/sagetech_mxs/sg_sdk/toInt16.c | 21 + .../transponder/sagetech_mxs/sg_sdk/toInt32.c | 22 + .../sagetech_mxs/sg_sdk/toLatLon.c | 24 + .../transponder/sagetech_mxs/sg_sdk/toTOA.c | 20 + .../sagetech_mxs/sg_sdk/toUint16.c | 21 + .../sagetech_mxs/sg_sdk/toUint32.c | 22 + .../transponder/sagetech_mxs/sg_sdk/toVel.c | 24 + .../sagetech_mxs/sg_sdk/uint162Buf.c | 20 + .../sagetech_mxs/sg_sdk/uint322Buf.c | 22 + 50 files changed, 5325 insertions(+), 3 deletions(-) create mode 100644 msg/templates/module.yaml create mode 100644 src/drivers/transponder/CMakeLists.txt create mode 100644 src/drivers/transponder/Kconfig create mode 100644 src/drivers/transponder/sagetech_mxs/CMakeLists.txt create mode 100644 src/drivers/transponder/sagetech_mxs/Kconfig create mode 100644 src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp create mode 100644 src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp create mode 100644 src/drivers/transponder/sagetech_mxs/module.yaml create mode 100644 src/drivers/transponder/sagetech_mxs/parameters.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/README.md create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/target.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/target.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index 258ba0bfa4..b843a235f1 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -16,7 +16,7 @@ class ModuleDocumentation(object): valid_categories = ['driver', 'estimator', 'controller', 'system', 'communication', 'command', 'template', 'simulation', 'autotune'] valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor', - 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor'] + 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder'] max_line_length = 80 # wrap lines that are longer than this diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index a262ced94e..38d2db27ce 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -106,3 +106,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y +#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index f3dd85dbb3..c8b1e35ed4 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -24,14 +24,13 @@ CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_DRIVERS_OSD=y -CONFIG_DRIVERS_PCA9685=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_DRIVERS_PX4IO=y -CONFIG_DRIVERS_ROBOCLAW=y +#CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y @@ -101,3 +100,4 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/msg/templates/module.yaml b/msg/templates/module.yaml new file mode 100644 index 0000000000..099976c11e --- /dev/null +++ b/msg/templates/module.yaml @@ -0,0 +1,6 @@ +module_name: Sagtech MXS +serial_config: + - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} + port_config_param: + name: TRANS_MXS_CFG + group: Transponders diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 360c5b9913..4c9b1ab90d 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -208,6 +208,8 @@ #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 +#define DRV_TRNS_DEVTYPE_MXS 0xB2 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/transponder/CMakeLists.txt b/src/drivers/transponder/CMakeLists.txt new file mode 100644 index 0000000000..571c65554a --- /dev/null +++ b/src/drivers/transponder/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(sagetech_mxs) diff --git a/src/drivers/transponder/Kconfig b/src/drivers/transponder/Kconfig new file mode 100644 index 0000000000..d1c2f9fbb8 --- /dev/null +++ b/src/drivers/transponder/Kconfig @@ -0,0 +1,10 @@ +menu "Transponder" + menuconfig TRANSPONDER + bool "Transponder" + default n + select DRIVERS_TRANSPONDER_SAGETECH_MXS + ---help--- + Enable default set of transponder drivers + + rsource "*/Kconfig" +endmenu #transponder diff --git a/src/drivers/transponder/sagetech_mxs/CMakeLists.txt b/src/drivers/transponder/sagetech_mxs/CMakeLists.txt new file mode 100644 index 0000000000..56d7049218 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/CMakeLists.txt @@ -0,0 +1,82 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__transponder__sagetech_mxs + MAIN sagetech_mxs + COMPILE_FLAGS + #-DDEBUG_BUILD # uncomment for PX4_DEBUG output + #-O0 # uncomment when debugging + SRCS + sg_sdk/appendChecksum.c + sg_sdk/float2Buf.c + sg_sdk/sgDecodeFlightId.c + sg_sdk/sgDecodeSVR.c + sg_sdk/sgEncodeGPS.c + sg_sdk/sgEncodeTargetReq.c + sg_sdk/target.c + sg_sdk/toGS.c + sg_sdk/toIcao.c + sg_sdk/toLatLon.c + sg_sdk/toUint32.c + sg_sdk/uint322Buf.c + sg_sdk/calcChecksum.c + sg_sdk/icao2Buf.c + sg_sdk/sgDecodeInstall.c + sg_sdk/sgEncodeDataReq.c + sg_sdk/sgEncodeInstall.c + sg_sdk/toHeading2.c + sg_sdk/toInt16.c + sg_sdk/toTOA.c + sg_sdk/toVel.c + sg_sdk/charArray2Buf.c + sg_sdk/sgDecodeAck.c + sg_sdk/sgDecodeMSR.c + sg_sdk/sgEncodeFlightId.c + sg_sdk/sgEncodeOperating.c + sg_sdk/toAlt.c + sg_sdk/toHeading.c + sg_sdk/toInt32.c + sg_sdk/toUint16.c + sg_sdk/uint162Buf.c + sg_sdk/sagetech_mxs.h + sg_sdk/sg.h + sg_sdk/target.h + sg_sdk/sgUtil.h + SagetechMXS.cpp + SagetechMXS.hpp + DEPENDS + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/transponder/sagetech_mxs/Kconfig b/src/drivers/transponder/sagetech_mxs/Kconfig new file mode 100644 index 0000000000..eeb3a8fed7 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_TRANSPONDER_SAGETECH_MXS + bool "sagetech_mxs" + default n + ---help--- + Enable support for my_work_item diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp new file mode 100644 index 0000000000..d355098235 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -0,0 +1,1380 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SagetechMXS.hpp" + +/*************************************** + * Workqueue Functions + * *************************************/ + +extern "C" __EXPORT int sagetech_mxs_main(int argc, char *argv[]) +{ + return SagetechMXS::main(argc, argv); +} + +SagetechMXS::SagetechMXS(const char *port) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) +{ + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; +} + +SagetechMXS::~SagetechMXS() +{ + free((char *)_port); + + if (!(_fd < 0)) { + close(_fd); + } + + perf_free(_loop_elapsed_perf); + perf_free(_loop_count_perf); + perf_free(_loop_interval_perf); + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SagetechMXS::task_spawn(int argc, char *argv[]) +{ + const char *device_path = nullptr; + int ch = '\0'; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("unrecognized flag"); + return PX4_ERROR; + break; + } + } + + SagetechMXS *instance = new SagetechMXS(device_path); + + if (instance == nullptr) { + PX4_ERR("Allocation Failed."); + } + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; +} + + + +bool SagetechMXS::init() +{ + ScheduleOnInterval(UPDATE_INTERVAL_US); // 50Hz + mxs_state.initialized = false; + mxs_state.init_failed = false; + + if (vehicle_list == nullptr) { + if (_adsb_list_max.get() > MAX_VEHICLES_LIMIT) { // Safety Check + _adsb_list_max.set(MAX_VEHICLES_LIMIT); + _adsb_list_max.commit(); + list_size_allocated = MAX_VEHICLES_LIMIT; + } + + vehicle_list = new transponder_report_s[_adsb_list_max.get()]; + + if (vehicle_list == nullptr) { + mxs_state.init_failed = true; + PX4_ERR("Unable to initialize vehicle list."); + return false; + } + + list_size_allocated = _adsb_list_max.get(); + } + + return true; +} + +int SagetechMXS::custom_command(int argc, char *argv[]) +{ + const char *verb = argv[0]; + + if (!is_running()) { + int ret = SagetechMXS::task_spawn(argc, argv); + return ret; + + + } + + if (!strcmp(verb, "flightid")) { + const char *fid = argv[1]; + + if (fid == nullptr) { + print_usage("Missing Flight ID"); + return PX4_ERROR; + } + + return get_instance()->handle_fid(fid); + } + + if (!strcmp(verb, "ident")) { + get_instance()->_adsb_ident.set(1); + return get_instance()->_adsb_ident.commit(); + } + + if (!strcmp(verb, "opmode")) { + const char *opmode = argv[1]; + + if (opmode == nullptr) { + print_usage("Missing Op Mode"); + return PX4_ERROR; + + } else if (!strcmp(opmode, "off") || !strcmp(opmode, "0")) { + get_instance()->_mxs_op_mode.set(0); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "on") || !strcmp(opmode, "1")) { + get_instance()->_mxs_op_mode.set(1); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "stby") || !strcmp(opmode, "2")) { + get_instance()->_mxs_op_mode.set(2); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "alt") || !strcmp(opmode, "3")) { + get_instance()->_mxs_op_mode.set(3); + return get_instance()->_mxs_op_mode.commit(); + + } else { + print_usage("Invalid Op Mode"); + return PX4_ERROR; + } + } + + if (!strcmp(verb, "squawk")) { + const char *squawk = argv[1]; + int sqk = 0; + + if (squawk == nullptr) { + print_usage("Missing Squawk Code"); + return PX4_ERROR; + } + + sqk = atoi(squawk); + + if (!get_instance()->check_valid_squawk(sqk)) { + print_usage("Invalid Squawk"); + return PX4_ERROR; + + } else { + get_instance()->_adsb_squawk.set(sqk); + return get_instance()->_adsb_squawk.commit(); + } + } + + + print_usage("Unknown Command"); + return PX4_ERROR; +} + +int SagetechMXS::print_usage(const char *reason) +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + + This driver integrates the Sagetech MXS Certified Transponder to send and receive ADSB messages and traffic. + + ### Examples + + Attempt to start driver on a specified serial device. + $ sagetech_mxs start -d /dev/ttyS1 + Stop driver + $ sagetech_mxs stop + Set Flight ID (8 char max) + $ sagetech_mxs flight_id MXS12345 + Set MXS Operating Mode + $ sagetech_mxs opmode off/on/stby/alt + $ sagetech_mxs opmode 0/1/2/3 + Set the Squawk Code + $ sagetech_mxs squawk 1200 + )DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sagetech_mxs", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("transponder"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("flightid", "Set Flight ID (8 char max)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("ident", "Set the IDENT bit in ADSB-Out messages"); + PRINT_MODULE_USAGE_COMMAND_DESCR("opmode", + "Set the MXS operating mode. ('off', 'on', 'stby', 'alt', or numerical [0-3])"); + PRINT_MODULE_USAGE_COMMAND_DESCR("squawk", "Set the Squawk Code. [0-7777] Octal (no digit larger than 7)"); + return PX4_OK; +} + +int SagetechMXS::print_status() +{ + perf_print_counter(_loop_count_perf); + perf_print_counter(_loop_elapsed_perf); + perf_print_counter(_loop_interval_perf); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + return 0; +} + + + + +/****************************** + * Main Run Thread + * ****************************/ + +void SagetechMXS::Run() +{ + // Thread Stop + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_elapsed_perf); + perf_count(_loop_interval_perf); + + // Count the number of times the loop has executed + perf_count(_loop_count_perf); + _loop_count = perf_event_count(_loop_count_perf); + + /************************* + * 50 Hz Timer + * ***********************/ + open_serial_port(); + + // Initialization + if (!mxs_state.initialized) { + if ((_loop_count > MXS_INIT_TIMEOUT_COUNT) && !mxs_state.init_failed) { // Initialization timeout + PX4_ERR("Timeout: Failed to Initialize."); + mxs_state.init_failed = true; + } + + if (!_mxs_ext_cfg.get()) { + // Auto configuration + auto_config_operating(); + auto_config_installation(); + auto_config_flightid(); + _mxs_op_mode.set(sg_op_mode_t::modeStby); + _mxs_op_mode.commit(); + send_targetreq_msg(); + mxs_state.initialized = true; + } + } + + // Parse Bytes + int bytes_available {}; + int ret = -1; + ret = ioctl(_fd, FIONREAD, (unsigned long)&bytes_available); + (void) ret; + + // PX4_INFO("Bytes in buffer: %d. Ret code: %d", bytes_available, ret); + if (bytes_available > 0) { + while (bytes_available > 0) { + uint16_t data = 0; + // tcflush(_fd, TCOFLUSH); + ret = read(_fd, &data, 1); + + if (ret < 0) { + PX4_ERR("Read Err."); + perf_count(_comms_errors); + continue; + } + + // PX4_INFO("GOT BYTE: %02x", (uint8_t)data); + parse_byte((uint8_t)data); + bytes_available -= 1; + } + } + + /****************** + * 5 Hz Timer + * ****************/ + if (!(_loop_count % FIVE_HZ_MOD)) { // 5Hz Timer (GPS Flight) + // PX4_INFO("5 Hz callback"); + + // Check if vehicle is landed or in air + if (_vehicle_land_detected_sub.updated()) { + _vehicle_land_detected_sub.copy(&_landed); + } + + // Check GPS for updates at 5Hz + if (_sensor_gps_sub.updated()) { + _sensor_gps_sub.copy(&_gps); + } + + // If Vehicle is in air send GPS messages at 5Hz + if (!_landed.landed) { + send_gps_msg(); + } + + } + + /*********************** + * 1 Hz Timer + * *********************/ + + if (!(_loop_count % ONE_HZ_MOD)) { // 1Hz Timer (Operating Message/GPS Ground) + // PX4_INFO("1 Hz callback"); + + if (!mxs_state.initialized && _mxs_ext_cfg.get()) { + // External Configuration + send_data_req(dataInstall); + } + + // If Vehicle is grounded send GPS message at 1 Hz + if (_landed.landed) { + send_gps_msg(); + } + + // Send 1Hz Operating Message + send_operating_msg(); + + // Update Parameters + if (_parameter_update_sub.updated()) { + parameter_update_s param_update{}; + _parameter_update_sub.copy(¶m_update); + ModuleParams::updateParams(); + + if (mxs_state.treq.transmitPort != (sg_transmitport_t)_mxs_targ_port.get()) { + send_targetreq_msg(); + } + } + } + + /************************ + * 8.2 Second Timer + * **********************/ + + if (!(_loop_count % EIGHT_TWO_SEC_MOD)) { // 8.2 second timer (Flight ID) + // PX4_INFO("8.2 second callback"); + send_flight_id_msg(); + } + + perf_end(_loop_elapsed_perf); +} + + +/*************************** + * ADSB Vehicle List Functions +****************************/ + +bool SagetechMXS::get_vehicle_by_ICAO(const uint32_t icao, transponder_report_s &vehicle) const +{ + transponder_report_s temp_vehicle{}; + temp_vehicle.icao_address = icao; + + uint16_t i = 0; + + if (find_index(temp_vehicle, &i)) { + memcpy(&vehicle, &vehicle_list[i], sizeof(transponder_report_s)); + return true; + + } else { + return false; + } +} + +bool SagetechMXS::find_index(const transponder_report_s &vehicle, uint16_t *index) const +{ + for (uint16_t i = 0; i < vehicle_count; i++) { + if (vehicle_list[i].icao_address == vehicle.icao_address) { + *index = i; + return true; + } + } + + return false; +} + +void SagetechMXS::set_vehicle(const uint16_t index, const transponder_report_s &vehicle) +{ + if (index >= list_size_allocated) { + return; // out of range + } + + vehicle_list[index] = vehicle; +} + +void SagetechMXS::determine_furthest_aircraft() +{ + float max_distance = 0; + uint16_t max_distance_index = 0; + + for (uint16_t index = 0; index < vehicle_count; index++) { + if (is_special_vehicle(vehicle_list[index].icao_address)) { + continue; + } + + const float distance = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle_list[index].lat, + vehicle_list[index].lon); + + if ((max_distance < distance) || (index == 0)) { + max_distance = distance; + max_distance_index = index; + } + } + + furthest_vehicle_index = max_distance_index; + furthest_vehicle_distance = max_distance; +} + +void SagetechMXS::delete_vehicle(const uint16_t index) +{ + if (index >= vehicle_count) { + return; + } + + if (index == furthest_vehicle_index && furthest_vehicle_distance > 0) { + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + } + + if (index != vehicle_count - 1) { + vehicle_list[index] = vehicle_list[vehicle_count - 1]; + } + + vehicle_count--; +} + +void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle) +{ + // needs to handle updating the vehicle list, keeping track of which vehicles to drop + // and which to keep, allocating new vehicles, and publishing to the transponder_report topic + uint16_t index = list_size_allocated + 1; // Make invalid to start with. + const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0); + const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle.lat, vehicle.lon); + const bool is_tracked_in_list = find_index(vehicle, &index); + // const bool is_special = is_special_vehicle(vehicle.icao_address); + const uint16_t required_flags_position = transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | + transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + + if (!(vehicle.flags & required_flags_position)) { + if (is_tracked_in_list) { + delete_vehicle(index); // If the vehicle is tracked in our list but doesn't have the right flags remove it + } + + return; + + } else if (is_tracked_in_list) { // If the vehicle is in the list update it with the index found + set_vehicle(index, vehicle); + + } else if (vehicle_count < + list_size_allocated) { // If the vehicle is not in the list, and the vehicle count is less than the max count + // then add it to the vehicle_count index (after the last vehicle) and increment vehicle_count + set_vehicle(vehicle_count, vehicle); + vehicle_count++; + + } else { // Buffer is full. If new vehicle is closer, replace furthest with new vehicle + if (my_loc_is_zero) { // Invalid GPS + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + + } else { + if (furthest_vehicle_distance <= 0) { + determine_furthest_aircraft(); + } + + if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { + set_vehicle(furthest_vehicle_index, vehicle); + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + } + } + } + + const uint16_t required_flags_avoidance = required_flags_position | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | + transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + + if (vehicle.flags & required_flags_avoidance) { + _transponder_report_pub.publish(vehicle); + } +} + +/************************************* + * Handlers for Received Messages + * ***********************************/ + +void SagetechMXS::handle_ack(const sg_ack_t ack) +{ + if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) { + // The message id doesn't match the last message sent. + PX4_ERR("Message Id %d of type %d not Acked by MXS", last.msg.id, last.msg.type); + } + + // System health + if (ack.failXpdr && !last.failXpdr) { + mavlink_log_info(&_mavlink_log_pub, "Transponder Failure\t"); + events::send(events::ID("mxs_xpdr_fail"), events::Log::Critical, "Transponder Failure"); + } + + if (ack.failSystem && !last.failSystem) { + mavlink_log_info(&_mavlink_log_pub, "Transponder System Failure\t"); + events::send(events::ID("mxs_system_fail"), events::Log::Critical, "Transponder System Failure"); + } + + last.failXpdr = ack.failXpdr; + last.failSystem = ack.failSystem; +} + +void SagetechMXS::handle_svr(sg_svr_t svr) +{ + if (svr.addrType != svrAdrIcaoUnknown && svr.addrType != svrAdrIcao && svr.addrType != svrAdrIcaoSurface) { + return; // invalid icao + } + + transponder_report_s t{}; + + // Check if vehicle already exist in buffer + if (!get_vehicle_by_ICAO(svr.addr, t)) { + memset(&t, 0, sizeof(t)); + t.icao_address = svr.addr; + } + + t.timestamp = hrt_absolute_time(); + t.flags &= ~transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; + + //Set data from svr message + if (svr.validity.position) { + t.lat = (double) svr.lat; + t.lon = (double) svr.lon; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + } + + if (svr.validity.geoAlt) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + t.altitude = (svr.airborne.geoAlt * SAGETECH_SCALE_FEET_TO_M); //Convert from Feet to Meters + + } else if (svr.validity.baroAlt) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + t.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; + t.altitude = (svr.airborne.baroAlt * SAGETECH_SCALE_FEET_TO_M); //Convert from Feet to Meters + } + + if (svr.validity.baroVRate || svr.validity.geoVRate) { + t.ver_velocity = svr.airborne.vrate * SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC; //Convert from feet/min to meters/second + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + if (svr.type == svrSurface) { + if (svr.validity.surfSpeed) { + t.hor_velocity = svr.surface.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + if (svr.validity.surfHeading) { + t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f) + M_PI_F); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + } + } + + if (svr.type == svrAirborne) { + if (svr.validity.airSpeed) { + t.hor_velocity = (svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC); //Convert from knots to meters/second + t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f) + M_PI_F); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + } + + // PX4_INFO("SVR ICAO %x, SVR Air Heading %f", (int) t.icao_address, (double) t.heading); + handle_vehicle(t); +} + +void SagetechMXS::handle_msr(sg_msr_t msr) +{ + + transponder_report_s t{}; + + if (!get_vehicle_by_ICAO(msr.addr, t)) { + // new vehicle creation isn't allowed here since position isn't provided + return; + } + + t.timestamp = hrt_absolute_time(); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; + + if (strlen(msr.callsign)) { + snprintf(t.callsign, sizeof(t.callsign), "%-8s", msr.callsign); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; + + } else { + t.flags &= ~transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; + } + + // PX4_INFO("Got MSR for ICAO: %x with callsign: %s", (int) t.icao_address, t.callsign); + + handle_vehicle(t); +} + +/************************************** + * Message Sending Functions + **************************************/ + +int SagetechMXS::msg_write(const uint8_t *data, const uint16_t len) const +{ + int ret = 0; + + if (_fd >= 0) { + ret = write(_fd, data, len); + + } else { + return PX4_ERROR; + } + + if (ret != len) { + perf_count(_comms_errors); + return PX4_ERROR; + } + + return PX4_OK; +} + +void SagetechMXS::send_data_req(const sg_datatype_t dataReqType) +{ + sg_datareq_t dataReq {}; + dataReq.reqType = dataReqType; + last.msg.type = SG_MSG_TYPE_HOST_DATAREQ; + uint8_t txComBuffer[SG_MSG_LEN_DATAREQ] {}; + sgEncodeDataReq(txComBuffer, &dataReq, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_DATAREQ); +} + +void SagetechMXS::send_install_msg() +{ + // MXS must be in OFF mode to change ICAO or Registration + if (mxs_state.op.opMode != modeOff) { + // gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: unable to send installation data while not in OFF mode."); + return; + } + + mxs_state.inst.icao = _adsb_icao.get(); + mxs_state.inst.emitter = convert_emitter_type_to_sg(_adsb_emit_type.get()); + mxs_state.inst.size = (sg_size_t)_adsb_len_width.get(); + mxs_state.inst.maxSpeed = (sg_airspeed_t)_adsb_max_speed.get(); + mxs_state.inst.antenna = sg_antenna_t::antBottom; + + last.msg.type = SG_MSG_TYPE_HOST_INSTALL; + uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; + sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_INSTALL); +} + +void SagetechMXS::send_flight_id_msg() +{ + last.msg.type = SG_MSG_TYPE_HOST_FLIGHT; + uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; + sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); +} + +void SagetechMXS::send_operating_msg() +{ + + mxs_state.op.opMode = (sg_op_mode_t)_mxs_op_mode.get(); + + if (check_valid_squawk(_adsb_squawk.get())) { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get()); + + } else { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, INVALID_SQUAWK); + } + + mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile + mxs_state.op.enableSqt = true; // Enable extended squitters + mxs_state.op.enableXBit = false; // Enable the x-bit + mxs_state.op.milEmergency = false; // Broadcast a military emergency + mxs_state.op.emergcType = sg_emergc_t::emergcNone; // Enumerated civilian emergency type + + mxs_state.op.altUseIntrnl = + true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field) + mxs_state.op.altHostAvlbl = false; + mxs_state.op.altRes25 = + !mxs_state.inst.altRes100; // Host Altitude Resolution from install + + mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + + mxs_state.op.identOn = _adsb_ident.get(); + + if (mxs_state.op.identOn) { + _adsb_ident.set(0); + _adsb_ident.commit(); + } + + if (_gps.vel_ned_valid) { + mxs_state.op.climbValid = true; + mxs_state.op.climbRate = _gps.vel_d_m_s * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; + mxs_state.op.airspdValid = true; + mxs_state.op.headingValid = true; + + } else { + // PX4_WARN("send_operating_msg: Invalid NED"); + mxs_state.op.climbValid = false; + mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; + mxs_state.op.airspdValid = false; + mxs_state.op.headingValid = false; + } + + const uint16_t speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + double heading = (double) math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + mxs_state.op.airspd = speed_knots; + mxs_state.op.heading = heading; + + last.msg.type = SG_MSG_TYPE_HOST_OPMSG; + uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; + sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_OPMSG); +} + + +void SagetechMXS::send_gps_msg() +{ + sg_gps_t gps {}; + + gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown + gps.hfom = _gps.eph >= 0 ? _gps.eph : 0; + gps.vfom = _gps.epv >= 0 ? _gps.epv : 0; + gps.nacv = sg_nacv_t::nacvUnknown; + + if (_gps.s_variance_m_s >= (float)10.0 || _gps.s_variance_m_s < 0) { + gps.nacv = sg_nacv_t::nacvUnknown; + + } else if (_gps.s_variance_m_s >= (float)3.0) { + gps.nacv = sg_nacv_t::nacv10dot0; + + } else if (_gps.s_variance_m_s >= (float)1.0) { + gps.nacv = sg_nacv_t::nacv3dot0; + + } else if (_gps.s_variance_m_s >= (float)0.3) { + gps.nacv = sg_nacv_t::nacv1dot0; + + } else { //if (_gps.s_variance_m_s >= 0.0) + gps.nacv = sg_nacv_t::nacv0dot3; + } + + // Get Vehicle Longitude and Latitude and Convert to string + const int32_t longitude = _gps.lon; + const int32_t latitude = _gps.lat; + const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1); + const double lon_minutes = (lon_deg - int(lon_deg)) * 60; + snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, + unsigned((lon_minutes - (int)lon_minutes) * 1.0E5)); + + const double lat_deg = latitude * 1.0E-7 * (latitude < 0 ? -1 : 1); + const double lat_minutes = (lat_deg - int(lat_deg)) * 60; + snprintf((char *)&gps.latitude, 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, + unsigned((lat_minutes - (int)lat_minutes) * 1.0E5)); + + const float speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + snprintf((char *)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, + unsigned((speed_knots - (int)speed_knots) * (float)1.0E2)); + + // const float heading = math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + const float heading = matrix::wrap_2pi(_gps.cog_rad) * (180.0f / M_PI_F); + // PX4_INFO("CoG: %f. Heading: %f", (double) _gps.cog_rad, (double) _gps.heading); + // PX4_INFO("Heading: %f", (double)heading); + + snprintf((char *)&gps.grdTrack, 9, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * (float)1.0E4)); + + gps.latNorth = latitude >= 0; + gps.lngEast = longitude >= 0; + + gps.gpsValid = !(_gps.fix_type < 2); // If the status is not OK, gpsValid is false. + + const time_t time_sec = _gps.time_utc_usec * 1E-6; + struct tm *tm = gmtime(&time_sec); + snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, + tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6); + // PX4_INFO("send_gps_msg: ToF %s, Longitude %s, Latitude %s, Grd Speed %s, Grd Track %s", gps.timeOfFix, gps.longitude, gps.latitude, gps.grdSpeed, gps.grdTrack); + + gps.height = _gps.alt_ellipsoid * 1E-3; + + // checkGPSInputs(&gps); + last.msg.type = SG_MSG_TYPE_HOST_GPS; + uint8_t txComBuffer[SG_MSG_LEN_GPS] {}; + sgEncodeGPS(txComBuffer, &gps, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_GPS); +} + + + +void SagetechMXS::send_targetreq_msg() +{ + mxs_state.treq.reqType = sg_reporttype_t::reportAuto; + mxs_state.treq.transmitPort = (sg_transmitport_t)_mxs_targ_port.get(); + mxs_state.treq.maxTargets = list_size_allocated; + mxs_state.treq.icao = _adsb_icao_specl.get(); + mxs_state.treq.stateVector = true; + mxs_state.treq.modeStatus = true; + mxs_state.treq.targetState = false; + mxs_state.treq.airRefVel = false; + mxs_state.treq.tisb = false; + mxs_state.treq.military = false; + mxs_state.treq.commA = false; + mxs_state.treq.ownship = true; + + last.msg.type = SG_MSG_TYPE_HOST_TARGETREQ; + uint8_t txComBuffer[SG_MSG_LEN_TARGETREQ] {}; + sgEncodeTargetReq(txComBuffer, &mxs_state.treq, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_TARGETREQ); +} + +/*********************************************** + * Byte Parsing and Packet Handling + * *********************************************/ + +void SagetechMXS::handle_packet(const Packet &msg) +{ + switch (msg.type) { + case MsgType::ACK: + if (sgDecodeAck((uint8_t *) &msg, &mxs_state.ack)) { + handle_ack(mxs_state.ack); + } + + break; + + case MsgType::Installation_Response: + if (!mxs_state.initialized && sgDecodeInstall((uint8_t *)&msg, &mxs_state.inst)) { + store_inst_resp(); + mxs_state.initialized = true; + } + + break; + + case MsgType::FlightID_Response: { + sg_flightid_t fid{}; + + if (sgDecodeFlightId((uint8_t *) &msg, &fid)) { + } + + break; + } + + case MsgType::ADSB_StateVector_Report: { + sg_svr_t svr{}; + + if (sgDecodeSVR((uint8_t *) &msg, &svr)) { + handle_svr(svr); + } + + break; + } + + case MsgType::ADSB_ModeStatus_Report: { + sg_msr_t msr{}; + + if (sgDecodeMSR((uint8_t *) &msg, &msr)) { + handle_msr(msr); + } + + break; + } + + case MsgType::FlightID: + case MsgType::Installation: + case MsgType::Operating: + case MsgType::GPS_Data: + case MsgType::Data_Request: + case MsgType::Target_Request: + case MsgType::Mode: + case MsgType::Status_Response: + case MsgType::RESERVED_0x84: + case MsgType::RESERVED_0x85: + case MsgType::Mode_Settings: + case MsgType::RESERVED_0x8D: + case MsgType::Version_Response: + case MsgType::Serial_Number_Response: + case MsgType::Target_Summary_Report: + case MsgType::ADSB_Target_State_Report: + case MsgType::ADSB_Air_Ref_Vel_Report: + // Not handling the rest of these. + break; + } +} + +bool SagetechMXS::parse_byte(const uint8_t data) +{ + switch (_message_in.state) { + default: + case ParseState::WaitingFor_Start: + if (data == SG_MSG_START_BYTE) { + _message_in.checksum = data; // initialize checksum here + _message_in.state = ParseState::WaitingFor_MsgType; + } + + break; + + case ParseState::WaitingFor_MsgType: + _message_in.checksum += data; + _message_in.packet.type = static_cast(data); + _message_in.state = ParseState::WaitingFor_MsgId; + break; + + case ParseState::WaitingFor_MsgId: + _message_in.checksum += data; + _message_in.packet.id = data; + _message_in.state = ParseState::WaitingFor_PayloadLen; + break; + + case ParseState::WaitingFor_PayloadLen: + _message_in.checksum += data; + _message_in.packet.payload_length = data; + // PX4_INFO("Packet Payload Length: %d", _message_in.packet.payload_length); + // maybe useful to add a length check here. Very few errors are due to length though... + _message_in.index = 0; + _message_in.state = (data == 0) ? ParseState::WaitingFor_Checksum : ParseState::WaitingFor_PayloadContents; + break; + + case ParseState::WaitingFor_PayloadContents: + _message_in.checksum += data; + _message_in.packet.payload[_message_in.index++] = data; + + if (_message_in.index >= + _message_in.packet.payload_length) { // Note: yeah, this is right since it will increment index after writing the last byte and it will equal the payload length + _message_in.state = ParseState::WaitingFor_Checksum; + } + + break; + + case ParseState::WaitingFor_Checksum: + // PX4_INFO("Payload Bytes Got: %d", _message_in.index); + _message_in.state = ParseState::WaitingFor_Start; + + if (_message_in.checksum == data) { + // append the checksum to the payload and zero out the payload index + _message_in.packet.payload[_message_in.index] = data; + handle_packet(_message_in.packet); + + } else if (data == SG_MSG_START_BYTE) { + PX4_INFO("ERROR: Byte Lost. Catching new packet."); + _message_in.state = ParseState::WaitingFor_MsgType; + _message_in.checksum = data; + + } else { + PX4_INFO("ERROR: Checksum Mismatch. Expected %02x. Received %02x.", _message_in.checksum, data); + } + + break; + } + + return false; +} + +/****************************** + * Helper Functions + * ***************************/ + +uint32_t SagetechMXS::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber) +{ + // Our only sensible input bases are 16 and 8 + if (baseIn != BASE_OCTAL && baseIn != BASE_HEX) { + return inputNumber; + } + + uint32_t outputNumber = 0; + + for (uint8_t i = 0; i < BASE_DEC; i++) { + outputNumber += (inputNumber % BASE_DEC) * powf(baseIn, i); + inputNumber /= BASE_DEC; + + if (inputNumber == 0) { break; } + } + + return outputNumber; +} + +int SagetechMXS::open_serial_port() +{ + + if (_fd < 0) { // Open port if not open + _fd = open(_port, (O_RDWR | O_NOCTTY | O_NONBLOCK)); + + if (_fd < 0) { + PX4_ERR("Opening port %s failed %i", _port, errno); + return PX4_ERROR; + } + + } else { + return PX4_OK; + } + + // UART Configuration + termios uart_config {}; + int termios_state = -1; + + if (tcgetattr(_fd, &uart_config)) { + PX4_ERR("Unable to get UART Configuration"); + close(_fd); + _fd = -1; + return PX4_ERROR; + } + + /***************************** + * UART Control Options + * ***************************/ + + // Enable Receiver and Set Local Mode + uart_config.c_cflag |= (CREAD | CLOCAL); + + // Send 8N1 No Parity + uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB); + uart_config.c_cflag |= CS8; + + // Disable hardware flow control + uart_config.c_cflag &= ~CRTSCTS; + + // Set Baud Rate + unsigned baud = convert_to_px4_baud(_ser_mxs_baud.get()); + + if ((cfsetispeed(&uart_config, baud) < 0) || (cfsetospeed(&uart_config, baud) < 0)) { + PX4_ERR("ERR SET BAUD %s: %d\n", _port, termios_state); + close(_fd); + return PX4_ERROR; + } + + /***************************** + * UART Local Options + * ***************************/ + + // Set Raw Input + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN); + + /***************************** + * UART Output Options + * ***************************/ + + // Set raw output and map NL to CR-LF + uart_config.c_oflag &= ~ONLCR; + + // Flush the buffer + tcflush(_fd, TCIOFLUSH); + + + /********************************* + * Apply Modified Port Attributes + * *******************************/ + if (tcsetattr(_fd, TCSANOW, &uart_config) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + return PX4_ERROR; + } + + PX4_INFO("Opened port %s", _port); + return PX4_OK; +} + +unsigned SagetechMXS::convert_to_px4_baud(int baudType) +{ + switch (baudType) { + case 0: return B38400; + + case 1: return B600; + + case 2: return B4800; + + case 3: return B9600; + + // case 4: return B0; + + case 5: return B57600; + + case 6: return B115200; + + case 7: return B230400; + + case 8: return B19200; + + case 9: return BAUD_460800; + + case 10: return BAUD_921600; + + default: return B57600; + } +} + +sg_emitter_t SagetechMXS::convert_emitter_type_to_sg(int emitType) +{ + switch (emitType) { + case 0: return sg_emitter_t::aUnknown; + + case 1: return sg_emitter_t::aLight; + + case 2: return sg_emitter_t::aSmall; + + case 3: return sg_emitter_t::aLarge; + + case 4: return sg_emitter_t::aHighVortex; + + case 5: return sg_emitter_t::aHeavy; + + case 6: return sg_emitter_t::aPerformance; + + case 7: return sg_emitter_t::aRotorCraft; + + case 8: return sg_emitter_t::bUnknown; + + case 9: return sg_emitter_t::bGlider; + + case 10: return sg_emitter_t::bAir; + + case 11: return sg_emitter_t::bParachutist; + + case 12: return sg_emitter_t::bUltralight; + + case 13: return sg_emitter_t::bUnknown; + + case 14: return sg_emitter_t::bUAV; + + case 15: return sg_emitter_t::bSpace; + + case 16: return sg_emitter_t::cUnknown; + + case 17: return sg_emitter_t::cEmergency; + + case 18: return sg_emitter_t::cService; + + case 19: return sg_emitter_t::cPoint; + + default: return sg_emitter_t::dUnknown; + } +} + +int SagetechMXS::convert_sg_to_emitter_type(sg_emitter_t sg_emit) +{ + switch (sg_emit) { + case sg_emitter_t::aUnknown: return 0; + + case sg_emitter_t::aLight: return 1; + + case sg_emitter_t::aSmall: return 2; + + case sg_emitter_t::aLarge: return 3; + + case sg_emitter_t::aHighVortex: return 4; + + case sg_emitter_t::aHeavy: return 5; + + case sg_emitter_t::aPerformance: return 6; + + case sg_emitter_t::aRotorCraft: return 7; + + case sg_emitter_t::bUnknown: return 8; + + case sg_emitter_t::bGlider: return 9; + + case sg_emitter_t::bAir: return 10; + + case sg_emitter_t::bParachutist: return 11; + + case sg_emitter_t::bUltralight: return 12; + + case sg_emitter_t::bUAV: return 14; + + case sg_emitter_t::bSpace: return 15; + + case sg_emitter_t::cUnknown: return 16; + + case sg_emitter_t::cEmergency: return 17; + + case sg_emitter_t::cService: return 18; + + case sg_emitter_t::cPoint: return 19; + + default: return 20; + } +} + +int SagetechMXS::handle_fid(const char *fid) +{ + if (snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", fid) != 8) { + PX4_ERR("Failed to write Flight ID"); + return PX4_ERROR; + } + + PX4_INFO("Changed Flight ID to %s", mxs_state.fid.flightId); + return PX4_OK; +} + +int SagetechMXS::store_inst_resp() +{ + _mxs_op_mode.set(mxs_state.ack.opMode); + _mxs_op_mode.commit(); + _adsb_icao.set(mxs_state.inst.icao); + _adsb_icao.commit(); + _adsb_len_width.set(mxs_state.inst.size); + _adsb_len_width.commit(); + _adsb_emit_type.set(convert_sg_to_emitter_type(mxs_state.inst.emitter)); + _adsb_emit_type.commit(); + return PX4_OK; +} + + +void SagetechMXS::auto_config_operating() +{ + mxs_state.op.opMode = sg_op_mode_t::modeOff; + _mxs_op_mode.set(sg_op_mode_t::modeOff); + _mxs_op_mode.commit(); + + if (check_valid_squawk(_adsb_squawk.get())) { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get()); + + } else { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, INVALID_SQUAWK); + } + + mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile + mxs_state.op.enableSqt = true; // Enable extended squitters + mxs_state.op.enableXBit = false; // Enable the x-bit + mxs_state.op.milEmergency = false; // Broadcast a military emergency + mxs_state.op.emergcType = (sg_emergc_t) + _adsb_emergc.get(); // Enumerated civilian emergency type + + mxs_state.op.altUseIntrnl = + true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field) + mxs_state.op.altHostAvlbl = false; + mxs_state.op.altRes25 = true; // Host Altitude Resolution from install + + mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + + mxs_state.op.identOn = false; + + if (_gps.vel_ned_valid) { + mxs_state.op.climbValid = true; + mxs_state.op.climbRate = _gps.vel_d_m_s * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; + mxs_state.op.airspdValid = true; + mxs_state.op.headingValid = true; + + } else { + // PX4_WARN("send_operating_msg: Invalid NED"); + mxs_state.op.climbValid = false; + mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; + mxs_state.op.airspdValid = false; + mxs_state.op.headingValid = false; + } + + const uint16_t speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + double heading = (double) math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + mxs_state.op.airspd = speed_knots; + mxs_state.op.heading = heading; + + last.msg.type = SG_MSG_TYPE_HOST_OPMSG; + uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; + sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_OPMSG); +} + +void SagetechMXS::auto_config_installation() +{ + if (mxs_state.ack.opMode != modeOff) { + PX4_ERR("MXS not put in OFF Mode before installation."); + return; + } + + mxs_state.inst.icao = (uint32_t) _adsb_icao.get(); + snprintf(mxs_state.inst.reg, 8, "%-7s", "PX4TEST"); + + mxs_state.inst.com0 = sg_baud_t::baud230400; + mxs_state.inst.com1 = sg_baud_t::baud57600; + + mxs_state.inst.eth.ipAddress = 0; + mxs_state.inst.eth.subnetMask = 0; + mxs_state.inst.eth.portNumber = 0; + + mxs_state.inst.sil = sg_sil_t::silUnknown; + mxs_state.inst.sda = sg_sda_t::sdaUnknown; + mxs_state.inst.emitter = convert_emitter_type_to_sg(_adsb_emit_type.get()); + mxs_state.inst.size = (sg_size_t)_adsb_len_width.get(); + mxs_state.inst.maxSpeed = (sg_airspeed_t) _adsb_max_speed.get(); + mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0. + mxs_state.inst.antenna = sg_antenna_t::antBottom; + + mxs_state.inst.altRes100 = true; + mxs_state.inst.hdgTrueNorth = false; + mxs_state.inst.airspeedTrue = false; + mxs_state.inst.heater = true; // Heater should always be connected. + mxs_state.inst.wowConnected = true; + + last.msg.type = SG_MSG_TYPE_HOST_INSTALL; + + uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; + sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_INSTALL); +} + +void SagetechMXS::auto_config_flightid() +{ + snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", mxs_state.inst.reg); + + last.msg.type = SG_MSG_TYPE_HOST_FLIGHT; + + uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; + sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); +} + +bool SagetechMXS::check_valid_squawk(int squawk) +{ + if (squawk > INVALID_SQUAWK) { + return false; + } + + for (int i = 4; i > 0; i--) { + squawk = squawk - ((int)(squawk / powf(10, i)) * (int)powf(10, i)); + + if ((int)(squawk / powf(10, i - 1)) > 7) { + return false; + } + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp new file mode 100644 index 0000000000..0c19d33f62 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sg_sdk/sagetech_mxs.h" + +using namespace time_literals; + +class SagetechMXS : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SagetechMXS(const char *port); + ~SagetechMXS() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void print_info(); + + bool init(); + + int print_status() override; + +private: + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _adsb_squawk, + (ParamInt) _adsb_ident, + (ParamInt) _adsb_list_max, + (ParamInt) _adsb_icao, + (ParamInt) _adsb_len_width, + (ParamInt) _adsb_emit_type, + (ParamInt) _adsb_max_speed, + (ParamInt) _adsb_icao_specl, + (ParamInt) _adsb_emergc, + (ParamInt) _mxs_op_mode, + (ParamInt) _mxs_targ_port, + // (ParamInt) _mxs_com0_baud, + // (ParamInt) _mxs_com1_baud, + (ParamInt) _mxs_ext_cfg, + (ParamInt) _ser_mxs_baud + ); + + // Serial Port Variables + char _port[20] {}; + int _fd{-1}; + + // Publications + uORB::Publication _transponder_report_pub{ORB_ID(transponder_report)}; + orb_advert_t _mavlink_log_pub{nullptr}; + + + // Subscriptions + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data + uORB::Subscription _transponder_report_sub{ORB_ID(transponder_report)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + + + // Performance (perf) counters + perf_counter_t _loop_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": run_loop")}; + perf_counter_t _loop_elapsed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + + // Constants + static constexpr uint32_t UPDATE_INTERVAL_US{1000000 / 50}; // 20ms = 50 Hz + static constexpr uint8_t FIVE_HZ_MOD{10}; // 0.2s = 5 Hz + static constexpr uint8_t TWO_HZ_MOD{25}; // 0.5s = 2 Hz + static constexpr uint8_t ONE_HZ_MOD{50}; // 1 Hz + static constexpr uint16_t EIGHT_TWO_SEC_MOD{410}; // 8.2 seconds + // static constexpr uint8_t SG_MSG_START_BYTE{0xAA}; + static constexpr uint32_t PAYLOAD_MXS_MAX_SIZE{255}; + static constexpr float SAGETECH_SCALE_FEET_TO_M{0.3048F}; + static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F}; + static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F}; + static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F}; + static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F}; + static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F}; + static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0}; + static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1}; + static constexpr uint16_t MAX_VEHICLES_LIMIT{50}; + static constexpr float SAGETECH_HPL_UNKNOWN{38000.0F}; + static constexpr float CLIMB_RATE_LIMIT{16448}; + static constexpr uint16_t MXS_INIT_TIMEOUT_COUNT{1000}; // 1000 loop cycles = 20 seconds + static constexpr uint8_t BASE_OCTAL{8}; + static constexpr uint8_t BASE_HEX{16}; + static constexpr uint8_t BASE_DEC{10}; + static constexpr uint16_t INVALID_SQUAWK{7777}; + static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios + static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios + + // Stored variables + uint64_t _loop_count; + + // Cached Subscription Data + sensor_gps_s _gps; + vehicle_land_detected_s _landed; + + enum class MsgType : uint8_t { + Installation = SG_MSG_TYPE_HOST_INSTALL, + FlightID = SG_MSG_TYPE_HOST_FLIGHT, + Operating = SG_MSG_TYPE_HOST_OPMSG, + GPS_Data = SG_MSG_TYPE_HOST_GPS, + Data_Request = SG_MSG_TYPE_HOST_DATAREQ, + // RESERVED 0x06 - 0x0A + Target_Request = SG_MSG_TYPE_HOST_TARGETREQ, + Mode = SG_MSG_TYPE_HOST_MODE, + // RESERVED 0x0D - 0xC1 + ACK = SG_MSG_TYPE_XPNDR_ACK, + Installation_Response = SG_MSG_TYPE_XPNDR_INSTALL, + FlightID_Response = SG_MSG_TYPE_XPNDR_FLIGHT, + Status_Response = SG_MSG_TYPE_XPNDR_STATUS, + RESERVED_0x84 = 0x84, + RESERVED_0x85 = 0x85, + Mode_Settings = SG_MSG_TYPE_XPNDR_MODE, + RESERVED_0x8D = 0x8D, + Version_Response = SG_MSG_TYPE_XPNDR_VERSION, + Serial_Number_Response = SG_MSG_TYPE_XPNDR_SERIALNUM, + Target_Summary_Report = SG_MSG_TYPE_ADSB_TSUMMARY, + + ADSB_StateVector_Report = SG_MSG_TYPE_ADSB_SVR, + ADSB_ModeStatus_Report = SG_MSG_TYPE_ADSB_MSR, + ADSB_Target_State_Report = SG_MSG_TYPE_ADSB_TSTATE, + ADSB_Air_Ref_Vel_Report = SG_MSG_TYPE_ADSB_ARVR, + }; + + enum class ParseState { + WaitingFor_Start, + WaitingFor_MsgType, + WaitingFor_MsgId, + WaitingFor_PayloadLen, + WaitingFor_PayloadContents, + WaitingFor_Checksum, + }; + + struct __attribute__((packed)) Packet { + const uint8_t start = SG_MSG_START_BYTE; + MsgType type; + uint8_t id; + uint8_t payload_length; + uint8_t payload[PAYLOAD_MXS_MAX_SIZE]; + }; + + struct { + ParseState state; + uint8_t index; + Packet packet; + uint8_t checksum; + } _message_in; + + struct { + bool initialized; + bool init_failed; + sg_operating_t op; + sg_install_t inst; + // sg_gps_t gps; + sg_targetreq_t treq; + sg_flightid_t fid; + sg_ack_t ack; + } mxs_state; + + struct { + // cached variables to compare against params so we can send msg on param change. + bool failXpdr; + bool failSystem; + struct { + uint8_t id; + uint8_t type; + } msg; + } last; + + // External Vehicle List + transponder_report_s *vehicle_list; + uint16_t list_size_allocated; + uint16_t vehicle_count = 0; + uint16_t furthest_vehicle_index = 0; + float furthest_vehicle_distance = 0; + + // Functions + void Run() override; + void handle_packet(const Packet &msg); + int msg_write(const uint8_t *data, const uint16_t len) const; + bool parse_byte(const uint8_t data); + void handle_ack(const sg_ack_t ack); + void handle_svr(const sg_svr_t svr); + void handle_msr(sg_msr_t msr); + bool get_vehicle_by_ICAO(const uint32_t icao, transponder_report_s &vehicle) const; + bool find_index(const transponder_report_s &vehicle, uint16_t *index) const; + void set_vehicle(const uint16_t index, const transponder_report_s &vehicle); + void delete_vehicle(const uint16_t index); + bool is_special_vehicle(uint32_t icao) const {return _adsb_icao_specl.get() != 0 && (_adsb_icao_specl.get() == (int32_t) icao);} + void handle_vehicle(const transponder_report_s &vehicle); + void determine_furthest_aircraft(); + void send_data_req(const sg_datatype_t dataReqType); + void send_install_msg(); + void send_flight_id_msg(); + void send_operating_msg(); + void send_gps_msg(); + void send_targetreq_msg(); + uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber); + int open_serial_port(); + sg_emitter_t convert_emitter_type_to_sg(int emitType); + int convert_sg_to_emitter_type(sg_emitter_t sg_emit); + int handle_fid(const char *fid); + int store_inst_resp(); + void auto_config_operating(); + void auto_config_installation(); + void auto_config_flightid(); + unsigned convert_to_px4_baud(int baudType); + bool check_valid_squawk(int squawk); +}; diff --git a/src/drivers/transponder/sagetech_mxs/module.yaml b/src/drivers/transponder/sagetech_mxs/module.yaml new file mode 100644 index 0000000000..7af6176bca --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/module.yaml @@ -0,0 +1,8 @@ +module_name: Sagetech MXS +serial_config: + - command: sagetech_mxs start -d ${SERIAL_DEV} + port_config_param: + name: MXS_SER_CFG + group: Transponder + default: TEL2 + label: Sagetech MXS Serial Port diff --git a/src/drivers/transponder/sagetech_mxs/parameters.c b/src/drivers/transponder/sagetech_mxs/parameters.c new file mode 100644 index 0000000000..0d31d1a445 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/parameters.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * parameters.c + * + * Sagetech MXS transponder custom parameters + * @author Megan McCormick megan.mccormick@sagetech.com + * @author Check Faber chuck.faber@sagetech.com + */ + +/** + * ADSB-Out squawk code configuration + * + * This parameter defines the squawk code. Value should be between 0000 and 7777. + * + * @reboot_required false + * @min 0 + * @max 7777 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_SQUAWK, 1200); + +/** + * ADSB-Out Ident Configuration + * + * Enable Identification of Position feature + * + * @boolean + * @reboot_required false + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_IDENT, 0); + +/** + * ADSB-In Vehicle List Size + * + * Change number of targets to track + * + * @min 0 + * @max 50 + * @reboot_required true + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_LIST_MAX, 25); + +/** + * ADSB-Out ICAO configuration + * + * Defines the ICAO ID of the vehicle + * + * @reboot_required true + * @min -1 + * @max 16777215 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_ICAO_ID, 1194684); + +/** + * ADSB-Out Vehicle Size Configuration + * + * Report the length and width of the vehicle in meters. In most cases, use '1' for the smallest vehicle size. + * + * @reboot_required true + * @min 0 + * @max 15 + * @group Transponder + * + * @value 0 SizeUnknown + * @value 1 Len15_Wid23 + * @value 2 Len25_Wid28 + * @value 3 Len25_Wid34 + * @value 4 Len35_Wid33 + * @value 5 Len35_Wid38 + * @value 6 Len45_Wid39 + * @value 7 Len45_Wid45 + * @value 8 Len55_Wid45 + * @value 9 Len55_Wid52 + * @value 10 Len65_Wid59 + * @value 11 Len65_Wid67 + * @value 12 Len75_Wid72 + * @value 13 Len75_Wid80 + * @value 14 Len85_Wid80 + * @value 15 Len85_Wid90 + */ +PARAM_DEFINE_INT32(ADSB_LEN_WIDTH, 1); + +/** + * ADSB-Out Vehicle Emitter Type + * + * Configure the emitter type of the vehicle. + * + * @reboot_required true + * @min 0 + * @max 15 + * @group Transponder + * + * @value 0 Unknown + * @value 1 Light + * @value 2 Small + * @value 3 Large + * @value 4 HighVortex + * @value 5 Heavy + * @value 6 Performance + * @value 7 Rotorcraft + * @value 8 RESERVED + * @value 9 Glider + * @value 10 LightAir + * @value 11 Parachute + * @value 12 UltraLight + * @value 13 RESERVED + * @value 14 UAV + * @value 15 Space + * @value 16 RESERVED + * @value 17 EmergencySurf + * @value 18 ServiceSurf + * @value 19 PointObstacle + */ +PARAM_DEFINE_INT32(ADSB_EMIT_TYPE, 14); + +/** + * ADSB-Out Vehicle Max Speed + * + * Informs ADSB vehicles of this vehicle's max speed capability + * + * @reboot_required true + * @min 0 + * @max 6 + * @value 0 UnknownMaxSpeed + * @value 1 75Kts + * @value 2 150Kts + * @value 3 300Kts + * @value 4 600Kts + * @value 5 1200Kts + * @value 6 Over1200Kts + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_MAX_SPEED, 0); + +/** + * ADSB-In Special ICAO configuration + * + * This vehicle is always tracked. Use 0 to disable. + * + * @reboot_required false + * @min 0 + * @max 16777215 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0); + +/** + * ADSB-Out Emergency State + * + * Sets the vehicle emergency state + * + * @reboot_required false + * @min 0 + * @max 6 + * @value 0 NoEmergency + * @value 1 General + * @value 2 Medical + * @value 3 LowFuel + * @value 4 NoCommunications + * @value 5 Interference + * @value 6 Downed + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_EMERGC, 0); + +/** + * Sagetech MXS mode configuration + * + * This parameter defines the operating mode of the MXS + * + * @reboot_required false + * @min 0 + * @max 3 + * @group Transponder + * + * @value 0 Off + * @value 1 On + * @value 2 Standby + * @value 3 Alt + */ +PARAM_DEFINE_INT32(MXS_OP_MODE, 0); + +/** + * Sagetech MXS Participant Configuration + * + * The MXS communication port to receive Target data from + * + * @min 0 + * @max 2 + * @reboot_required false + * @group Transponder + * + * @value 0 Auto + * @value 1 COM0 + * @value 2 COM1 + */ +PARAM_DEFINE_INT32(MXS_TARG_PORT, 1); + +/** + * Sagetech External Configuration Mode + * + * Disables auto-configuration mode enabling MXS config through external software. + * + * @reboot_required true + * @boolean + * @group Transponder + */ +PARAM_DEFINE_INT32(MXS_EXT_CFG, 0); + +/** + * MXS Serial Communication Baud rate + * + * Baudrate for the Serial Port connected to the MXS Transponder + * + * @reboot_required true + * @min 0 + * @max 10 + * @value 0 38400 + * @value 1 600 + * @value 2 4800 + * @value 3 9600 + * @value 4 RESERVED + * @value 5 57600 + * @value 6 115200 + * @value 7 230400 + * @value 8 19200 + * @value 9 460800 + * @value 10 921600 + * @group Serial + */ +PARAM_DEFINE_INT32(SER_MXS_BAUD, 5); diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE b/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE new file mode 100644 index 0000000000..f49a4e16e6 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md b/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md new file mode 100644 index 0000000000..c0a769e11e --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md @@ -0,0 +1,6 @@ +# Sagetech SDK +The contents of this folder includes selected functions from the full Sagetech SDK which can be found here: +https://github.com/Sagetech-Avionics/sagetech-mxs-sdk + +## Links +[Host Interface Control Document for MXS](https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf) diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c new file mode 100644 index 0000000000..1a4311ca39 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c @@ -0,0 +1,19 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file appendChecksum.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file + */ +void appendChecksum(uint8_t *buffer, uint8_t len) +{ + buffer[len - 1] = calcChecksum(buffer, len); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c new file mode 100644 index 0000000000..6a54c44fc0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c @@ -0,0 +1,26 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file calcChecksum.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file + */ +uint8_t calcChecksum(uint8_t *buffer, uint8_t len) +{ + uint8_t sum = 0x00; + + // Add all bytes excluding checksum + for (uint8_t i = 0; i < len - 1; ++i) { + sum += buffer[i]; + } + + return sum; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c new file mode 100644 index 0000000000..80aa4e6b7d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file charArray2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" +#include + +/* + * Documented in the header file. + */ +void charArray2Buf(uint8_t *bufferPos, char arr[], uint8_t len) +{ + for (uint8_t i = 0; i < len; ++i) { + bufferPos[i] = toupper(arr[i]); + } +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c new file mode 100644 index 0000000000..e51d43a22f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c @@ -0,0 +1,30 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file float2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void float2Buf(uint8_t *bufferPos, float value) +{ + const uint16_t FLOAT_SIZE = 4; + + union { + float val; + unsigned char bytes[FLOAT_SIZE]; + } conversion; + + conversion.val = value; + + for (int i = 0; i < FLOAT_SIZE; ++i) { + bufferPos[i] = conversion.bytes[i]; + } +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c new file mode 100644 index 0000000000..63ee41b748 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file icao2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void icao2Buf(uint8_t *bufferPos, uint32_t icao) +{ + bufferPos[0] = (icao & 0x00FF0000) >> 16; + bufferPos[1] = (icao & 0x0000FF00) >> 8; + bufferPos[2] = (icao & 0x000000FF); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h new file mode 100644 index 0000000000..587b1dec39 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h @@ -0,0 +1,13 @@ +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "sg.h" +#include "sgUtil.h" + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h new file mode 100644 index 0000000000..b5e3ce6ced --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h @@ -0,0 +1,899 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sg.h + * @author jimb + * + * @date Feb 10, 2021 + * + * Sagetech protocol for host message building and parsing. + * + * This module performs both the following: + * 1. Parses raw Sagetech host messages defined in the SDIM and + * returns a populated struct dataset of the message type. + * 2. Receives a populated struct dataset of the desired host message + * and returns the corresponding raw message data buffer. + */ + +#ifndef SG_H +#define SG_H + +#include +#include + +/// Host Message Lengths (bytes) +#define SG_MSG_LEN_INSTALL 41 +#define SG_MSG_LEN_FLIGHT 17 +#define SG_MSG_LEN_OPMSG 17 +#define SG_MSG_LEN_GPS 68 +#define SG_MSG_LEN_DATAREQ 9 +#define SG_MSG_LEN_TARGETREQ 12 +#define SG_MSG_LEN_MODE 10 + +/// Host Message Types +#define SG_MSG_TYPE_HOST_INSTALL 0x01 +#define SG_MSG_TYPE_HOST_FLIGHT 0x02 +#define SG_MSG_TYPE_HOST_OPMSG 0x03 +#define SG_MSG_TYPE_HOST_GPS 0x04 +#define SG_MSG_TYPE_HOST_DATAREQ 0x05 +#define SG_MSG_TYPE_HOST_TARGETREQ 0x0B +#define SG_MSG_TYPE_HOST_MODE 0x0C + +/// XPNDR Message Types +#define SG_MSG_TYPE_XPNDR_ACK 0x80 +#define SG_MSG_TYPE_XPNDR_INSTALL 0x81 +#define SG_MSG_TYPE_XPNDR_FLIGHT 0x82 +#define SG_MSG_TYPE_XPNDR_STATUS 0x83 +#define SG_MSG_TYPE_XPNDR_COMMA 0x85 +#define SG_MSG_TYPE_XPNDR_MODE 0x8C +#define SG_MSG_TYPE_XPNDR_VERSION 0x8E +#define SG_MSG_TYPE_XPNDR_SERIALNUM 0x8F + +/// ADS-B Message Types +#define SG_MSG_TYPE_ADSB_TSUMMARY 0x90 +#define SG_MSG_TYPE_ADSB_SVR 0x91 +#define SG_MSG_TYPE_ADSB_MSR 0x92 +#define SG_MSG_TYPE_ADSB_TSTATE 0x97 +#define SG_MSG_TYPE_ADSB_ARVR 0x98 + +/// Start byte for all host messages +#define SG_MSG_START_BYTE 0xAA + +/// Emitter category set byte values +#define SG_EMIT_GROUP_A 0x00 +#define SG_EMIT_GROUP_B 0x01 +#define SG_EMIT_GROUP_C 0x02 +#define SG_EMIT_GROUP_D 0x03 + +/// Emitter category enumeration offsets +#define SG_EMIT_OFFSET_A 0x00 +#define SG_EMIT_OFFSET_B 0x10 +#define SG_EMIT_OFFSET_C 0x20 +#define SG_EMIT_OFFSET_D 0x30 + +/** + * Available COM port baud rates. + */ +typedef enum { + baud38400 = 0, + baud600, + baud4800, + baud9600, + baud28800, + baud57600, + baud115200, + baud230400, + baud19200, + baud460800, + baud921600 +} sg_baud_t; + +/** + * Transponder ethernet configuration + */ +typedef struct { + uint32_t ipAddress; /// The transponder ip address + uint32_t subnetMask; /// The transponder subnet mask + uint16_t portNumber; /// The transponder port number +} sg_ethernet_t; + +/** + * Available GPS integrity SIL values + */ +typedef enum { + silUnknown = 0, + silLow, + silMedium, + silHigh +} sg_sil_t; + +/** + * Available GPS integrity SDA values + */ +typedef enum { + sdaUnknown = 0, + sdaMinor, + sdaMajor, + sdaHazardous +} sg_sda_t; + +/** + * Available emitter types + */ +typedef enum { + aUnknown = SG_EMIT_OFFSET_A, + aLight, + aSmall, + aLarge, + aHighVortex, + aHeavy, + aPerformance, + aRotorCraft, + bUnknown = SG_EMIT_OFFSET_B, + bGlider, + bAir, + bParachutist, + bUltralight, + bUAV = SG_EMIT_OFFSET_B + 6, + bSpace, + cUnknown = SG_EMIT_OFFSET_C, + cEmergency, + cService, + cPoint, + cCluster, + cLine, + dUnknown = SG_EMIT_OFFSET_D +} sg_emitter_t; + +/** + * Available aircraft sizes in meters + */ +typedef enum { + sizeUnknown = 0, /// Dimensions unknown + sizeL15W23, /// Length <= 15m & Width <= 23m + sizeL25W28, /// Length <= 25m & Width <= 28.5m + sizeL25W34, /// Length <= 25m & Width <= 34m + sizeL35W33, /// Length <= 35m & Width <= 33m + sizeL35W38, /// Length <= 35m & Width <= 38m + sizeL45W39, /// Length <= 45m & Width <= 39.5m + sizeL45W45, /// Length <= 45m & Width <= 45m + sizeL55W45, /// Length <= 55m & Width <= 45m + sizeL55W52, /// Length <= 55m & Width <= 52m + sizeL65W59, /// Length <= 65m & Width <= 59.5m + sizeL65W67, /// Length <= 65m & Width <= 67m + sizeL75W72, /// Length <= 75m & Width <= 72.5m + sizeL75W80, /// Length <= 75m & Width <= 80m + sizeL85W80, /// Length <= 85m & Width <= 80m + sizeL85W90 /// Length <= 85m & Width <= 90m +} sg_size_t; + +/** + * Available aircraft maximum airspeeds + */ +typedef enum { + speedUnknown = 0, /// Max speed unknown + speed75kt, /// 0 knots < Max speed < 75 knots + speed150kt, /// 75 knots < Max speed < 150 knots + speed300kt, /// 150 knots < Max speed < 300 knots + speed600kt, /// 300 knots < Max speed < 600 knots + speed1200kt, /// 600 knots < Max speed < 1200 knots + speedGreater /// 1200 knots < Max speed +} sg_airspeed_t; + +/** + * Available antenna configurations + */ +typedef enum { + antBottom = 1, /// bottom antenna only + antBoth = 3 /// both top and bottom antennae +} sg_antenna_t; + +/** + * The XPNDR Installation Message. + * Host --> XPNDR. + * XPNDR --> Host. + * Use 'strcpy(install.reg, "REGVAL1")' to assign the registration. + */ +typedef struct { + uint32_t icao; /// The aircraft's ICAO address + char reg[8]; /// The aircraft's registration (left-justified alphanumeric characters padded with spaces) + sg_baud_t com0; /// The baud rate for COM Port 0 + sg_baud_t com1; /// The baud rate for COM Port 1 + sg_ethernet_t eth; /// The ethernet configuration + sg_sil_t sil; /// The gps integrity SIL parameter + sg_sda_t sda; /// The gps integrity SDA parameter + sg_emitter_t emitter; /// The platform's emitter type + sg_size_t size; /// The platform's dimensions + sg_airspeed_t maxSpeed; /// The platform's maximum airspeed + int16_t altOffset; /// The altitude encoder offset is a legacy field that should always = 0 + sg_antenna_t antenna; /// The antenna configuration + bool altRes100; /// Altitude resolution. true = 100 foot, false = 25 foot + bool hdgTrueNorth; /// Heading type. true = true north, false = magnetic north + bool airspeedTrue; /// Airspeed type. true = true speed, false = indicated speed + bool heater; /// true = heater enabled, false = heater disabled + bool wowConnected; /// Weight on Wheels sensor. true = connected, false = not connected +} sg_install_t; + +/** + * The XPNDR Flight ID Message. + * Host --> XPNDR. + * XPNDR --> Host. + * * Use 'strcpy(id.flightID, "FLIGHTNO")' to assign the flight identification. + */ +typedef struct { + char flightId[9]; /// The flight identification (left-justified alphanumeric characters padded with spaces) +} sg_flightid_t; + +/** + * Available transponder operating modes. The enumerated values are + * offset from the host message protocol values. + */ +typedef enum { + modeOff = 0, /// 'Off' Mode: Xpdr will not transmit + modeOn, /// 'On' Mode: Full functionality with Altitude = Invalid + modeStby, /// 'Standby' Mode: Reply to lethal interrogations, only + modeAlt /// 'Alt' Mode: Full functionality +} sg_op_mode_t; + +/** + * Available emergency status codes. + */ +typedef enum { + emergcNone = 0, /// No Emergency + emergcGeneral, /// General Emergency + emergcMed, /// Lifeguard/Medical Emergency + emergcFuel, /// Minimum Fuel + emergcComm, /// No Communications + emergcIntrfrc, /// Unlawful Interference + emergcDowned /// Downed Aircraft +} sg_emergc_t; + +/** + * The XPNDR Operating Message. + * Host --> XPNDR. + */ +typedef struct { + uint16_t squawk; /// 4-digit octal Mode A code + sg_op_mode_t opMode; /// Operational mode + bool savePowerUp; /// Save power-up state in non-volatile + bool enableSqt; /// Enable extended squitters + bool enableXBit; /// Enable the x-bit + bool milEmergency; /// Broadcast a military emergency + sg_emergc_t emergcType; /// Enumerated civilian emergency type + bool identOn; /// Set the identification switch = On + bool altUseIntrnl; /// True = Report altitude from internal pressure sensor (will ignore other bits in the field) + bool altHostAvlbl; /// True = Host Altitude is being provided + bool altRes25; /// Host Altitude Resolution from install message, True = 25 ft, False = 100 ft + int32_t altitude; /// Sea-level altitude in feet. Field is ignored when internal altitude is selected. + bool climbValid; /// Climb rate is provided; + int16_t climbRate; /// Climb rate in ft/min. Limits are +/- 16,448 ft/min. + bool headingValid; /// Heading is valid. + double heading; /// Heading in degrees + bool airspdValid; /// Airspeed is valid. + uint16_t airspd; /// Airspeed in knots. +} sg_operating_t; + +/** + * Avaiable NACp values. + */ +typedef enum { + nacpUnknown, /// >= 18.52 km ( 10 nmile) + nacp10dot0, /// < 18.52 km ( 10 nmile) + nacp4dot0, /// < 7.408 km ( 4 nmile) + nacp2dot0, /// < 3.704 km ( 2 nmile) + nacp1dot0, /// < 1.852 km ( 1 nmile) + nacp0dot5, /// < 0.926 km (0.5 nmile) + nacp0dot3, /// < 0.556 km (0.3 nmile) + nacp0dot1, /// < 0.185 km (0.1 nmile) + nacp0dot05, /// < 92.6 m (0.05 nmile) + nacp30, /// < 30.0 m + nacp10, /// < 10.0 m + nacp3 /// < 3.0 m +} sg_nacp_t; + +/** + * Available NACv values (m/s) + */ +typedef enum { + nacvUnknown = 0, /// 10 <= NACv (or NACv is unknown) + nacv10dot0, /// 3 <= NACv < 10 + nacv3dot0, /// 1 <= NACv < 3 + nacv1dot0, /// 0.3 <= NACv < 1 + nacv0dot3 /// 0.0 <= NACv < 0.3 +} sg_nacv_t; + +/** + * The XPNDR Simulated GPS Message. + * Host --> XPNDR. + */ +typedef struct { + char longitude[12]; /// The absolute value of longitude (degree and decimal minute) + char latitude[11]; /// The absolute value of latitude (degree and decimal minute) + char grdSpeed[7]; /// The GPS over-ground speed (knots) + char grdTrack[9]; /// The GPS track referenced from True North (degrees, clockwise) + bool latNorth; /// The aircraft is in the northern hemisphere + bool lngEast; /// The aircraft is in the eastern hemisphere + bool fdeFail; /// True = A satellite error has occurred + bool gpsValid; /// True = GPS data is valid + char timeOfFix[11]; /// Time, relative to midnight UTC (can optionally be filled spaces) + float height; /// The height above the WGS-84 ellipsoid (meters) + float hpl; /// The Horizontal Protection Limit (meters) + float hfom; /// The Horizontal Figure of Merit (meters) + float vfom; /// The Vertical Figure of Merit (meters) + sg_nacv_t nacv; /// Navigation Accuracy for Velocity (meters/second) +} sg_gps_t; + +/** + * Available data request types + */ +typedef enum { + dataInstall = 0x81, /// Installation data + dataFlightID = 0x82, /// Flight Identification data + dataStatus = 0x83, /// Status Response data + dataMode = 0x8C, /// Mode Settings data + dataHealth = 0x8D, /// Health Monitor data + dataVersion = 0x8E, /// Version data + dataSerialNum = 0x8F, /// Serial Number data + dataTOD = 0xD2, /// Time of Day data + dataMode5 = 0xD3, /// Mode 5 Indication data + dataCrypto = 0xD4, /// Crypto Status data + dataMilSettings = 0xD7 /// Military Settings data +} sg_datatype_t; + +/** + * The Data Request message. + * Host --> XPDR. + */ +typedef struct { + sg_datatype_t reqType; /// The desired data response + uint8_t resv[3]; +} sg_datareq_t; + +/** + * Available target request types + */ +typedef enum { + reportAuto = 0, /// Enable auto output of all target reports + reportSummary, /// Report list of all tracked targets (disables auto-output) + reportIcao, /// Generate reports for specific target, only (disables auto-output) + reportNone /// Disable all target reports +} sg_reporttype_t; + +/** + * Available target report transmission ports + */ +typedef enum { + transmitSource = 0, /// Transmit reports on channel where target request was received + transmitCom0, /// Transmit reports on Com0 + transmitCom1, /// Transmit reports on Com1 + transmitEth /// Transmit reports on Ethernet +} sg_transmitport_t; + +/** + * The Target Request message for ADS-B 'in' data. + * Host --> XPDR. + */ +typedef struct { + sg_reporttype_t reqType; /// The desired report mode + sg_transmitport_t transmitPort; /// The communication port used for report transmission + uint16_t maxTargets; /// The maximum number of targets to track (max value: 404) + uint32_t icao; /// The desired target's ID, if applicable + bool stateVector; /// Transmit state vector reports + bool modeStatus; /// Transmit mode status reports + bool targetState; /// Transmit target state reports + bool airRefVel; /// Transmit air referenced velocity reports + bool tisb; /// Transmit raw TIS-B message reports (requires auto-output) + bool military; /// Enable tracking of military aircraft + bool commA; /// Transmit Comm-A Reports (requires auto-output) + bool ownship; /// Transmit reports about own aircraft +} sg_targetreq_t; + +/** + * The Mode message. + * Host --> XPDR. + */ +typedef struct { + bool reboot; /// Reboot the MX +} sg_mode_t; + +/** + * The XPNDR Acknowledge Message following all host messages. + * XPNDR --> Host. + */ +typedef struct { + uint8_t ackType; /// Message type being acknowledged + uint8_t ackId; /// Message ID being acknowledged + bool failXpdr; /// Built-in-test failure + bool failSystem; /// Required system input missing + bool failCrypto; /// Crypto status failure + bool wow; /// Weight-on-wheels indicates aircraft is on-ground + bool maint; /// Maintenance mode enabled + bool isHostAlt; /// False = Pressure sensor altitude, True = Host provided value + sg_op_mode_t opMode; /// Operational mode + int32_t alt; /// Altitude (feet) + bool altValid; /// Altitude is valid +} sg_ack_t; + +/** + * The XPNDR Status Response Message following a Data Request for Status. + * XPNDR --> Host. + */ +typedef struct { + uint8_t versionSW; /// SW Version # installed on the XPNDR + uint8_t versionFW; /// FW Version # installed on the XPNDR + uint32_t crc; /// CRC Checksum for the installed XPNDR SW/FW versions + + bool powerUp : 1; /// Integrity of CPU and Non-Volatile data at power-up + bool continuous : 1; /// Set by any other B.I.T. failures during operation + bool processor : 1; /// One-time processor instruction set test at power-up + bool crcValid : 1; /// Calculate then verifies the CRC against the stored value + bool memory : 1; /// Processor RAM is functional + bool calibrated : 1; /// Transponder is calibrated + bool receiver : 1; /// RF signals travel through hardware correctly + bool power53v : 1; /// Voltage at the 53V power supply is correct + bool adc : 1; /// Analog-to-Digital Converter is functional + bool pressure : 1; /// Internal pressure transducer is functional + bool fpga : 1; /// FPGA I/O operations are functional + bool rxLock : 1; /// Rx oscillator reporting PLL Lock at reference frequency + bool txLock : 1; /// Tx oscillator reporting PLL Lock at reference frequency + bool mtSuppress : 1; /// Mutual suppression is operating correctly + bool temp : 1; /// Internal temperature is within range (< 110 C) + bool sqMonitor : 1; /// Squitters are transmitting at their nominal rates + bool txRate : 1; /// Transmission duty cycle is in the safe range + bool sysLatency : 1; /// Systems events occurred within expected time limits + bool txPower : 1; /// Transmission power is in-range + bool voltageIn : 1; /// Input voltage is in-range (10V-32V) + bool icao : 1; /// ICAO Address is valid (fail at '000000' or 'FFFFFF') + bool gps : 1; /// Valid GPS data is received at 1Hz, minimum +} sg_status_t; + +/** + * The XPNDR Health Monitor Response Message. + * XPNDR --> Host. + */ +typedef struct { + int8_t socTemp; /// System on a Chip temperature + int8_t rfTemp; /// RF Board temperature + int8_t ptTemp; /// Pressure Transducer temperature +} sg_healthmonitor_t; + +/** + * The XPNDR Version Response Message. + * XPNDR --> Host. + */ +typedef struct { + uint8_t swVersion; /// The SW Version major revision number + uint8_t fwVersion; /// The FW Version major revision number + uint16_t swSvnRevision; /// The SW Repository version number + uint16_t fwSvnRevision; /// The FW Repository version number +} sg_version_t; + +/** + * The XPNDR Serial Number Response Message. + * XPNDR --> Host. + */ +typedef struct { + char ifSN[33]; /// The Interface Board serial number + char rfSN[33]; /// The RF Board serial number + char xpndrSN[33]; /// The Transponder serial number +} sg_serialnumber_t; + +/// The state vector report type. +typedef enum { + svrAirborne = 1, /// Airborne state vector report type. + svrSurface /// Surface state vector report type. +} sg_svr_type_t; + +/// The state vector report participant address type. +typedef enum { + svrAdrIcaoUnknown, /// ICAO address unknown emitter category. + svrAdrNonIcaoUnknown, /// Non-ICAO address unknown emitter category. + svrAdrIcao, /// ICAO address aircraft. + svrAdrNonIcao, /// Non-ICAO address aircraft. + svrAdrIcaoSurface, /// ICAO address surface vehicle, fixed ground, tethered obstruction. + svrAdrNonIcaoSurface, /// Non-ICAO address surface vehicle, fixed ground, tethered obstruction. + svrAdrDup, /// Duplicate target of another ICAO address. + svrAdrAdsr /// ADS-R target. +} sg_addr_type_t; + +/// The surface part of a state vector report. +typedef struct { + int16_t speed; /// Surface speed. + int16_t heading; /// Surface heading. +} sg_svr_surface_t; + +/// The airborne part of a state vector report. +typedef struct { + int16_t velNS; /// The NS speed vector component. [knots] + int16_t velEW; /// The EW speed vector component. [knots] + int16_t speed; /// Speed from N/S and E/W velocity. [knots] + int16_t heading; /// Heading from N/S and E/W velocity. [deg from N] + int32_t geoAlt; /// Geometric altitude. [ft] + int32_t baroAlt; /// Barometric altitude. [ft] + int16_t vrate; /// Vertical rate. [ft/min] + float estLat; /// Estimated latitude. [deg N] + float estLon; /// Estimated longitude. [deg E] +} sg_svr_airborne_t; + +typedef struct { + bool baroVRate : 1; /// Barometric vertical rate valid. + bool geoVRate : 1; /// Geometric vertical rate valid. + bool baroAlt : 1; /// Barometric altitude valid. + bool surfHeading : 1; /// Surface heading valid. + bool surfSpeed : 1; /// Surface speed valid. + bool airSpeed : 1; /// Airborne speed and heading valid. + bool geoAlt : 1; /// Geometric altitude valid. + bool position : 1; /// Lat and lon data valid. +} sg_svr_validity_t; + +typedef struct { + uint8_t reserved : 6; /// Reserved. + bool estSpeed : 1; /// Estimated N/S and E/W velocity. + bool estPosition : 1; /// Estimated lat/lon position. +} sg_svr_est_validity_t; + +/** + * The XPDR ADS-B state vector report Message. + * Host --> XPDR. + * + * @note The time of applicability values are based on the MX system clock that starts + * at 0 on power up. The time is the floating point number that is the seconds since + * power up. The time number rolls over at 512.0. + */ +typedef struct { + sg_svr_type_t type; /// Report type. + union { + uint8_t flags; + sg_svr_validity_t validity; /// Field validity flags. + }; + union { + uint8_t eflags; + sg_svr_est_validity_t evalidity; /// Estimated field validity flags. + }; + uint32_t addr; /// Participant address. + sg_addr_type_t addrType; /// Participant address type. + float toaEst; /// Report estimated position and speed time of applicability. + float toaPosition; /// Report position time of applicability. + float toaSpeed; /// Report speed time of applicability. + uint8_t survStatus; /// Surveillance status. + uint8_t mode; /// Report mode. + uint8_t nic; /// Navigation integrity category. + float lat; /// Latitude. + float lon; /// Longitude. + union { + sg_svr_surface_t surface; /// Surface SVR data. + sg_svr_airborne_t airborne; /// Airborne SVR data. + }; +} sg_svr_t; + +typedef enum { + msrTypeV0, + msrTypeV1Airborne, + msrTypeV1Surface, + msrTypeV2Airborne, + msrTypeV2Surface +} sg_msr_type_t; + +typedef struct { + uint8_t reserved : 2; + bool priority : 1; + bool sil : 1; + bool nacv : 1; + bool nacp : 1; + bool opmode : 1; + bool capcodes : 1; +} sg_msr_validity_t; + +typedef enum { + adsbVerDO260, + adsbVerDO260A, + adsbVerDO260B +} sg_adsb_version_t; + +typedef enum { + adsbUnknown, + adsbLight, + adsbSmall = 0x3, + adsbLarge = 0x5, + adsbHighVortex, + adsbHeavy, + adsbPerformance, + adsbRotorcraft = 0x0A, + adsbGlider, + adsbAir, + adsbUnmaned, + adsbSpace, + adsbUltralight, + adsbParachutist, + adsbVehicle_emg = 0x14, + adsbVehicle_serv, + adsbObsticlePoint, + adsbObsticleCluster, + adsbObsticleLinear +} sg_adsb_emitter_t; + +typedef enum { + priNone, + priGeneral, + priMedical, + priFuel, + priComm, + priUnlawful, + priDowned +} sg_priority_t; + +typedef enum { + tcrNone, + tcrSingle, + tcrMultiple +} sg_tcr_t; + +typedef struct { + bool b2low : 1; + bool uat : 1; + bool arv : 1; + bool tsr : 1; + bool adsb : 1; + bool tcas : 1; + sg_tcr_t tcr; +} sg_capability_t; + +typedef enum { + gpsLonNodata, + gpsLonSensorSupplied, + gpsLon2m, + gpsLon4m, + gpsLon6m, + gpsLon8m, + gpsLon10m, + gpsLon12m, + gpsLon14m, + gpsLon16m, + gpsLon18m, + gpsLon20m, + gpsLon22m, + gpsLon24m, + gpsLon26m, + gpsLon28m, + gpsLon30m, + gpsLon32m, + gpsLon34m, + gpsLon36m, + gpsLon38m, + gpsLon40m, + gpsLon42m, + gpsLon44m, + gpsLon46m, + gpsLon48m, + gpsLon50m, + gpsLon52m, + gpsLon54m, + gpsLon56m, + gpsLon58m, + gpsLon60m +} sg_gps_lonofs_t; + +typedef enum { + gpslatNodata, + gpslatLeft2m, + gpslatLeft4m, + gpslatLeft6m, + gpslatRight0m, + gpslatRight2m, + gpslatRight4m, + gpslatRight6m, +} sg_gps_latofs_t; + +typedef struct { + bool gpsLatFmt; + sg_gps_latofs_t gpsLatOfs; + bool gpsLonFmt; + sg_gps_lonofs_t gpsLonOfs; + bool tcasRA : 1; + bool ident : 1; + bool singleAnt : 1; +} sg_adsb_opmode_t; + +typedef enum { + gvaUnknown, + gvaLT150m, + gvaLT45m +} sg_gva_t; + +typedef enum { + nicGolham, + nicNonGilham +} sg_nicbaro_t; + +typedef enum { + svsilUnknown, + svsilPow3, + svsilPow5, + svsilPow7 +} sg_svsil_t; + +typedef struct { + sg_nacp_t nacp; + sg_nacv_t nacv; + sg_sda_t sda; + bool silSupp; + sg_svsil_t sil; + sg_gva_t gva; + sg_nicbaro_t nicBaro; +} sg_sv_qual_t; + +typedef enum { + trackTrueNorth, + trackMagNorth, + headingTrueNorth, + headingMagNorth +} sg_trackheading_t; + +typedef enum { + vrateBaroAlt, + vrateGeoAlt +} sg_vratetype_t; + +/** + * The XPDR ADS-B mode status report Message. + * Host --> XPDR. + * + * @note The time of applicability values are based on the MX system clock that starts + * at 0 on power up. The time is the floating point number that is the seconds since + * power up. The time number rolls over at 512.0. + */ +typedef struct { + sg_msr_type_t type; /// Report type. + + union { + uint8_t flags; + sg_msr_validity_t validity; /// Field validity flags. + }; + + uint32_t addr; /// Participant address. + sg_addr_type_t addrType; /// Participant address type. + + float toa; + sg_adsb_version_t version; + char callsign[9]; + sg_adsb_emitter_t emitter; + sg_size_t size; + sg_priority_t priority; + sg_capability_t capability; + sg_adsb_opmode_t opMode; + sg_sv_qual_t svQuality; + sg_trackheading_t trackHeading; + sg_vratetype_t vrateType; +} sg_msr_t; + +/** + * Convert install message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw install message. + * @param[in] stl The install message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in stl parameter must be pre-validated. + */ +bool sgEncodeInstall(uint8_t *buffer, sg_install_t *stl, uint8_t msgId); + +/** + * Convert flight identification struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw flight identification message. + * @param[in] id The flight id struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in id parameter must be pre-validated. + */ +bool sgEncodeFlightId(uint8_t *buffer, sg_flightid_t *id, uint8_t msgId); + +/** + * Convert operating message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw operating message. + * @param[in] op The operating message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in op parameter must be pre-validated. + */ +bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId); + +/* TODO: Create GPS helper functions to convert other data types --> char buffers */ + +/** + * Convert GPS message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw GPS message. + * @param[in] gps The GPS message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in gps parameter must be pre-validated. + */ +bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId); + +/** + * Convert data request message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw target request message. + * @param[in] data The data request message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in data parameter must be pre-validated. + */ +bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId); + +/** + * Convert target request message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw target request message. + * @param[in] tgt The target request message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in tgt parameter must be pre-validated. + */ +bool sgEncodeTargetReq(uint8_t *buffer, sg_targetreq_t *tgt, uint8_t msgId); + +/** + * Process the ACK message response from the transponder. + * + * @param[in] buffer The raw ACK message buffer. + * @param[out] ack The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack); + +/** + * Process the Install message response from the transponder. + * + * @param[in] buffer The raw Install message buffer. + * @param[out] stl The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl); + +/** + * Process the Flight ID message response from the transponder. + * + * @param[in] buffer The raw Flight ID message buffer. + * @param[out] id The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id); + +/** + * Process the state vector report message. + * + * @param[in] buffer The raw SVR message buffer. + * @param[out] svr The parsed SVR message. + * + * @return true if successful or false on failure. + */ +bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr); + +/** + * Process the mode status report message. + * + * @param buffer The raw MSR message buffer. + * @param msr The parsed MSR message. + * + * @return true if successful or false on failure. + */ +bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr); + +#endif /* SG_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c new file mode 100644 index 0000000000..c1a2afe5e0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c @@ -0,0 +1,72 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeAck.c + * @author jimb + * + * @date Feb 10, 2021 + * + * This file receives a raw Acknowledge message buffer and + * parses the payload into a data struct. + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_ACK_XPNDR_FAIL 0x01 +#define SG_ACK_SYSTEM_FAIL 0x02 +#define SG_ACK_CRYPTO_FAIL 0x04 +#define SG_ACK_WOW 0x08 +#define SG_ACK_MAINT 0x10 +#define SG_ACK_ALT_SOURCE 0x20 +#define SG_ACK_OP_MODE 0xC0 + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + uint8_t ackType; + uint8_t ackId; + uint8_t state; + uint8_t alt[3]; + uint8_t checksum; +} ack_t; + +/* + * Documented in the header file. + */ +bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack) +{ + ack_t sgAck; + memcpy(&sgAck, buffer, sizeof(ack_t)); + + ack->ackType = sgAck.ackType; + ack->ackId = sgAck.ackId; + ack->failXpdr = (sgAck.state & SG_ACK_XPNDR_FAIL) > 0; + ack->failSystem = (sgAck.state & SG_ACK_SYSTEM_FAIL) > 0; + ack->failCrypto = (sgAck.state & SG_ACK_CRYPTO_FAIL) > 0; + ack->wow = (sgAck.state & SG_ACK_WOW) > 0; + ack->maint = (sgAck.state & SG_ACK_MAINT) > 0; + ack->isHostAlt = (sgAck.state & SG_ACK_ALT_SOURCE) > 0; + ack->opMode = (sgAck.state & SG_ACK_OP_MODE) >> 6; + + int32_t int24 = toInt32(sgAck.alt); + + // Bitmask altitude field to determine if alt = invalid + if ((int24 & 0x00FFFFFF) == 0x00800000) { + ack->alt = 0; + ack->altValid = false; + + } else { + ack->alt = int24; + ack->altValid = true; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c new file mode 100644 index 0000000000..e9cb01ff0f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c @@ -0,0 +1,41 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeFlightId.c + * @author Jacob.Garrison + * + * @date Mar 10, 2021 + * + */ + +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_ID_LEN 8 // The number of bytes in the flight id field + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + char flightId[SG_ID_LEN]; + uint8_t rsvd[4]; + uint8_t checksum; +} flightid_t; + +/* + * Documented in the header file. + */ +bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id) +{ + flightid_t sgId; + memcpy(&sgId, buffer, sizeof(flightid_t)); + + strcpy(id->flightId, sgId.flightId); + memset(&id->flightId[SG_ID_LEN], '\0', 1); // Ensure flight id is null-terminated + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c new file mode 100644 index 0000000000..c969509f1f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c @@ -0,0 +1,84 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeInstall.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_REG_LEN 7 // The number of bytes in the registration field + +#define SG_STL_ANTENNA 0x03 +#define SG_STL_ALT_RES 0x08 +#define SG_STL_HDG_TYPE 0x10 +#define SG_STL_SPD_TYPE 0x20 +#define SG_STL_HEATER 0x40 +#define SG_STL_WOW 0x80 + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + uint8_t icao[3]; + char registration[SG_REG_LEN]; + uint8_t rsvd1[2]; + uint8_t com0; + uint8_t com1; + uint8_t ipAddress[4]; + uint8_t subnetMask[4]; + uint8_t port[2]; + uint8_t gpsIntegrity; + uint8_t emitterSet; + uint8_t emitterType; + uint8_t size; + uint8_t maxSpeed; + uint8_t altOffset[2]; + uint8_t rsvd2[2]; + uint8_t config; + uint8_t rsvd3[2]; + uint8_t checksum; +} stl_t; + +/* + * Documented in the header file. + */ +bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl) +{ + memset(&stl->reg[0], '\0', sizeof(stl->reg)); // Ensure registration is null-terminated + + stl_t sgStl; + memcpy(&sgStl, buffer, sizeof(stl_t)); + + stl->icao = toIcao(sgStl.icao); + strcpy(stl->reg, sgStl.registration); + memset(&stl->reg[SG_REG_LEN], 0, 1); // Ensure registration is null-terminated + stl->com0 = (sg_baud_t)(sgStl.com0); + stl->com1 = (sg_baud_t)(sgStl.com1); + stl->eth.ipAddress = toUint32(sgStl.ipAddress); + stl->eth.subnetMask = toUint32(sgStl.subnetMask); + stl->eth.portNumber = toUint16(sgStl.port); + stl->sil = (sg_sil_t)(sgStl.gpsIntegrity >> 4); + stl->sda = (sg_sda_t)(sgStl.gpsIntegrity & 0x0F); + stl->emitter = (sg_emitter_t)(0x10 * sgStl.emitterSet + sgStl.emitterType); + stl->size = (sg_size_t)sgStl.size; + stl->maxSpeed = (sg_airspeed_t)sgStl.maxSpeed; + stl->altOffset = toUint16(sgStl.altOffset); + stl->antenna = (sg_antenna_t)sgStl.config & SG_STL_ANTENNA; + stl->altRes100 = sgStl.config & SG_STL_ALT_RES; + stl->hdgTrueNorth = sgStl.config & SG_STL_HDG_TYPE; + stl->airspeedTrue = sgStl.config & SG_STL_SPD_TYPE; + stl->heater = sgStl.config & SG_STL_HEATER; + stl->wowConnected = sgStl.config & SG_STL_WOW; + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c new file mode 100644 index 0000000000..522b1b58d0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c @@ -0,0 +1,183 @@ +/** + * @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeMSR.c + * @author jim.billmeyer + * + * @date Mar 17, 2021 + */ + +#include "sg.h" +#include +#include +#include +#include "sgUtil.h" + +#define MS_PARAM_TOA (1 << 3) +#define MS_PARAM_ADSBVER (1 << 2) +#define MS_PARAM_CALLSIGN (1 << 1) +#define MS_PARAM_CATEMITTER (1 << 0) + +#define MS_PARAM_AVLEN (1 << 7) +#define MS_PARAM_PRIORITY (1 << 6) +#define MS_PARAM_CAPCODES (1 << 5) +#define MS_PARAM_OPMODE (1 << 4) +#define MS_PARAM_NACP (1 << 3) +#define MS_PARAM_NACV (1 << 2) +#define MS_PARAM_SDA (1 << 1) +#define MS_PARAM_GVA (1 << 0) + +#define MS_PARAM_NIC (1 << 7) +#define MS_PARAM_HEADING (1 << 6) +#define MS_PARAM_VRATE (1 << 5) + +#define MS_CAP_B2LOW (1 << 3) +#define MS_CAP_UAT (1 << 1) +#define MS_CAP_TCR ((1 << 2) | (1 << 3)) +#define MS_CAP_TSR (1 << 4) +#define MS_CAP_ARV (1 << 5) +#define MS_CAP_ADSB (1 << 6) +#define MS_CAP_TCAS (1 << 7) + +#define MS_OP_GPS_LATFMT (1 << 7) +#define MS_OP_GPS_LONFMT (1 << 6) +#define MS_OP_TCAS_RA (1 << 5) +#define MS_OP_IDENT (1 << 4) +#define MS_OP_SINGLE_ANT (1 << 2) + +/// the payload offset. +#define PBASE 4 + +/* + * Documented in the header file. + */ +bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr) +{ + memset(msr, 0, sizeof(sg_msr_t)); + + uint8_t fields[3]; + memcpy(fields, &buffer[PBASE + 0], 3); + + if (buffer[PBASE + 1] == 0x6E && buffer[PBASE + 2] == 0x60) { + msr->type = msrTypeV0; + + } else if (buffer[PBASE + 1] == 0x7E && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV1Airborne; + + } else if (buffer[PBASE + 1] == 0xFE && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV1Surface; + + } else if (buffer[PBASE + 1] == 0x7F && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV2Airborne; + + } else if (buffer[PBASE + 1] == 0xFF && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV2Surface; + } + + msr->flags = buffer[PBASE + 3]; + msr->addr = toInt32(&buffer[PBASE + 4]) & 0xFFFFFF; + msr->addrType = buffer[PBASE + 7] & 0xFF; + + uint8_t ofs = 8; + + if (fields[0] & MS_PARAM_TOA) { + msr->toa = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & MS_PARAM_ADSBVER) { + msr->version = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[0] & MS_PARAM_CALLSIGN) { + memset(msr->callsign, 0, 9); + memcpy(msr->callsign, &buffer[PBASE + ofs], 8); + ofs += 8; + } + + if (fields[0] & MS_PARAM_CATEMITTER) { + msr->emitter = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_AVLEN) { + msr->size = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_PRIORITY) { + msr->priority = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_CAPCODES) { + uint8_t cap = buffer[PBASE + ofs + 0]; + msr->capability.b2low = cap & MS_CAP_B2LOW; + + cap = buffer[PBASE + ofs + 1]; + msr->capability.uat = cap & MS_CAP_UAT; + msr->capability.tcr = (cap & MS_CAP_TCR) >> 2; + msr->capability.tsr = cap & MS_CAP_TSR; + msr->capability.arv = cap & MS_CAP_ARV; + msr->capability.adsb = cap & MS_CAP_ADSB; + msr->capability.tcas = cap & MS_CAP_TCAS; + + ofs += 3; + } + + if (fields[1] & MS_PARAM_OPMODE) { + uint8_t op = buffer[PBASE + ofs + 0]; + msr->opMode.gpsLatFmt = (op & MS_OP_GPS_LATFMT) == 0; + msr->opMode.gpsLonFmt = (op & MS_OP_GPS_LONFMT) == 0; + msr->opMode.tcasRA = op & MS_OP_TCAS_RA; + msr->opMode.ident = op & MS_OP_IDENT; + msr->opMode.singleAnt = op & MS_OP_SINGLE_ANT; + + op = buffer[PBASE + ofs + 1]; + msr->opMode.gpsLatOfs = op >> 5; + msr->opMode.gpsLonOfs = op & 0x17; + + ofs += 2; + } + + if (fields[1] & MS_PARAM_NACP) { + msr->svQuality.nacp = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_NACV) { + msr->svQuality.nacv = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_SDA) { + uint8_t sda = buffer[PBASE + ofs]; + msr->svQuality.sda = (sda & 0x18) >> 3; + msr->svQuality.silSupp = (sda & 0x04); + msr->svQuality.sil = (sda & 0x03); + ofs++; + } + + if (fields[1] & MS_PARAM_GVA) { + msr->svQuality.gva = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_NIC) { + msr->svQuality.nicBaro = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_HEADING) { + msr->trackHeading = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_VRATE) { + msr->vrateType = buffer[PBASE + ofs]; + ofs++; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c new file mode 100644 index 0000000000..3d24a7729d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c @@ -0,0 +1,209 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeSVR.c + * @author jimb + * + * @date Feb 10, 2021 + * + * This file receives a raw ADS-B target state vector report message buffer and + * parses the payload into a data struct. + */ + +#include +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" +#include "target.h" + +// airborne surface +// -------- -------- +#define SV_PARAM_TOA_EPOS (1 << 3) // x +#define SV_PARAM_TOA_POS (1 << 2) // x x +#define SV_PARAM_TOA_VEL (1 << 1) // x x +#define SV_PARAM_LATLON (1 << 0) // x x + +#define SV_PARAM_GEOALT (1 << 7) // x +#define SV_PARAM_VEL (1 << 6) // x +#define SV_PARAM_SURF_GS (1 << 5) // x +#define SV_PARAM_SURF_HEAD (1 << 4) // x +#define SV_PARAM_BAROALT (1 << 3) // x +#define SV_PARAM_VRATE (1 << 2) // x +#define SV_PARAM_NIC (1 << 1) // x x +#define SV_PARAM_ESTLAT (1 << 0) // x + +#define SV_PARAM_ESTLON (1 << 7) // x +#define SV_PARAM_ESTNVEL (1 << 6) +#define SV_PARAM_ESTEVAL (1 << 5) +#define SV_PARAM_SURV (1 << 4) // x x +#define SV_PARAM_REPORT (1 << 3) // x x + +/// the payload offset. +#define PBASE 4 + +/* + * Documented in the header file. + */ +bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr) +{ + // memset(svr, 0, sizeof(sg_svr_t)); + + uint8_t fields[3]; + memcpy(&fields, &buffer[PBASE + 0], 3); + + svr->type = buffer[PBASE + 0] == 0x1F ? svrAirborne : svrSurface; + svr->flags = buffer[PBASE + 3]; + svr->eflags = buffer[PBASE + 4]; + svr->addr = toInt32(&buffer[PBASE + 5]) & 0xFFFFFF; + svr->addrType = buffer[PBASE + 8] & 0xFF; + + uint8_t ofs = 9; + + if (fields[0] & SV_PARAM_TOA_EPOS) { + svr->toaEst = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_TOA_POS) { + svr->toaPosition = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_TOA_VEL) { + svr->toaSpeed = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_LATLON) { + if (svr->validity.position) { + svr->lat = toLatLon(&buffer[PBASE + ofs + 0]); + svr->lon = toLatLon(&buffer[PBASE + ofs + 3]); + + } else { + svr->lat = 0.0; + svr->lon = 0.0; + } + + ofs += 6; + } + + if (svr->type == svrAirborne) { + if (fields[1] & SV_PARAM_GEOALT) { + if (svr->validity.geoAlt) { + svr->airborne.geoAlt = toAlt(&buffer[PBASE + ofs]); + + } else { + svr->airborne.geoAlt = 0; + } + + ofs += 3; + } + + if (fields[1] & SV_PARAM_VEL) { + if (svr->validity.airSpeed) { + int16_t nvel = toVel(&buffer[PBASE + ofs + 0]); + int16_t evel = toVel(&buffer[PBASE + ofs + 2]); + + svr->airborne.heading = toHeading2((double)nvel, (double)evel); + svr->airborne.speed = sqrt(nvel * nvel + evel * evel); + svr->airborne.velEW = evel; + svr->airborne.velNS = nvel; + + } else { + svr->airborne.heading = 0; + svr->airborne.speed = 0; + svr->airborne.velEW = 0; + svr->airborne.velNS = 0; + } + + ofs += 4; + } + + if (fields[1] & SV_PARAM_BAROALT) { + if (svr->validity.baroAlt) { + svr->airborne.baroAlt = toAlt(&buffer[PBASE + ofs]); + + } else { + svr->airborne.baroAlt = 0; + } + + ofs += 3; + } + + if (fields[1] & SV_PARAM_VRATE) { + if (svr->validity.baroVRate || svr->validity.geoVRate) { + svr->airborne.vrate = toInt16(&buffer[PBASE + ofs]); + + } else { + svr->airborne.vrate = 0; + } + + ofs += 2; + } + + } else { + if (fields[1] & SV_PARAM_SURF_GS) { + if (svr->validity.surfSpeed) { + svr->surface.speed = toGS(&buffer[PBASE + ofs]); + + } else { + svr->surface.speed = 0; + } + + ofs += 1; + } + + if (fields[1] & SV_PARAM_SURF_HEAD) { + if (svr->validity.surfHeading) { + svr->surface.heading = toHeading(&buffer[PBASE + ofs]); + + } else { + svr->surface.heading = 0; + } + + ofs += 1; + } + } + + if (fields[1] & SV_PARAM_NIC) { + svr->nic = buffer[PBASE + ofs]; + + ofs += 1; + } + + if (fields[1] & SV_PARAM_ESTLAT) { + if (svr->evalidity.estPosition) { + svr->airborne.estLat = toLatLon(&buffer[PBASE + ofs]); + + } else { + svr->airborne.estLat = 0; + } + + ofs += 3; + } + + if (fields[2] & SV_PARAM_ESTLON) { + if (svr->evalidity.estPosition) { + svr->airborne.estLon = toLatLon(&buffer[PBASE + ofs]); + + } else { + svr->airborne.estLon = 0; + } + + ofs += 3; + } + + if (fields[2] & SV_PARAM_SURV) { + svr->survStatus = buffer[PBASE + ofs]; + ofs += 1; + } + + if (fields[2] & SV_PARAM_REPORT) { + svr->mode = buffer[PBASE + ofs]; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c new file mode 100644 index 0000000000..4a048548eb --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c @@ -0,0 +1,51 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeDataReq.c + * @author Jacob.Garrison + * + * @date Feb 23, 2021 + * + * This file receives a populated data request struct and + * converts it into a data request message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_DATAREQ SG_MSG_LEN_DATAREQ - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. + +#define OFFSET_REQ_TYPE 0 /// the requested response message type +#define OFFSET_RSVD_1 1 /// a reserved field +#define OFFSET_RSVD_2 2 /// a reserved field +#define OFFSET_RSVD_3 3 /// a reserved field + +/* + * Documented in the header file. + */ +bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_DATAREQ; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_DATAREQ; + + // populate Request Type + buffer[PBASE + OFFSET_REQ_TYPE] = data->reqType; + + // populate Reserved fields + buffer[PBASE + OFFSET_RSVD_1] = 0; + buffer[PBASE + OFFSET_RSVD_2] = 0; + buffer[PBASE + OFFSET_RSVD_3] = 0; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_DATAREQ); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c new file mode 100644 index 0000000000..a4466b7a07 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c @@ -0,0 +1,47 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeFlightId.c + * @author Jacob.Garrison + * + * @date Feb 25, 2021 + * + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_FLIGHT SG_MSG_LEN_FLIGHT - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_ID 0 /// the flight id offset in the payload. +#define OFFSET_RSVD 8 /// the reserved field offset in the payload. + +#define ID_LEN 8 /// the length of the flight identification field. + +/* + * Documented in the header file. + */ +bool sgEncodeFlightId(uint8_t *buffer, sg_flightid_t *id, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_FLIGHT; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_FLIGHT; + + // populate flight identification + charArray2Buf(&buffer[PBASE + OFFSET_ID], id->flightId, ID_LEN); + + // populate reserved field + uint322Buf(&buffer[PBASE + OFFSET_RSVD], 0); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_FLIGHT); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c new file mode 100644 index 0000000000..4820e0424b --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c @@ -0,0 +1,92 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeGPS.c + * @author Jacob.Garrison + * + * @date Mar 1, 2021 + * + * This file receives a populated GPS message struct and + * converts it into a GPS message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_GPS SG_MSG_LEN_GPS - 5 /// the payload length. +#define _UNUSED(x) ((void)(x)) + +#define PBASE 4 /// the payload offset. +#define OFFSET_LONGITUDE 0 /// the longitude offset in the payload. +#define OFFSET_LATITUDE 11 /// the latitude offset in the payload. +#define OFFSET_SPEED 21 /// the ground speed offset in the payload. +#define OFFSET_TRACK 27 /// the ground track offset in the payload. +#define OFFSET_STATUS 35 /// the hemisphere/data status offset in the payload. +#define OFFSET_TIME 36 /// the time of fix offset in the payload. +#define OFFSET_HEIGHT 46 /// the GNSS height offset in the payload. +#define OFFSET_HPL 50 /// the horizontal protection limit offset in the payload. +#define OFFSET_HFOM 54 /// the horizontal figure of merit offset in the payload. +#define OFFSET_VFOM 58 /// the vertical figure of merit offset in the payload. +#define OFFSET_NACV 62 /// the navigation accuracy for velocity offset in the payload. + +#define LEN_LNG 11 /// bytes in the longitude field +#define LEN_LAT 10 /// bytes in the latitude field +#define LEN_SPD 6 /// bytes in the speed over ground field +#define LEN_TRK 8 /// bytes in the ground track field +#define LEN_TIME 10 /// bytes in the time of fix field + +/* + * Documented in the header file. + */ +bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_GPS; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_GPS; + + // populate longitude + charArray2Buf(&buffer[PBASE + OFFSET_LONGITUDE], gps->longitude, LEN_LNG); + + // populate latitude + charArray2Buf(&buffer[PBASE + OFFSET_LATITUDE], gps->latitude, LEN_LAT); + + // populate ground speed + charArray2Buf(&buffer[PBASE + OFFSET_SPEED], gps->grdSpeed, LEN_SPD); + + // populate ground track + charArray2Buf(&buffer[PBASE + OFFSET_TRACK], gps->grdTrack, LEN_TRK); + + // populate hemisphere/data status + buffer[PBASE + OFFSET_STATUS] = !gps->gpsValid << 7 | + gps->fdeFail << 6 | + gps->lngEast << 1 | + gps->latNorth; + + // populate time of fix + charArray2Buf(&buffer[PBASE + OFFSET_TIME], gps->timeOfFix, LEN_TIME); + + // populate gnss height + float2Buf(&buffer[PBASE + OFFSET_HEIGHT], gps->height); + + // populate HPL + float2Buf(&buffer[PBASE + OFFSET_HPL], gps->hpl); + + // populate HFOM + float2Buf(&buffer[PBASE + OFFSET_HFOM], gps->hfom); + + // populate VFOM + float2Buf(&buffer[PBASE + OFFSET_VFOM], gps->vfom); + + // populate NACv + buffer[PBASE + OFFSET_NACV] = gps->nacv << 4; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_GPS); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c new file mode 100644 index 0000000000..c7fb0876cd --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c @@ -0,0 +1,132 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeInstall.c + * @author Jacob.Garrison + * + * @date Feb 23, 2021 + * + * This file receives a populated installation message struct and + * converts it into a installation message buffer. + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_INSTALL SG_MSG_LEN_INSTALL - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_ICAO 0 /// the icao address offset in the payload. +#define OFFSET_REG 3 /// the registration offset in the payload. +#define OFFSET_RSVD1 10 /// the first reserved field offset in the payload. +#define OFFSET_COM0 12 /// the COM port 0 offset in the payload. +#define OFFSET_COM1 13 /// the COM port 1 offset in the payload. +#define OFFSET_IP 14 /// the IP address offset in the payload. +#define OFFSET_MASK 18 /// the net mask offset in the payload. +#define OFFSET_PORT 22 /// the port number offset in the payload. +#define OFFSET_GPS 24 /// the GPS integrity offset in the payload. +#define OFFSET_EMIT_SET 25 /// the emitter category offset in the payload. +#define OFFSET_EMIT_TYPE 26 /// the emitter type offset in the payload. +#define OFFSET_SIZE 27 /// the aircraft size offset in the payload. +#define OFFSET_SPEED 28 /// the maximum airspeed offset in the payload. +#define OFFSET_ENCODER 29 /// the altitude-encoder-offset offset in the payload. +#define OFFSET_RSVD2 31 /// the second reserved field offset in the payload. +#define OFFSET_CONFIG 33 /// the configuration offset in the payload. +#define OFFSET_RSVD3 34 /// the third reserved field offset in the payload. + +#define REG_LEN 7 /// the registration field length. +/* + * Documented in the header file. + */ +bool sgEncodeInstall(uint8_t *buffer, sg_install_t *stl, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_INSTALL; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_INSTALL; + + // populate icao address + icao2Buf(&buffer[PBASE + OFFSET_ICAO], stl->icao); + + // populate aircraft registration + charArray2Buf(&buffer[PBASE + OFFSET_REG], stl->reg, REG_LEN); + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD1], 0); + + // populate COM port 0, correct enum offset + buffer[PBASE + OFFSET_COM0] = stl->com0; + + // populate COM port 1, correct enum offset + buffer[PBASE + OFFSET_COM1] = stl->com1; + + // populate IP address + uint322Buf(&buffer[PBASE + OFFSET_IP], stl->eth.ipAddress); + + // populate net mask + uint322Buf(&buffer[PBASE + OFFSET_MASK], stl->eth.subnetMask); + + // populate port number + uint162Buf(&buffer[PBASE + OFFSET_PORT], stl->eth.portNumber); + + // populate gps integrity + buffer[PBASE + OFFSET_GPS] = stl->sil << 4 | + stl->sda; + + // populate emitter category set and type + uint8_t emitSet; + uint8_t emitType; + + if (stl->emitter < SG_EMIT_OFFSET_B) { // group A + emitSet = SG_EMIT_GROUP_A; + emitType = stl->emitter - SG_EMIT_OFFSET_A; + + } else if (stl->emitter < SG_EMIT_OFFSET_C) { // group B + emitSet = SG_EMIT_GROUP_B; + emitType = stl->emitter - SG_EMIT_OFFSET_B; + + } else if (stl->emitter < SG_EMIT_OFFSET_D) { // group C + emitSet = SG_EMIT_GROUP_C; + emitType = stl->emitter - SG_EMIT_OFFSET_C; + + } else { // group D + emitSet = SG_EMIT_GROUP_D; + emitType = stl->emitter - SG_EMIT_OFFSET_D; + } + + buffer[PBASE + OFFSET_EMIT_SET] = emitSet; + buffer[PBASE + OFFSET_EMIT_TYPE] = emitType; + + // populate aircraft size + buffer[PBASE + OFFSET_SIZE] = stl->size; + + // populate max airspeed + buffer[PBASE + OFFSET_SPEED] = stl->maxSpeed; + + // populate altitude encoder offset + uint162Buf(&buffer[PBASE + OFFSET_ENCODER], stl->altOffset); + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD2], 0); + + // populate install configuration + buffer[PBASE + OFFSET_CONFIG] = stl->wowConnected << 7 | + stl->heater << 6 | + stl->airspeedTrue << 5 | + stl->hdgTrueNorth << 4 | + stl->altRes100 << 3 | + stl->antenna; + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD3], 0); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_INSTALL); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c new file mode 100644 index 0000000000..70de6c7b85 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c @@ -0,0 +1,107 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeOperating.c + * @author Jacob.Garrison + * + * @date Feb 15, 2021 + * + * This file receives a populated operating message struct and + * converts it into a operating message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_OPMSG SG_MSG_LEN_OPMSG - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_SQUAWK 0 /// the squawk code offset in the payload. +#define OFFSET_CONFIG 2 /// the mode/config offset in the payload. +#define OFFSET_EMRG_ID 3 /// the emergency flag offset in the payload. +#define OFFSET_ALT 4 /// the altitude offset in the payload. +#define OFFSET_RATE 6 /// the climb rate offset in the payload. +#define OFFSET_HEADING 8 /// the heading offset in the payload. +#define OFFSET_AIRSPEED 10 /// the airspeed offset in the payload. + +/* + * Documented in the header file. + */ +bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_OPMSG; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_OPMSG; + + // populate Squawk code + uint162Buf(&buffer[PBASE + OFFSET_SQUAWK], op->squawk); + + // populate Mode/Config + buffer[PBASE + OFFSET_CONFIG] = op->milEmergency << 5 | + op->enableXBit << 4 | + op->enableSqt << 3 | + op->savePowerUp << 2 | + op->opMode; + + // populate Emergency/Ident + buffer[PBASE + OFFSET_EMRG_ID] = op->identOn << 3 | + op->emergcType; + + // populate Altitude + uint16_t altCode = 0; + + if (op->altUseIntrnl) { + altCode = 0x8000; + + } else if (op->altHostAvlbl) { + // 100 foot encoding conversion + altCode = (op->altitude + 1200) / 100; + + if (op->altRes25) { + altCode *= 4; + } + + // 'Host altitude available' flag + altCode += 0x4000; + } + + uint162Buf(&buffer[PBASE + OFFSET_ALT], altCode); + + // populate Altitude Rate + int16_t rate = op->climbRate / 64; + + if (!op->climbValid) { + rate = 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_RATE], rate); + + // populate Heading + // conversion: heading * ( pow(2, 15) / 360 ) + uint16_t heading = op->heading * 32768 / 360; + + if (op->headingValid) { + heading += 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_HEADING], heading); + + // populate Airspeed + uint16_t airspeed = op->airspd; + + if (op->airspdValid) { + airspeed += 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_AIRSPEED], airspeed); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_OPMSG); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c new file mode 100644 index 0000000000..1cd918c1b0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c @@ -0,0 +1,63 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeTargetReq.c + * @author Jacob.Garrison + * + * @date Feb 19, 2021 + * + * This file receives a populated target request struct and + * converts it into a target request message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_TARGETREQ SG_MSG_LEN_TARGETREQ - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. + +#define OFFSET_REQ_TYPE 0 /// the adsb reporting type and transmit port offset +#define OFFSET_MAX_TARGETS 1 /// the maximum number of targets offset +#define OFFSET_ICAO 3 /// the requested target icao offset +#define OFFSET_REPORTS 6 /// the requested report type offset +/* + * Documented in the header file. + */ +bool sgEncodeTargetReq(uint8_t *buffer, sg_targetreq_t *tgt, uint8_t msgId) +{ + + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_TARGETREQ; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_TARGETREQ; + + // populate Request Type + buffer[PBASE + OFFSET_REQ_TYPE] = tgt->transmitPort << 6 | + tgt->reqType; + + // populate Max Targets + uint162Buf(&buffer[PBASE + OFFSET_MAX_TARGETS], tgt->maxTargets); + + // populate Requested ICAO + icao2Buf(&buffer[PBASE + OFFSET_ICAO], tgt->icao); + + // populated Requested Reports + buffer[PBASE + OFFSET_REPORTS] = tgt->ownship << 7 | + tgt->commA << 6 | + tgt->military << 5 | + tgt->tisb << 4 | + tgt->airRefVel << 3 | + tgt->targetState << 2 | + tgt->modeStatus << 1 | + tgt->stateVector; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_TARGETREQ); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h new file mode 100644 index 0000000000..1901305446 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h @@ -0,0 +1,325 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgUtil.h + * @author jimb + * + * @date Feb 2, 2021 + */ + +#ifndef UTIL_H +#define UTIL_H + +#include + +#ifndef swap16 +#define swap16(data) \ + ((((data) >> 8) & 0x00FF) | (((data) << 8) & 0xFF00)) +#endif + +#ifndef swap24 +#define swap24(data) \ + (((data) >> 16) | ((data)&0x00FF00) | (((data) << 16) & 0xFF0000)) +#endif + +#ifndef swap32 +#define swap32(data) \ + (((data) >> 24) | (((data)&0x00FF0000) >> 8) | (((data)&0x0000FF00) << 8) | ((data) << 24)) +#endif + +#ifndef swap64 +#define swap64(data) \ + (swap32((data & 0x00000000ffffffffULL))) << 32 | swap32(data >> 32)) +#endif + +#ifndef PI +#define PI 3.14159265359 +#endif + +#ifndef toRad +#define toRad(deg) \ + ((deg)*PI / 180.0) +#endif + +#ifndef toDeg +#define toDeg(rad) \ + ((rad)*180 / PI) +#endif + +#ifndef toMeter +#define toMeter(feet) \ + ((feet)*0.3048) +#endif + +#ifndef toFeet +#define toFeet(meter) \ + ((meter)*3.2808) +#endif + +/** + * Converts an array of bytes to a 16 bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 16 bit integer. + */ +int16_t toInt16(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 32 bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit integer. + */ +int32_t toInt32(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 16 unsigned bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 16 bit integer. + */ +uint16_t toUint16(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 24 bit unsigned integer with leading 0s. + * + * @param bytes The array of bytes to convert. + * + * @return The 24 bit unsigned integer with leading 0s. + */ +uint32_t toUint24(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 32 bit unsigned integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit unsigned integer. + */ +uint32_t toUint32(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a distance. + * + * @param bytes The array of bytes to convert. + * + * @return The distance value. + */ +double toDist(const uint8_t *bytes); + +/** + * Converts an array of bytes to a 24 bit unsigned integer with leading 0's. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit unsigned integer. + */ +uint32_t toIcao(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a lat/lon floating point number. + * + * @param bytes The array of bytes to convert. + * + * @return The lat/lon value. + */ +double toLatLon(const uint8_t bytes[]); + +/** + * Convert an array to an altitude. + * + * @param bytes The bytes to get the altitude from. + * + * @return The converted altitude. + */ +double toAlt(const uint8_t bytes[]); + +/** + * Converts an array of bytes to an airborne velocity. + * + * @param bytes The bytes to extract the velocity. + * + * @return The converted velocity. + */ +double toVel(const uint8_t bytes[]); + +/** + * Converts the array of bytes to the surface ground speed. + * + * @param bytes The bytes to extract the ground speed. + * + * @return The converted ground speed. + */ +uint8_t toGS(const uint8_t bytes[]); + +/** + * Converts the bytes into the heading value. + * + * @param bytes The bytes holding the heading value. + * + * @return The heading. + */ +double toHeading(const uint8_t bytes[]); + +/** + * Determine heading from y and x speed vectors. + * + * @param y The y speed vector. + * @param x The x speed vector. + * + * @return The resulting heading. + */ +int16_t toHeading2(double y, double x); + +/** + * Convert the array of bytes to a time of applicability (TOA). + * + * @param bytes The bytes to convert to a TOA. + * + * @return The converted TOA. + */ +float toTOA(const uint8_t bytes[]); + +/** + * Convert an array of bytes to a float + * + * @param bufferPos the address of the field's first corresponding buffer byte. + * + * @return The converted float value. + */ + +float toFloat(const uint8_t *bufferPos); + +/** + * Convert an array of bytes to a double + * + * @param bufferPos the address of the field's first corresponding buffer byte. + * + * @return The converted double value. + */ + +double toDouble(const uint8_t *bufferPos); + +/** + * Converts a uint16_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The uint16_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void uint162Buf(uint8_t *bufferPos, uint16_t value); + +/** + * Converts a int16_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The int32_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void int242Buf(uint8_t *bufferPos, int32_t value); + +/** + * Converts a uint32_t into a 24 bit host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The int32_t to be converted. + * + * no return value, three buffer bytes are filled by reference + */ +void uint242Buf(uint8_t *bufferPos, uint32_t value); + +/** + * Converts a uint32_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The uint32_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void uint322Buf(uint8_t *bufferPos, uint32_t value); + +/** + * Converts a uint32_t containing an ICAO into its 24-bit host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param icao The uint32_t ICAO to be converted. + * + * no return value, three buffer bytes are filled by reference + * + * @warning icao parameter must be between 0x000000 and 0xFFFFFF + */ +void icao2Buf(uint8_t *bufferPos, uint32_t icao); + +/** + * Converts an array of characters into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param arr[] The array of characters. + * @param len The number of characters in the array. + * + * no return value, the specified quantity of buffer bytes are filled by reference + */ +void charArray2Buf(uint8_t *bufferPos, char arr[], uint8_t len); + +/** + * Converts a float into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The float to be converted. + * + * no return value, four buffer bytes are filled by reference + * + * @warning The output of this function depends on the machine's endianness. It is designed + * for Little-Endian machines, only. + */ +void float2Buf(uint8_t *bufferPos, float value); + +/** + * Converts a double into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte + * @param value The double to be converted + * + * no return value, eight buffer bytes are filled by reference + * + * @warning The output of this function depends on the machine's endianness. It is designed + * for Little-Endian machines, only + */ +void double2Buf(uint8_t *bufferPos, double value); + +/** + * Converts a double into an encoded lat/lon buffer format. + * + * @param bytes address of the field's first corresponding buffer byte + * @param value the double to be converted. + * + * no return value, 3 buffer bytes are filled by reference. + */ +void latLon2Buf(uint8_t bytes[], double value); + +/** + * Calculate checksum for a host message. + * + * @param buffer The raw message buffer. + * @param len The total quantity of bytes in the buffer + * + * @return The resulting checksum. + */ +uint8_t calcChecksum(uint8_t *buffer, uint8_t len); + +/** + * Add the checksum to a host message. + * + * @param buffer The raw message buffer. + * @param len The total quantity of bytes in the buffer + * + * no return value, final buffer byte is set to the checksum value. + */ +void appendChecksum(uint8_t *buffer, uint8_t len); + +#endif /* UTIL_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c new file mode 100644 index 0000000000..d1d51ad65d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c @@ -0,0 +1,127 @@ +/** + * @copyright Copyright (c) 2020 SoftSolutions, Inc. All rights reserved. + * + * @File: target.c + * @Author: jim billmeyer + * + * @date December 11, 2020, 12:50 AM + */ + +#include +#include "target.h" + +static ownship_t ownship; +static target_t targets[XPNDR_ADSB_TARGETS] = { + { + 0, + }, +}; + +/* + * Documented in the header file. + */ +target_t *targetList(void) +{ + return targets; +} + +/* + * Documented in the header file. + */ +ownship_t *targetOwnship(void) +{ + return &ownship; +} + +/* + * Documented in the header file. + */ +target_t *targetFind(uint32_t icao) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + if (icao == targets[i].icao) { + // clear strike counter and set find flag while preserving the used bit. + targets[i].flag = TARGET_FLAG_FOUND | TARGET_FLAG_USED; + return &targets[i]; + } + } + + return 0; +} + +/* + * Documented in the header file. + */ +void targetPurge(void) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + // the the found flag was not set increment the strike counter. + if ((targets[i].flag & TARGET_FLAG_USED) && ((targets[i].flag & TARGET_FLAG_FOUND) == 0)) { + uint8_t strikes = (targets[i].flag & 0xFE) >> 2; + strikes++; + + if (strikes > 5) { + memset(&targets[i], 0, sizeof(target_t)); + + } else { + // set the strike counter and clear the found flag. + targets[i].flag = strikes << 2 | TARGET_FLAG_USED; + } + + } else { + // clear the found flag so the target find function can set it + // to signal that the icao address is still in range. + targets[i].flag = targets[i].flag & (TARGET_FLAG_STRIKE_MASK | TARGET_FLAG_USED); + } + } +} + +/* + * Documented in the header file. + */ +void targetAdd(target_t *target) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + if ((targets[i].flag & TARGET_FLAG_USED) == 0x0) { + memcpy(&targets[i], target, sizeof(target_t)); + targets[i].flag = TARGET_FLAG_USED; + break; + } + } +} + +/* + * Documented in the header file. + */ +targetclimb_t targetClimb(int16_t vrate) +{ + if (abs(vrate) < 500) { + return trafLevel; + + } else if (vrate > 0) { + return trafClimb; + + } else { + return trafDescend; + } +} + +/* + * Documented in the header file. + */ +targetalert_t targetAlert(double dist, + uint16_t alt, + int16_t nvel, + int16_t evel) +{ + if (alt <= 3000) { + if (dist <= 3.0) { + return trafResolution; + + } else if (dist <= 6.0) { + return trafAdvisory; + } + } + + return trafTraffic; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h new file mode 100644 index 0000000000..a06961f421 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h @@ -0,0 +1,128 @@ +/** + * @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved. + * + * @File: target.h + * @Author: jim billmeyer + * + * @date December 11, 2020, 12:49 AM + */ + +#ifndef TARGET_H +#define TARGET_H + +#include +#include +#include + +#define XPNDR_ADSB_TARGETS 400 // change this to the max number of +// target supported in the system. + +typedef enum { + trafLevel, + trafClimb, + trafDescend +} targetclimb_t; + +typedef enum { + trafTraffic, + trafAdvisory, + trafResolution +} targetalert_t; + +// bit 0 - target found flag. +// bit 1 - target slot in use. +// bits 2-7 - the strike counter. +#define TARGET_FLAG_FOUND 0x01 +#define TARGET_FLAG_USED 0x02 +#define TARGET_FLAG_STRIKE_MASK 0xFC + +typedef struct __attribute__((packed)) +{ + uint32_t icao; + bool airborne; + float bearing; + uint8_t distance; + int8_t altDiff; + int16_t nvel; + int16_t evel; + targetclimb_t climb; + targetalert_t alert; +#ifdef TARGET_SVR + msg_svr_t svr; +#endif + uint8_t flag; // used internally to purge stale targets. +} target_t; + +typedef struct { + uint32_t icao; + bool airborne; + float lat; + float lon; + int32_t alt; + int16_t heading; + uint16_t speed; +} ownship_t; + +/** + * Gets the target list. + * + * @return The array of traffic targets. + */ +target_t *targetList(void); + +/** + * Gets the ownship target information. + * + * @return The ownship target info. + */ +ownship_t *targetOwnship(void); + +/** + * Find a target based on its icao number. + * + * @param icao The target's icao number + * + * @return A pointer to the target element or null if not found. + */ +target_t *targetFind(uint32_t icao); + +/** + * Purge the traffic target list of stale traffic. + * + * The traffic gets purged if a find has not been done based + * on a strike counter. + */ +void targetPurge(void); + +/** + * Adds a target to the traffic target list. + * + * @param target The target to add. + */ +void targetAdd(target_t *target); + +/** + * Gets the target climb flag based on the vertical rate. + * + * @param vrate The current vertical rate of climb for the target. + * + * @return The level, climb or descend flag. + */ +targetclimb_t targetClimb(int16_t vrate); + +/** + * Gets the traffic alert flag. + * + * @param dist The distance of the target to the ownship. + * @param alt The altitude difference between the target and ownship. + * @param nvel The NS speed vector of the target. + * @param evel The EW speed vector of the target. + * + * @return The traffic flag based on the parameters. + */ +targetalert_t targetAlert(double dist, + uint16_t alt, + int16_t nvel, + int16_t evel); + +#endif /* TARGET_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c new file mode 100644 index 0000000000..188dfae4bb --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toAlt.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_ALT 0.015625 + +/* + * Documented in the header file. + */ +double toAlt(const uint8_t bytes[]) +{ + double value = toInt32(bytes); + value *= SV_RES_ALT; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c new file mode 100644 index 0000000000..66c57b94e4 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c @@ -0,0 +1,50 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toGS.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint8_t toGS(const uint8_t bytes[]) +{ + uint8_t code = bytes[0]; + float gs = 0.0f; + + if (code <= 0x01) { + gs = 0.0f; + + } else if (code <= 0x08) { + gs = 1.0f; + + } else if (code <= 0x0C) { + gs = 1.0f + (code - 0x09) * 0.25f; + + } else if (code <= 0x26) { + gs = 2.0f + (code - 0x0D) * 0.5f; + + } else if (code <= 0x5D) { + gs = 15.0f + (code - 0x27) * 1.0f; + + } else if (code <= 0x6C) { + gs = 70.0f + (code - 0x5E) * 2.0f; + + } else if (code <= 0x7B) { + gs = 100.0f + (code - 0x6D) * 5.0f; + + } else { + gs = 176.0f; + } + + // first converting to an 16 bit integer is necessary + // to keep the floating point conversion from + // truncating to 0. + return (uint8_t)((int16_t)gs & 0xFF); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c new file mode 100644 index 0000000000..a89623b512 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toHeading.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_HEAD 1.40625 + +/* + * Documented in the header file. + */ +double toHeading(const uint8_t bytes[]) +{ + double value = bytes[0]; + value *= SV_RES_HEAD; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c new file mode 100644 index 0000000000..4c65f873e7 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c @@ -0,0 +1,33 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toHeading2.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int16_t toHeading2(double y, double x) +{ + int16_t heading = (toDeg(atan2(y, x)) + 0.5); + heading = 360 - heading + 90; // atan is ccw 0 degrees at x = 1 and y = 0. + + if (heading > 360) { + heading -= 360; + + } else { + while (heading < 0) { + heading += 360; + } + } + + return heading; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c new file mode 100644 index 0000000000..cc683e19cf --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toIcao.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint32_t toIcao(const uint8_t bytes[]) +{ + uint32_t icao = (0 << 24) | ((uint32_t)bytes[0] << 16) | ((uint32_t)bytes[1] << 8) | ((uint32_t)bytes[2]); + + return icao; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c new file mode 100644 index 0000000000..0bf4842db8 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toInt16.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int16_t toInt16(const uint8_t bytes[]) +{ + int16_t int16 = ((int16_t)bytes[0] << 8) | bytes[1]; + + return int16; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c new file mode 100644 index 0000000000..555844cc10 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toInt32.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int32_t toInt32(const uint8_t bytes[]) +{ + int32_t int32 = ((int32_t)bytes[0] << 24) | ((int32_t)bytes[1] << 16) | ((int32_t)bytes[2] << 8); + int32 = int32 >> 8; + + return int32; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c new file mode 100644 index 0000000000..bc09e9ee49 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toLatLon.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_LATLON 180.0 / 8388608.0 // 180 degrees / 2^23 + +/* + * Documented in the header file. + */ +double toLatLon(const uint8_t bytes[]) +{ + double value = toInt32(bytes); + value *= SV_RES_LATLON; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c new file mode 100644 index 0000000000..557fe31774 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c @@ -0,0 +1,20 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toTOA.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +float toTOA(const uint8_t bytes[]) +{ + float toa = toInt16(bytes) & 0xFFFF; + return toa / (float) 128.0; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c new file mode 100644 index 0000000000..9b627bd0fd --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toUint16.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint16_t toUint16(const uint8_t bytes[]) +{ + uint16_t uint16 = ((uint16_t) bytes[0] << 8 | (uint16_t) bytes[1]); + + return uint16; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c new file mode 100644 index 0000000000..0f869ea755 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toUint32.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint32_t toUint32(const uint8_t bytes[]) +{ + uint32_t uint32 = ((uint32_t) bytes[0] << 24) | ((uint32_t) bytes[1] << 16) | ((uint32_t) bytes[2] << 8) | (( + uint32_t) bytes[3]); + + return uint32; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c new file mode 100644 index 0000000000..c21fe14272 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toVel.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_VEL 0.125 + +/* + * Documented in the header file. + */ +double toVel(const uint8_t bytes[]) +{ + double value = toInt16(bytes); + value *= SV_RES_VEL; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c new file mode 100644 index 0000000000..775d76057c --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c @@ -0,0 +1,20 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file uint162Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void uint162Buf(uint8_t *bufferPos, uint16_t value) +{ + bufferPos[0] = value >> 8; + bufferPos[1] = value & 0xFF; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c new file mode 100644 index 0000000000..f2e0986e4d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file uint322Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void uint322Buf(uint8_t *bufferPos, uint32_t value) +{ + bufferPos[0] = (value & 0xFF000000) >> 24; + bufferPos[1] = (value & 0x00FF0000) >> 16; + bufferPos[2] = (value & 0x0000FF00) >> 8; + bufferPos[3] = (value & 0x000000FF); +}