aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_ADSB: Added Sagtetech MXS Driver and it's SDK

Browse Source
This commit is contained in:
Chuck Faber 2022-06-01 11:29:17 -07:00 committed by Tom Pittenger
parent 0645aee865
commit c0b2b679a1
41 changed files with 4430 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_ADSB/AP_ADSB.cpp
View File

@ -26,6 +26,7 @@
#include "AP_ADSB_uAvionix_MAVLink.h"
#include "AP_ADSB_uAvionix_UCP.h"
#include "AP_ADSB_Sagetech.h"
#include "AP_ADSB_Sagetech_MXS.h"
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
@ -55,7 +56,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Param: TYPE
// @DisplayName: ADSB Type
// @Description: Type of ADS-B hardware for ADSB-in and ADSB-out configuration and operation. If any type is selected then MAVLink based ADSB-in messages will always be enabled
// @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech,3:uAvionix-UCP
// @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech,3:uAvionix-UCP,4:Sagetech MX Series
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], 0, AP_PARAM_FLAG_ENABLE),
@ -159,7 +160,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Param: OPTIONS
// @DisplayName: ADS-B Options
// @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe
// @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config
// @User: Advanced
AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0),
@ -276,7 +277,14 @@ void AP_ADSB::detect_instance(uint8_t instance)
#if HAL_ADSB_SAGETECH_ENABLED
if (AP_ADSB_Sagetech::detect()) {
_backend[instance] = new AP_ADSB_Sagetech(*this, instance);
return;
}
#endif
break;
case Type::Sagetech_MXS:
#if HAL_ADSB_SAGETECH_MXS_ENABLED
if (AP_ADSB_Sagetech_MXS::detect()) {
_backend[instance] = new AP_ADSB_Sagetech_MXS(*this, instance);
}
#endif
break;
@ -671,7 +679,7 @@ void AP_ADSB::handle_transceiver_report(const mavlink_channel_t chan, const mavl
void AP_ADSB::send_adsb_out_status(const mavlink_channel_t chan) const
{
for (uint8_t i=0; i < ADSB_MAX_INSTANCES; i++) {
if (_type[i] == (int8_t)(AP_ADSB::Type::uAvionix_UCP)) {
if (_type[i] == (int8_t)(AP_ADSB::Type::uAvionix_UCP) || (int8_t)(AP_ADSB::Type::Sagetech_MXS)) {
mavlink_msg_uavionix_adsb_out_status_send_struct(chan, &out_state.tx_status);
return;
}

5
libraries/AP_ADSB/AP_ADSB.h
View File

@ -37,6 +37,8 @@
#define ADSB_BITBASK_RF_CAPABILITIES_UAT_IN (1 << 0)
#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN (1 << 1)
#define ADSB_BITBASK_RF_CAPABILITIES_UAT_OUT (1 << 2)
#define ADSB_BITBASK_RF_CAPABILITIES_1090ES_OUT (1 << 3)
class AP_ADSB_Backend;
@ -46,6 +48,7 @@ public:
friend class AP_ADSB_uAvionix_MAVLink;
friend class AP_ADSB_uAvionix_UCP;
friend class AP_ADSB_Sagetech;
friend class AP_ADSB_Sagetech_MXS;
// constructor
AP_ADSB();
@ -65,6 +68,7 @@ public:
uAvionix_MAVLink = 1,
Sagetech = 2,
uAvionix_UCP = 3,
Sagetech_MXS = 4,
};
struct adsb_vehicle_t {
@ -77,6 +81,7 @@ public:
Ping200X_Send_GPS = (1<<0),
Squawk_7400_FS_RC = (1<<1),
Squawk_7400_FS_GCS = (1<<2),
SagteTech_MXS_External_Config = (1<<3),
};
// for holding parameters

757
libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp Normal file
View File

@ -0,0 +1,757 @@
/*
Copyright 2022 Sagetech Avionics Inc. All rights reserved.
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.
https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf
Author: Chuck Faber, Tom Pittenger
*/
#include "AP_ADSB_Sagetech_MXS.h"
#if HAL_ADSB_SAGETECH_MXS_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_RTC/AP_RTC.h>
#include <stdio.h>
#include <time.h>
#define MXS_INIT_TIMEOUT 20000
#define SAGETECH_SCALE_CM_TO_FEET (0.0328084f)
#define SAGETECH_SCALE_FEET_TO_MM (304.8f)
#define SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC (51.4444f)
#define SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC (0.508f)
#define SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN (196.85f)
#define CLIMB_RATE_LIMIT 16448
#define SAGETECH_INSTALL_MSG_RATE 5000
#define SAGETECH_OPERATING_MSG_RATE 1000
#define SAGETECH_FLIGHT_ID_MSG_RATE 8200
#define SAGETECH_GPS_MSG_RATE_FLYING 200
#define SAGETECH_GPS_MSG_RATE_GROUNDED 1000
#define SAGETECH_TARGETREQ_MSG_RATE 1000
#define SAGETECH_HFOM_UNKNOWN (19000.0f)
#define SAGETECH_VFOM_UNKNOWN (151.0f)
#define SAGETECH_HPL_UNKNOWN (38000.0f)
bool AP_ADSB_Sagetech_MXS::detect()
{
return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
}
bool AP_ADSB_Sagetech_MXS::init()
{
_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
if (_port == nullptr) {
return false;
}
return true;
}
void AP_ADSB_Sagetech_MXS::update()
{
if (_port == nullptr) {
return;
}
// -----------------------------
// read any available data on serial port
// -----------------------------
uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_MXS_MAX_SIZE);
while (nbytes-- > 0) {
const int16_t data = _port->read();
if (data < 0) {
break;
}
parse_byte((uint8_t)data);
}
const uint32_t now_ms = AP_HAL::millis();
// -----------------------------
// handle timers for generating data
// -----------------------------
if (!mxs_state.init) {
if (!last.packet_initialize_ms || (now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE)) {
last.packet_initialize_ms = now_ms;
if (_frontend._options & uint32_t(AP_ADSB::AdsbOption::SagteTech_MXS_External_Config)) {
// request the device's configuration
send_data_req(dataInstall);
} else {
// auto configure based on autopilot's saved parameters
auto_config_operating();
auto_config_installation();
auto_config_flightid();
send_targetreq_msg();
_frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to 1090ES TX and RX
mxs_state.init = true;
}
} else if (last.packet_initialize_ms > MXS_INIT_TIMEOUT && !mxs_state.init_failed) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "ADSB Sagetech MXS: Initialization Timeout. Failed to initialize.");
mxs_state.init_failed = true;
}
// before init is done, don't run any other update() tasks
return;
}
if ((now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE) &&
(mxs_state.inst.icao != (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get() ||
mxs_state.inst.emitter != convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()) ||
mxs_state.inst.size != _frontend.out_state.cfg.lengthWidth.get() ||
mxs_state.inst.maxSpeed != convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots)
)) {
last.packet_initialize_ms = now_ms;
send_install_msg();
} else if (!last.packet_PreFlight_ms || (now_ms - last.packet_PreFlight_ms >= SAGETECH_FLIGHT_ID_MSG_RATE)) {
last.packet_PreFlight_ms = now_ms;
send_flight_id_msg();
} else if (!last.packet_Operating_ms || // Send once at boot
now_ms - last.packet_Operating_ms >= SAGETECH_OPERATING_MSG_RATE || // Send Operating Message every second
last.operating_squawk != _frontend.out_state.ctrl.squawkCode || // Or anytime Operating data changes
last.operating_squawk != _frontend.out_state.cfg.squawk_octal ||
abs(last.operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit
last.operating_rf_select != _frontend.out_state.cfg.rfSelect || // The following booleans control the MXS OpMode
last.modeAEnabled != _frontend.out_state.ctrl.modeAEnabled ||
last.modeCEnabled != _frontend.out_state.ctrl.modeCEnabled ||
last.modeSEnabled != _frontend.out_state.ctrl.modeSEnabled
) {
if (last.operating_squawk != _frontend.out_state.ctrl.squawkCode) {
last.operating_squawk = _frontend.out_state.ctrl.squawkCode;
_frontend.out_state.cfg.squawk_octal_param.set_and_notify(last.operating_squawk);
} else if (last.operating_squawk != _frontend.out_state.cfg.squawk_octal) {
last.operating_squawk = _frontend.out_state.cfg.squawk_octal;
_frontend.out_state.ctrl.squawkCode = last.operating_squawk;
}
last.operating_rf_select = _frontend.out_state.cfg.rfSelect;
last.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;
last.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;
last.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled;
last.operating_alt = _frontend._my_loc.alt;
last.packet_Operating_ms = now_ms;
send_operating_msg();
} else if (now_ms - last.packet_GPS_ms >= (_frontend.out_state.is_flying ? SAGETECH_GPS_MSG_RATE_FLYING : SAGETECH_GPS_MSG_RATE_GROUNDED)) {
last.packet_GPS_ms = now_ms;
send_gps_msg();
} else if ((now_ms - last.packet_targetReq >= SAGETECH_TARGETREQ_MSG_RATE) &&
((mxs_state.treq.icao != (uint32_t)_frontend._special_ICAO_target) || (mxs_state.treq.maxTargets != (uint16_t)_frontend.in_state.list_size_param))) {
send_targetreq_msg();
}
}
void AP_ADSB_Sagetech_MXS::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.init && sgDecodeInstall((uint8_t*)&msg, &mxs_state.inst)) {
// If not doing auto-config, get the current configuration of the MXS
// Fill out configuration parameters with preconfigured values
if (mxs_state.ack.opMode == modeOff) { // If the last ACK showed an OFF state, turn off all rfSelect bits.
_frontend.out_state.cfg.rfSelect.set_and_notify(0);
} else if (mxs_state.ack.opMode == modeStby) {
_frontend.out_state.ctrl.modeAEnabled = false;
_frontend.out_state.ctrl.modeCEnabled = false;
_frontend.out_state.ctrl.modeSEnabled = false;
_frontend.out_state.ctrl.es1090TxEnabled = false;
} else if (mxs_state.ack.opMode == modeOn) {
_frontend.out_state.ctrl.modeAEnabled = true;
_frontend.out_state.ctrl.modeCEnabled = false;
_frontend.out_state.ctrl.modeSEnabled = true;
_frontend.out_state.ctrl.es1090TxEnabled = true;
} else if (mxs_state.ack.opMode == modeAlt) {
_frontend.out_state.ctrl.modeAEnabled = true;
_frontend.out_state.ctrl.modeCEnabled = true;
_frontend.out_state.ctrl.modeSEnabled = true;
_frontend.out_state.ctrl.es1090TxEnabled = true;
}
_frontend.out_state.cfg.ICAO_id_param.set_and_notify(mxs_state.inst.icao);
_frontend.out_state.cfg.lengthWidth.set_and_notify(mxs_state.inst.size);
_frontend.out_state.cfg.emitterType.set_and_notify(convert_sg_emitter_type_to_adsb(mxs_state.inst.emitter));
_frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to UAT and 1090ES TX and RX
mxs_state.init = true;
}
break;
case MsgType::FlightID_Response: {
sg_flightid_t flightId {};
if (sgDecodeFlightId((uint8_t*) &msg, &flightId)) {
_frontend.set_callsign(flightId.flightId, false);
}
break;
}
// ADSB Messages
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::Data_Request:
case MsgType::Target_Request:
case MsgType::Mode:
case MsgType::Installation:
case MsgType::FlightID:
case MsgType::Operating:
case MsgType::GPS_Data:
case MsgType::Status_Response:
case MsgType::Version_Response:
case MsgType::Serial_Number_Response:
case MsgType::Mode_Settings:
case MsgType::Target_Summary_Report:
case MsgType::RESERVED_0x84:
case MsgType::RESERVED_0x85:
case MsgType::RESERVED_0x8D:
case MsgType::ADSB_Target_State_Report:
case MsgType::ADSB_Air_Ref_Vel_Report:
// unhandled or intended for out-bound only
break;
}
}
bool AP_ADSB_Sagetech_MXS::parse_byte(const uint8_t data)
{
switch (message_in.state) {
default:
case ParseState::WaitingFor_Start:
if (data == 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<MsgType>(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;
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) {
message_in.state = ParseState::WaitingFor_Checksum;
}
break;
case ParseState::WaitingFor_Checksum:
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;
message_in.index = 0;
handle_packet(message_in.packet);
}
break;
}
return false;
}
void AP_ADSB_Sagetech_MXS::msg_write(const uint8_t *data, const uint16_t len) const
{
if (_port != nullptr) {
_port->write(data, len);
}
}
void AP_ADSB_Sagetech_MXS::auto_config_operating()
{
// Configure the Default Operation Message Data
mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, _frontend.out_state.cfg.squawk_octal);
mxs_state.op.opMode = sg_op_mode_t::modeOff; // MXS needs to start in OFF mode to accept installation message
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)_frontend.out_state.ctrl.emergencyState; // 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
int32_t height;
if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {
mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet
} else {
mxs_state.op.altitude = 0;
}
mxs_state.op.identOn = false;
float vertRate;
if (AP::ahrs().get_vert_pos_rate(vertRate)) {
mxs_state.op.climbRate = vertRate * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;
mxs_state.op.climbValid = true;
} else {
mxs_state.op.climbValid = false;
mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;
}
const Vector2f speed = AP::ahrs().groundspeed_vector();
if (!speed.is_nan() && !speed.is_zero()) {
mxs_state.op.headingValid = true;
mxs_state.op.airspdValid = true;
} else {
mxs_state.op.headingValid = false;
mxs_state.op.airspdValid = false;
}
const uint16_t speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
double heading = wrap_360(degrees(speed.angle()));
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 AP_ADSB_Sagetech_MXS::auto_config_installation()
{
// Configure the Default Installation Message Data
mxs_state.inst.icao = (uint32_t) _frontend.out_state.cfg.ICAO_id_param;
snprintf(mxs_state.inst.reg, 8, "%-7s", "TEST01Z");
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(_frontend.out_state.cfg.emitterType.get());
mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots);
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 AP_ADSB_Sagetech_MXS::auto_config_flightid()
{
if (!strlen(_frontend.out_state.cfg.callsign)) {
snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", "TESTMXS0");
} else {
snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", _frontend.out_state.cfg.callsign);
}
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 AP_ADSB_Sagetech_MXS::handle_ack(const sg_ack_t ack)
{
if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) {
// gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: ACK: Message %d of type %02x not acknowledged.", last.msg.id, last.msg.type);
}
// System health
if (ack.failXpdr && !last.failXpdr) {
gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: Transponder Failure");
_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
}
if (ack.failSystem && !last.failSystem) {
gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: System Failure");
_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;
}
last.failXpdr = ack.failXpdr;
last.failSystem = ack.failSystem;
}
void AP_ADSB_Sagetech_MXS::handle_svr(const sg_svr_t svr)
{
if (svr.addrType != svrAdrIcaoUnknown && svr.addrType != svrAdrIcao && svr.addrType != svrAdrIcaoSurface) {
// invalid icao
return;
}
AP_ADSB::adsb_vehicle_t vehicle;
if (!_frontend.get_vehicle_by_ICAO(svr.addr, vehicle)) {
// new vehicle
memset(&vehicle, 0, sizeof(vehicle));
vehicle.info.ICAO_address = svr.addr;
}
vehicle.info.flags &= ~ADSB_FLAGS_VALID_SQUAWK;
if (svr.validity.position) {
vehicle.info.lat = (int32_t) (svr.lat * 1e7);
vehicle.info.lon = (int32_t) (svr.lon * 1e7);
vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
}
if (svr.validity.geoAlt) {
vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
vehicle.info.altitude = svr.airborne.geoAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm
vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
}
if (svr.validity.baroAlt) {
vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;
vehicle.info.altitude = svr.airborne.baroAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm
vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
}
if (svr.validity.surfSpeed) {
vehicle.info.hor_velocity = svr.surface.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s
vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
}
if (svr.validity.surfHeading) {
vehicle.info.heading = svr.surface.heading * 100;
vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
}
if (svr.validity.airSpeed) {
vehicle.info.hor_velocity = svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s
vehicle.info.heading = svr.airborne.heading * 100;
vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
}
if (svr.validity.geoVRate || svr.validity.baroVRate) {
vehicle.info.ver_velocity = svr.airborne.vrate * SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC; // Convert from ft/min to cm/s
vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
}
vehicle.last_update_ms = AP_HAL::millis();
_frontend.handle_adsb_vehicle(vehicle);
}
void AP_ADSB_Sagetech_MXS::handle_msr(const sg_msr_t msr)
{
AP_ADSB::adsb_vehicle_t vehicle;
if (!_frontend.get_vehicle_by_ICAO(msr.addr, vehicle)) {
// new vehicle is not allowed here because we don't know the lat/lng
// yet and we don't allow lat/lng of (0,0) so it will get rejected anyway
return;
}
if (strlen(msr.callsign)) {
snprintf(vehicle.info.callsign, sizeof(vehicle.info.callsign), "%-8s", msr.callsign);
vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;
} else {
vehicle.info.flags &= ~ADSB_FLAGS_VALID_CALLSIGN;
}
vehicle.last_update_ms = AP_HAL::millis();
_frontend.handle_adsb_vehicle(vehicle);
}
void AP_ADSB_Sagetech_MXS::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 AP_ADSB_Sagetech_MXS::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 = (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get();
mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());
mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();
mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots);
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 AP_ADSB_Sagetech_MXS::send_flight_id_msg()
{
if (!strlen((char*) _frontend.out_state.ctrl.callsign)) {
return;
}
snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", (char*) _frontend.out_state.ctrl.callsign);
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 AP_ADSB_Sagetech_MXS::send_operating_msg()
{
if (!_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&
!_frontend.out_state.ctrl.modeSEnabled && !_frontend.out_state.ctrl.es1090TxEnabled) {
mxs_state.op.opMode = modeStby;
}
if (_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&
_frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {
mxs_state.op.opMode = modeOn;
}
if (_frontend.out_state.ctrl.modeAEnabled && _frontend.out_state.ctrl.modeCEnabled &&
_frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {
mxs_state.op.opMode = modeAlt;
}
if ((_frontend.out_state.cfg.rfSelect & 1) == 0) {
mxs_state.op.opMode = modeOff;
}
mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, last.operating_squawk);
mxs_state.op.emergcType = (sg_emergc_t) _frontend.out_state.ctrl.emergencyState;
int32_t height;
if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {
mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet
} else {
mxs_state.op.altitude = 0;
}
float vertRate;
if (AP::ahrs().get_vert_pos_rate(vertRate)) {
mxs_state.op.climbRate = vertRate * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;
mxs_state.op.climbValid = true;
} else {
mxs_state.op.climbValid = false;
mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;
}
const Vector2f speed = AP::ahrs().groundspeed_vector();
if (!speed.is_nan() && !speed.is_zero()) {
mxs_state.op.headingValid = true;
mxs_state.op.airspdValid = true;
} else {
mxs_state.op.headingValid = false;
mxs_state.op.airspdValid = false;
}
const uint16_t speed_knots = (speed.length() * M_PER_SEC_TO_KNOTS);
const double heading = wrap_360(degrees(speed.angle()));
mxs_state.op.airspd = speed_knots;
mxs_state.op.heading = heading;
mxs_state.op.identOn = _frontend.out_state.ctrl.identActive;
_frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request
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 AP_ADSB_Sagetech_MXS::send_gps_msg()
{
sg_gps_t gps {};
const AP_GPS &ap_gps = AP::gps();
float hAcc, vAcc, velAcc;
gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown
gps.hfom = ap_gps.horizontal_accuracy(hAcc) ? hAcc : SAGETECH_HFOM_UNKNOWN; // HFOM over 18,520 specifies unknown
gps.vfom = ap_gps.vertical_accuracy(vAcc) ? vAcc : SAGETECH_VFOM_UNKNOWN; // VFOM over 150 specifies unknown
gps.nacv = sg_nacv_t::nacvUnknown;
if (ap_gps.speed_accuracy(velAcc)) {
if (velAcc >= 10.0 || velAcc < 0) {
gps.nacv = sg_nacv_t::nacvUnknown;
}
else if (velAcc >= 3.0) {
gps.nacv = sg_nacv_t::nacv10dot0;
}
else if (velAcc >= 1.0) {
gps.nacv = sg_nacv_t::nacv3dot0;
}
else if (velAcc >= 0.3) {
gps.nacv = sg_nacv_t::nacv1dot0;
}
else { //if (velAcc >= 0.0)
gps.nacv = sg_nacv_t::nacv0dot3;
}
}
// Get Vehicle Longitude and Latitude and Convert to string
const int32_t longitude = _frontend._my_loc.lng;
const int32_t latitude = _frontend._my_loc.lat;
const double lon_deg = longitude * (double)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 * (double)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 Vector2f speed = AP::ahrs().groundspeed_vector();
const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
snprintf((char*)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
const float heading = wrap_360(degrees(speed.angle()));
snprintf((char*)&gps.grdTrack, 9, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * 1.0E4));
gps.latNorth = (latitude >= 0 ? true: false);
gps.lngEast = (longitude >= 0 ? true: false);
gps.gpsValid = (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) ? false : true; // If the status is not OK, gpsValid is false.
uint64_t time_usec;
if (AP::rtc().get_utc_usec(time_usec)) {
const time_t time_sec = time_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 + (time_usec % 1000000) * 1.0e-6);
} else {
strncpy(gps.timeOfFix, " . ", 11);
}
int32_t height;
if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, height)) {
gps.height = height * 0.01;
} else {
gps.height = 0.0;
}
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 AP_ADSB_Sagetech_MXS::send_targetreq_msg()
{
mxs_state.treq.reqType = sg_reporttype_t::reportAuto;
mxs_state.treq.transmitPort = sg_transmitport_t::transmitCom1;
mxs_state.treq.maxTargets = _frontend.in_state.list_size_param;
mxs_state.treq.icao = _frontend._special_ICAO_target.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);
}
sg_emitter_t AP_ADSB_Sagetech_MXS::convert_emitter_type_to_sg(const uint8_t emitterType) const
{
uint8_t result = SG_EMIT_OFFSET_D;
if (emitterType < 8) {
result = emitterType;
} else if (emitterType < 13 && emitterType >= 8) {
result = (emitterType - 8) + SG_EMIT_OFFSET_B;
} else if (emitterType < 16 && emitterType >= 14) {
result = (emitterType - 14) + (SG_EMIT_OFFSET_B + 6); // Subtracting 14 because SG emitter types don't include the reserved state at value 13.
} else if (emitterType < 21 && emitterType >= 16) {
result = (emitterType - 16) + SG_EMIT_OFFSET_C;
}
return (sg_emitter_t)result;
}
uint8_t AP_ADSB_Sagetech_MXS::convert_sg_emitter_type_to_adsb(const sg_emitter_t sgEmitterType) const
{
if (sgEmitterType < SG_EMIT_OFFSET_B) {
return sgEmitterType;
} else if ((sgEmitterType < (SG_EMIT_OFFSET_B + 6)) && (sgEmitterType >= SG_EMIT_OFFSET_B)) {
return (sgEmitterType - SG_EMIT_OFFSET_B) + 8;
} else if ((sgEmitterType < SG_EMIT_OFFSET_C) && (sgEmitterType >= SG_EMIT_OFFSET_B + 6)) {
return (sgEmitterType - (SG_EMIT_OFFSET_B + 6)) + 14; // Starts at UAV = 14
} else if ((sgEmitterType < SG_EMIT_OFFSET_D) && (sgEmitterType >= SG_EMIT_OFFSET_C)) {
return (sgEmitterType - SG_EMIT_OFFSET_C) + 16;
} else {
return 0;
}
}
sg_airspeed_t AP_ADSB_Sagetech_MXS::convert_airspeed_knots_to_sg(const float maxAirSpeed) const
{
const int32_t airspeed = (int) maxAirSpeed;
if (airspeed < 0) {
return sg_airspeed_t::speedUnknown;
} else if (airspeed < 75) {
return sg_airspeed_t::speed75kt;
} else if (airspeed < 150) {
return sg_airspeed_t::speed150kt;
} else if (airspeed < 300) {
return sg_airspeed_t::speed300kt;
} else if (airspeed < 600) {
return sg_airspeed_t::speed600kt;
} else if (airspeed < 1200) {
return sg_airspeed_t::speed1200kt;
} else { //if (airspeed >= 1200)
return sg_airspeed_t::speedGreater;
}
}
#endif // HAL_ADSB_SAGETECH_MXS_ENABLED

268
libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h Normal file
View File

@ -0,0 +1,268 @@
/*
Copyright 2022 Sagetech Avionics Inc. All rights reserved.
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.
https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf
Author: Chuck Faber, Tom Pittenger
*/
#pragma once
#include "AP_ADSB_Backend.h"
#ifndef HAL_ADSB_SAGETECH_MXS_ENABLED
// this feature is only enabled by default by select hardware
#define HAL_ADSB_SAGETECH_MXS_ENABLED HAL_ADSB_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL
#endif
#if HAL_ADSB_SAGETECH_MXS_ENABLED
#include "sagetech-sdk/sagetech_mxs.h"
class AP_ADSB_Sagetech_MXS : public AP_ADSB_Backend {
public:
using AP_ADSB_Backend::AP_ADSB_Backend;
/**
* @brief Performs required initialization for this instance
*
* @return true if initialization successful
*/
bool init() override;
/**
* @brief The main callback function (Called with freq of 10Hz) that sends
* appropriate message types at specific times.
*
* Read Byte from Serial Port Buffer (10Hz)
* Send installation message (every 5 seconds)
* Send Flight ID (every 8.2 s)
* Send Operating Message (every second)
* Send GPS data (flying: 5Hz, not flying: 1Hz)
*
*/
void update() override;
/**
* @brief Detect if a port is configured as Sagetech
*
* @return true
* @return false
*/
static bool detect();
private:
static const uint32_t PAYLOAD_MXS_MAX_SIZE = 255;
static const uint8_t START_BYTE = 0xAA;
static const uint8_t rf_capable_flags_default =
ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN |
ADSB_BITBASK_RF_CAPABILITIES_1090ES_OUT;
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;
/**
* @brief Given the dataReqType, send the appropriate data request message
*
* @param dataReqType
*/
void send_data_req(const sg_datatype_t dataReqType);
/**
* @brief Takes incoming packets, gets their message type, and
* appropriately handles them with the correct callbacks.
*
* @param msg Message packet received, cast into Packet type.
*/
void handle_packet(const Packet &msg);
/**
* @brief Sends data received from ADSB State Vector Report to AutoPilot
*
* @param svr
*/
void handle_svr(const sg_svr_t svr);
/**
* @brief Handle a received ADSB mode status report and updates the vehicle list
*
* @param msr Sagetech SDK Mode Status Report type
*/
void handle_msr(const sg_msr_t msr);
/**
* @brief Handles an incoming byte and processes it through the state
* machine to determine if end of message is reached.
*
* @param data : incoming byte
* @return false : if not yet reached packet termination
*/
bool parse_byte(const uint8_t data);
/**
* @brief Takes a raw buffer and writes it out to the device port.
*
* @param data : pointer to data buffer
* @param len : number of bytes to write
*/
void msg_write(const uint8_t *data, const uint16_t len) const;
/**
* @brief Callback for sending an installation message.
*
*/
void send_install_msg();
/**
* @brief Callback for sending a FlightID message
*
*/
void send_flight_id_msg();
/**
* @brief Callback for sending an operating message.
*
*/
void send_operating_msg();
/**
* @brief Callback for sending a GPS data message
*
*/
void send_gps_msg();
/**
* @brief Callback for sending a Target Request message
*
*/
void send_targetreq_msg();
/**
* @brief Convert the AP_ADSB uint8_t Emitter Type to the Sagetech Emitter Type definition
*
* @param emitterType
* @return sg_emitter_t
*/
sg_emitter_t convert_emitter_type_to_sg(const uint8_t emitterType) const;
/**
* @brief Convert the float maxAirSpeed value to the Sagetech Max Airspeed Type
*
* @param maxAirSpeed
* @return sg_airspeed_t
*/
sg_airspeed_t convert_airspeed_knots_to_sg(const float maxAirSpeed) const;
/**
* @brief Converts a Sagetech Emitter type value to the values used by ADSB.
*
* @return uint8_t
*/
uint8_t convert_sg_emitter_type_to_adsb(const sg_emitter_t sgEmitterType) const;
void auto_config_operating();
void auto_config_installation();
void auto_config_flightid();
void handle_ack(const sg_ack_t ack);
struct {
// timers for each out-bound packet
uint32_t packet_initialize_ms;
uint32_t packet_PreFlight_ms;
uint32_t packet_GPS_ms;
uint32_t packet_Operating_ms;
uint32_t packet_targetReq;
// cached variables to compare against params so we can send msg on param change.
uint16_t operating_squawk;
int32_t operating_alt;
uint8_t operating_rf_select;
bool modeAEnabled;
bool modeCEnabled;
bool modeSEnabled;
bool failXpdr;
bool failSystem;
uint8_t callsign[8];
struct {
uint8_t id;
uint8_t type;
} msg;
} last;
struct {
bool init;
bool init_failed;
sg_operating_t op;
sg_install_t inst;
sg_targetreq_t treq;
sg_flightid_t fid;
sg_ack_t ack;
} mxs_state;
};
#endif // HAL_ADSB_SAGETECH_MXS_ENABLED

201
libraries/AP_ADSB/sagetech-sdk/LICENSE Normal file
View File

@ -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.

6
libraries/AP_ADSB/sagetech-sdk/README.md Normal file
View File

@ -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)

19
libraries/AP_ADSB/sagetech-sdk/appendChecksum.c Normal file
View File

@ -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);
}

27
libraries/AP_ADSB/sagetech-sdk/calcChecksum.c Normal file
View File

@ -0,0 +1,27 @@
/**
* @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;
}

23
libraries/AP_ADSB/sagetech-sdk/charArray2Buf.c Normal file
View File

@ -0,0 +1,23 @@
/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file charArray2Buf.c
* @author Jacob.Garrison
*
* @date Mar 2, 2021
*
*/
#include "sgUtil.h"
#include <ctype.h>
/*
* 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]);
}
}

32
libraries/AP_ADSB/sagetech-sdk/float2Buf.c Normal file
View File

@ -0,0 +1,32 @@
/**
* @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];
}
}

21
libraries/AP_ADSB/sagetech-sdk/icao2Buf.c Normal file
View File

@ -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);
}

13
libraries/AP_ADSB/sagetech-sdk/sagetech_mxs.h Normal file
View File

@ -0,0 +1,13 @@
#pragma once
#ifdef __cplusplus
extern "C"
{
#endif
#include "sg.h"
#include "sgUtil.h"
#ifdef __cplusplus
}
#endif

954
libraries/AP_ADSB/sagetech-sdk/sg.h Normal file
View File

@ -0,0 +1,954 @@
/**
* @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 <stdint.h>
#include <stdbool.h>
/// 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 */

74
libraries/AP_ADSB/sagetech-sdk/sgDecodeAck.c Normal file
View File

@ -0,0 +1,74 @@
/**
* @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 <string.h>
#include <stdbool.h>
#include <math.h>
#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;
}

41
libraries/AP_ADSB/sagetech-sdk/sgDecodeFlightId.c Normal file
View File

@ -0,0 +1,41 @@
/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file sgDecodeFlightId.c
* @author Jacob.Garrison
*
* @date Mar 10, 2021
*
*/
#include <string.h>
#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;
}

84
libraries/AP_ADSB/sagetech-sdk/sgDecodeInstall.c Normal file
View File

@ -0,0 +1,84 @@
/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file sgDecodeInstall.c
* @author Jacob.Garrison
*
* @date Mar 9, 2021
*
*/
#include <string.h>
#include <stdbool.h>
#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;
}

203
libraries/AP_ADSB/sagetech-sdk/sgDecodeMSR.c Normal file
View File

@ -0,0 +1,203 @@
/**
* @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved.
*
* @file sgDecodeMSR.c
* @author jim.billmeyer
*
* @date Mar 17, 2021
*/
#include "sg.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#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;
}

242
libraries/AP_ADSB/sagetech-sdk/sgDecodeSVR.c Normal file
View File

@ -0,0 +1,242 @@
/**
* @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 <string.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#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;
}

51
libraries/AP_ADSB/sagetech-sdk/sgEncodeDataReq.c Normal file
View File

@ -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 <stdbool.h>
#include <stdlib.h>
#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;
}

47
libraries/AP_ADSB/sagetech-sdk/sgEncodeFlightId.c Normal file
View File

@ -0,0 +1,47 @@
/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file sgEncodeFlightId.c
* @author Jacob.Garrison
*
* @date Feb 25, 2021
*
*/
#include <ctype.h>
#include <stdbool.h>
#include <stdlib.h>
#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;
}

92
libraries/AP_ADSB/sagetech-sdk/sgEncodeGPS.c Normal file
View File

@ -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 <stdbool.h>
#include <stdlib.h>
#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;
}

134
libraries/AP_ADSB/sagetech-sdk/sgEncodeInstall.c Normal file
View File

@ -0,0 +1,134 @@
/**
* @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 <ctype.h>
#include <string.h>
#include <stdbool.h>
#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;
}

105
libraries/AP_ADSB/sagetech-sdk/sgEncodeOperating.c Normal file
View File

@ -0,0 +1,105 @@
/**
* @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 <stdbool.h>
#include <stdlib.h>
#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;
}

63
libraries/AP_ADSB/sagetech-sdk/sgEncodeTargetReq.c Normal file
View File

@ -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 <stdbool.h>
#include <stdlib.h>
#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;
}

325
libraries/AP_ADSB/sagetech-sdk/sgUtil.h Normal file
View File

@ -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 <stdint.h>
#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 */

142
libraries/AP_ADSB/sagetech-sdk/target.c Normal file
View File

@ -0,0 +1,142 @@
/**
* @copyright Copyright (c) 2020 SoftSolutions, Inc. All rights reserved.
*
* @File: target.c
* @Author: jim billmeyer
*
* @date December 11, 2020, 12:50 AM
*/
#include <stdlib.h>
#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;
}

131
libraries/AP_ADSB/sagetech-sdk/target.h Normal file
View File

@ -0,0 +1,131 @@
/**
* @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 <string.h>
#include <stdbool.h>
#include <stdint.h>
#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 */

24
libraries/AP_ADSB/sagetech-sdk/toAlt.c Normal file
View File

@ -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;
}

58
libraries/AP_ADSB/sagetech-sdk/toGS.c Normal file
View File

@ -0,0 +1,58 @@
/**
* @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);
}

24
libraries/AP_ADSB/sagetech-sdk/toHeading.c Normal file
View File

@ -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;
}

36
libraries/AP_ADSB/sagetech-sdk/toHeading2.c Normal file
View File

@ -0,0 +1,36 @@
/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file toHeading2.c
* @author Jacob.Garrison
*
* @date Mar 2, 2021
*
*/
#include <math.h>
#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;
}

21
libraries/AP_ADSB/sagetech-sdk/toIcao.c Normal file
View File

@ -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;
}

21
libraries/AP_ADSB/sagetech-sdk/toInt16.c Normal file
View File

@ -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;
}

22
libraries/AP_ADSB/sagetech-sdk/toInt32.c Normal file
View File

@ -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;
}

24
libraries/AP_ADSB/sagetech-sdk/toLatLon.c Normal file
View File

@ -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;
}

20
libraries/AP_ADSB/sagetech-sdk/toTOA.c Normal file
View File

@ -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 / 128.0;
}

21
libraries/AP_ADSB/sagetech-sdk/toUint16.c Normal file
View File

@ -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;
}

21
libraries/AP_ADSB/sagetech-sdk/toUint32.c Normal file
View File

@ -0,0 +1,21 @@
/**
* @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;
}

24
libraries/AP_ADSB/sagetech-sdk/toVel.c Normal file
View File

@ -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;
}

20
libraries/AP_ADSB/sagetech-sdk/uint162Buf.c Normal file
View File

@ -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;
}

22
libraries/AP_ADSB/sagetech-sdk/uint322Buf.c Normal file
View File

@ -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);
}