Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp
Views: 1798
/*1* Copyright (C) 2022 Sagetech Avionics Inc. All rights reserved.2*3* This file is free software: you can redistribute it and/or modify it4* under the terms of the GNU General Public License as published by the5* Free Software Foundation, either version 3 of the License, or6* (at your option) any later version.7*8* This file is distributed in the hope that it will be useful, but9* WITHOUT ANY WARRANTY; without even the implied warranty of10* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.11* See the GNU General Public License for more details.12*13* You should have received a copy of the GNU General Public License along14* with this program. If not, see <http://www.gnu.org/licenses/>.15*16* SDK specification17* https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf18*19* Authors: Chuck Faber, Tom Pittenger20*/212223#include "AP_ADSB_Sagetech_MXS.h"2425#if HAL_ADSB_SAGETECH_MXS_ENABLED26#include <GCS_MAVLink/GCS.h>27#include <AP_SerialManager/AP_SerialManager.h>28#include <stdio.h>29#include <time.h>30#include <AP_Vehicle/AP_Vehicle_Type.h>3132#define SAGETECH_USE_MXS_SDK !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)3334#define MXS_INIT_TIMEOUT 200003536#define SAGETECH_SCALE_CM_TO_FEET (0.0328084f)37#define SAGETECH_SCALE_FEET_TO_MM (304.8f)38#define SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC (51.4444f)39#define SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC (0.508f)40#define SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN (196.85f)41#define CLIMB_RATE_LIMIT 164484243#define SAGETECH_INSTALL_MSG_RATE 500044#define SAGETECH_OPERATING_MSG_RATE 100045#define SAGETECH_FLIGHT_ID_MSG_RATE 820046#define SAGETECH_GPS_MSG_RATE_FLYING 20047#define SAGETECH_GPS_MSG_RATE_GROUNDED 100048#define SAGETECH_TARGETREQ_MSG_RATE 100049#define SAGETECH_HFOM_UNKNOWN (19000.0f)50#define SAGETECH_VFOM_UNKNOWN (151.0f)51#define SAGETECH_HPL_UNKNOWN (38000.0f)5253bool AP_ADSB_Sagetech_MXS::detect()54{55return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_ADSB, 0);56}575859bool AP_ADSB_Sagetech_MXS::init()60{61_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);62if (_port == nullptr) {63return false;64}65return true;66}6768void AP_ADSB_Sagetech_MXS::update()69{70if (_port == nullptr) {71return;72}7374// -----------------------------75// read any available data on serial port76// -----------------------------77uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_MXS_MAX_SIZE);78while (nbytes-- > 0) {79uint8_t data;80if (!_port->read(data)) {81break;82}83parse_byte(data);84}8586const uint32_t now_ms = AP_HAL::millis();8788// -----------------------------89// handle timers for generating data90// -----------------------------91if (!mxs_state.init) {92if (!last.packet_initialize_ms || (now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE)) {93last.packet_initialize_ms = now_ms;9495if (_frontend._options & uint32_t(AP_ADSB::AdsbOption::SagteTech_MXS_External_Config)) {96// request the device's configuration97send_data_req(dataInstall);9899} else {100// auto configure based on autopilot's saved parameters101auto_config_operating();102auto_config_installation();103auto_config_flightid();104send_targetreq_msg();105_frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to 1090ES TX and RX106mxs_state.init = true;107}108109} else if (last.packet_initialize_ms > MXS_INIT_TIMEOUT && !mxs_state.init_failed) {110GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Sagetech MXS: Initialization Timeout. Failed to initialize.");111mxs_state.init_failed = true;112}113114// before init is done, don't run any other update() tasks115return;116}117118if ((now_ms - last.packet_initialize_ms >= SAGETECH_INSTALL_MSG_RATE) &&119(mxs_state.inst.icao != (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get() ||120mxs_state.inst.emitter != convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()) ||121mxs_state.inst.size != _frontend.out_state.cfg.lengthWidth.get() ||122mxs_state.inst.maxSpeed != (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots)123)) {124last.packet_initialize_ms = now_ms;125send_install_msg();126127} else if (!last.packet_PreFlight_ms || (now_ms - last.packet_PreFlight_ms >= SAGETECH_FLIGHT_ID_MSG_RATE)) {128last.packet_PreFlight_ms = now_ms;129send_flight_id_msg();130131} else if (!last.packet_Operating_ms || // Send once at boot132now_ms - last.packet_Operating_ms >= SAGETECH_OPERATING_MSG_RATE || // Send Operating Message every second133last.operating_squawk != _frontend.out_state.ctrl.squawkCode || // Or anytime Operating data changes134last.operating_squawk != _frontend.out_state.cfg.squawk_octal ||135abs(last.operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit136last.operating_rf_select != _frontend.out_state.cfg.rfSelect || // The following booleans control the MXS OpMode137last.modeAEnabled != _frontend.out_state.ctrl.modeAEnabled ||138last.modeCEnabled != _frontend.out_state.ctrl.modeCEnabled ||139last.modeSEnabled != _frontend.out_state.ctrl.modeSEnabled140) {141142if (last.operating_squawk != _frontend.out_state.ctrl.squawkCode) {143last.operating_squawk = _frontend.out_state.ctrl.squawkCode;144_frontend.out_state.cfg.squawk_octal_param.set_and_notify(last.operating_squawk);145} else if (last.operating_squawk != _frontend.out_state.cfg.squawk_octal) {146last.operating_squawk = _frontend.out_state.cfg.squawk_octal;147_frontend.out_state.ctrl.squawkCode = last.operating_squawk;148}149last.operating_rf_select = _frontend.out_state.cfg.rfSelect;150last.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled;151last.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled;152last.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled;153154last.operating_alt = _frontend._my_loc.alt;155last.packet_Operating_ms = now_ms;156send_operating_msg();157158} else if (now_ms - last.packet_GPS_ms >= (_frontend.out_state.is_flying ? SAGETECH_GPS_MSG_RATE_FLYING : SAGETECH_GPS_MSG_RATE_GROUNDED)) {159last.packet_GPS_ms = now_ms;160send_gps_msg();161162} else if ((now_ms - last.packet_targetReq >= SAGETECH_TARGETREQ_MSG_RATE) &&163((mxs_state.treq.icao != (uint32_t)_frontend._special_ICAO_target) || (mxs_state.treq.maxTargets != (uint16_t)_frontend.in_state.list_size_param))) {164send_targetreq_msg();165}166}167168void AP_ADSB_Sagetech_MXS::handle_packet(const Packet &msg)169{170#if SAGETECH_USE_MXS_SDK171switch (msg.type) {172case MsgType::ACK:173if(sgDecodeAck((uint8_t*) &msg, &mxs_state.ack)) {174handle_ack(mxs_state.ack);175}176break;177178case MsgType::Installation_Response:179if (!mxs_state.init && sgDecodeInstall((uint8_t*)&msg, &mxs_state.inst)) {180// If not doing auto-config, get the current configuration of the MXS181// Fill out configuration parameters with preconfigured values182if (mxs_state.ack.opMode == modeOff) { // If the last ACK showed an OFF state, turn off all rfSelect bits.183_frontend.out_state.cfg.rfSelect.set_and_notify(0);184} else if (mxs_state.ack.opMode == modeStby) {185_frontend.out_state.ctrl.modeAEnabled = false;186_frontend.out_state.ctrl.modeCEnabled = false;187_frontend.out_state.ctrl.modeSEnabled = false;188_frontend.out_state.ctrl.es1090TxEnabled = false;189} else if (mxs_state.ack.opMode == modeOn) {190_frontend.out_state.ctrl.modeAEnabled = true;191_frontend.out_state.ctrl.modeCEnabled = false;192_frontend.out_state.ctrl.modeSEnabled = true;193_frontend.out_state.ctrl.es1090TxEnabled = true;194} else if (mxs_state.ack.opMode == modeAlt) {195_frontend.out_state.ctrl.modeAEnabled = true;196_frontend.out_state.ctrl.modeCEnabled = true;197_frontend.out_state.ctrl.modeSEnabled = true;198_frontend.out_state.ctrl.es1090TxEnabled = true;199}200_frontend.out_state.cfg.ICAO_id_param.set_and_notify(mxs_state.inst.icao);201_frontend.out_state.cfg.lengthWidth.set_and_notify(mxs_state.inst.size);202_frontend.out_state.cfg.emitterType.set_and_notify(convert_sg_emitter_type_to_adsb(mxs_state.inst.emitter));203204_frontend.out_state.cfg.rf_capable.set_and_notify(rf_capable_flags_default); // Set the RF Capability to UAT and 1090ES TX and RX205206mxs_state.init = true;207}208break;209210case MsgType::FlightID_Response: {211sg_flightid_t flightId {};212if (sgDecodeFlightId((uint8_t*) &msg, &flightId)) {213_frontend.set_callsign(flightId.flightId, false);214}215break;216}217218// ADSB Messages219case MsgType::ADSB_StateVector_Report: {220sg_svr_t svr {};221if (sgDecodeSVR((uint8_t*) &msg, &svr)) {222handle_svr(svr);223}224break;225}226227case MsgType::ADSB_ModeStatus_Report: {228sg_msr_t msr {};229if (sgDecodeMSR((uint8_t*) &msg, &msr)) {230handle_msr(msr);231}232break;233}234235case MsgType::Data_Request:236case MsgType::Target_Request:237case MsgType::Mode:238case MsgType::Installation:239case MsgType::FlightID:240case MsgType::Operating:241case MsgType::GPS_Data:242case MsgType::Status_Response:243case MsgType::Version_Response:244case MsgType::Serial_Number_Response:245case MsgType::Mode_Settings:246case MsgType::Target_Summary_Report:247case MsgType::RESERVED_0x84:248case MsgType::RESERVED_0x85:249case MsgType::RESERVED_0x8D:250case MsgType::ADSB_Target_State_Report:251case MsgType::ADSB_Air_Ref_Vel_Report:252// unhandled or intended for out-bound only253break;254}255#endif // SAGETECH_USE_MXS_SDK256}257258bool AP_ADSB_Sagetech_MXS::parse_byte(const uint8_t data)259{260switch (message_in.state) {261default:262case ParseState::WaitingFor_Start:263if (data == START_BYTE) {264message_in.checksum = data; // initialize checksum here265message_in.state = ParseState::WaitingFor_MsgType;266}267break;268case ParseState::WaitingFor_MsgType:269message_in.checksum += data;270message_in.packet.type = static_cast<MsgType>(data);271message_in.state = ParseState::WaitingFor_MsgId;272break;273case ParseState::WaitingFor_MsgId:274message_in.checksum += data;275message_in.packet.id = data;276message_in.state = ParseState::WaitingFor_PayloadLen;277break;278case ParseState::WaitingFor_PayloadLen:279message_in.checksum += data;280message_in.packet.payload_length = data;281message_in.index = 0;282message_in.state = (data == 0) ? ParseState::WaitingFor_Checksum : ParseState::WaitingFor_PayloadContents;283break;284case ParseState::WaitingFor_PayloadContents:285message_in.checksum += data;286message_in.packet.payload[message_in.index++] = data;287if (message_in.index >= message_in.packet.payload_length) {288message_in.state = ParseState::WaitingFor_Checksum;289}290break;291case ParseState::WaitingFor_Checksum:292message_in.state = ParseState::WaitingFor_Start;293if (message_in.checksum == data) {294// append the checksum to the payload and zero out the payload index295message_in.packet.payload[message_in.index] = data;296message_in.index = 0;297handle_packet(message_in.packet);298}299break;300}301return false;302}303304void AP_ADSB_Sagetech_MXS::msg_write(const uint8_t *data, const uint16_t len) const305{306if (_port != nullptr) {307_port->write(data, len);308}309}310311void AP_ADSB_Sagetech_MXS::auto_config_operating()312{313// Configure the Default Operation Message Data314mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, _frontend.out_state.cfg.squawk_octal);315mxs_state.op.opMode = sg_op_mode_t::modeOff; // MXS needs to start in OFF mode to accept installation message316mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile317mxs_state.op.enableSqt = true; // Enable extended squitters318mxs_state.op.enableXBit = false; // Enable the x-bit319mxs_state.op.milEmergency = false; // Broadcast a military emergency320mxs_state.op.emergcType = (sg_emergc_t)_frontend.out_state.ctrl.emergencyState; // Enumerated civilian emergency type321322mxs_state.op.altUseIntrnl = true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field)323mxs_state.op.altHostAvlbl = false;324mxs_state.op.altRes25 = !mxs_state.inst.altRes100; // Host Altitude Resolution from install325326mxs_state.op.identOn = false;327328const auto &my_loc = _frontend._my_loc;329330populate_op_altitude(my_loc);331populate_op_climbrate(my_loc);332populate_op_airspeed_and_heading(my_loc);333334last.msg.type = SG_MSG_TYPE_HOST_OPMSG;335336#if SAGETECH_USE_MXS_SDK337uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {};338sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id);339msg_write(txComBuffer, SG_MSG_LEN_OPMSG);340#endif341}342343void AP_ADSB_Sagetech_MXS::auto_config_installation()344{345// Configure the Default Installation Message Data346mxs_state.inst.icao = (uint32_t) _frontend.out_state.cfg.ICAO_id_param;347snprintf(mxs_state.inst.reg, 8, "%-7s", "TEST01Z");348349mxs_state.inst.com0 = sg_baud_t::baud230400;350mxs_state.inst.com1 = sg_baud_t::baud57600;351352mxs_state.inst.eth.ipAddress = 0;353mxs_state.inst.eth.subnetMask = 0;354mxs_state.inst.eth.portNumber = 0;355356mxs_state.inst.sil = sg_sil_t::silUnknown;357mxs_state.inst.sda = sg_sda_t::sdaUnknown;358mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());359mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();360mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);361mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0.362mxs_state.inst.antenna = sg_antenna_t::antBottom;363364mxs_state.inst.altRes100 = true;365mxs_state.inst.hdgTrueNorth = false;366mxs_state.inst.airspeedTrue = false;367mxs_state.inst.heater = true; // Heater should always be connected.368mxs_state.inst.wowConnected = true;369370last.msg.type = SG_MSG_TYPE_HOST_INSTALL;371372#if SAGETECH_USE_MXS_SDK373uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {};374sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id);375msg_write(txComBuffer, SG_MSG_LEN_INSTALL);376#endif377}378379void AP_ADSB_Sagetech_MXS::auto_config_flightid()380{381if (!strlen(_frontend.out_state.cfg.callsign)) {382snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", "TESTMXS0");383} else {384snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", _frontend.out_state.cfg.callsign);385}386last.msg.type = SG_MSG_TYPE_HOST_FLIGHT;387388#if SAGETECH_USE_MXS_SDK389uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {};390sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id);391msg_write(txComBuffer, SG_MSG_LEN_FLIGHT);392#endif393}394395void AP_ADSB_Sagetech_MXS::handle_ack(const sg_ack_t ack)396{397if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) {398// GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: ACK: Message %d of type %02x not acknowledged.", last.msg.id, last.msg.type);399}400// System health401if (ack.failXpdr && !last.failXpdr) {402GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: Transponder Failure");403_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;404}405if (ack.failSystem && !last.failSystem) {406GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: System Failure");407_frontend.out_state.tx_status.fault |= UAVIONIX_ADSB_OUT_STATUS_FAULT_TX_SYSTEM_FAIL;408}409last.failXpdr = ack.failXpdr;410last.failSystem = ack.failSystem;411}412413void AP_ADSB_Sagetech_MXS::handle_svr(const sg_svr_t svr)414{415if (svr.addrType != svrAdrIcaoUnknown && svr.addrType != svrAdrIcao && svr.addrType != svrAdrIcaoSurface) {416// invalid icao417return;418}419420AP_ADSB::adsb_vehicle_t vehicle;421if (!_frontend.get_vehicle_by_ICAO(svr.addr, vehicle)) {422// new vehicle423memset(&vehicle, 0, sizeof(vehicle));424vehicle.info.ICAO_address = svr.addr;425}426427vehicle.info.flags &= ~ADSB_FLAGS_VALID_SQUAWK;428429if (svr.validity.position) {430vehicle.info.lat = (int32_t) (svr.lat * 1e7);431vehicle.info.lon = (int32_t) (svr.lon * 1e7);432vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;433}434if (svr.validity.geoAlt) {435vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;436vehicle.info.altitude = svr.airborne.geoAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm437vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;438}439if (svr.validity.baroAlt) {440vehicle.info.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH;441vehicle.info.altitude = svr.airborne.baroAlt * SAGETECH_SCALE_FEET_TO_MM; // Convert from feet to mm442vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;443}444if (svr.validity.surfSpeed) {445vehicle.info.hor_velocity = svr.surface.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s446vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;447}448if (svr.validity.surfHeading) {449vehicle.info.heading = svr.surface.heading * 100;450vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;451}452if (svr.validity.airSpeed) {453vehicle.info.hor_velocity = svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_CM_PER_SEC; // Convert from knots to cm/s454vehicle.info.heading = svr.airborne.heading * 100;455vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;456vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;457}458if (svr.validity.geoVRate || svr.validity.baroVRate) {459vehicle.info.ver_velocity = svr.airborne.vrate * SAGETECH_SCALE_FT_PER_MIN_TO_CM_PER_SEC; // Convert from ft/min to cm/s460vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;461}462463vehicle.last_update_ms = AP_HAL::millis();464_frontend.handle_adsb_vehicle(vehicle);465}466467void AP_ADSB_Sagetech_MXS::handle_msr(const sg_msr_t msr)468{469AP_ADSB::adsb_vehicle_t vehicle;470if (!_frontend.get_vehicle_by_ICAO(msr.addr, vehicle)) {471// new vehicle is not allowed here because we don't know the lat/lng472// yet and we don't allow lat/lng of (0,0) so it will get rejected anyway473return;474}475476if (strlen(msr.callsign)) {477snprintf(vehicle.info.callsign, sizeof(vehicle.info.callsign), "%-8s", msr.callsign);478vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;479} else {480vehicle.info.flags &= ~ADSB_FLAGS_VALID_CALLSIGN;481}482483vehicle.last_update_ms = AP_HAL::millis();484_frontend.handle_adsb_vehicle(vehicle);485}486487void AP_ADSB_Sagetech_MXS::send_data_req(const sg_datatype_t dataReqType)488{489sg_datareq_t dataReq {};490dataReq.reqType = dataReqType;491last.msg.type = SG_MSG_TYPE_HOST_DATAREQ;492493#if SAGETECH_USE_MXS_SDK494uint8_t txComBuffer[SG_MSG_LEN_DATAREQ] {};495sgEncodeDataReq(txComBuffer, &dataReq, ++last.msg.id);496msg_write(txComBuffer, SG_MSG_LEN_DATAREQ);497#else498(void)dataReq;499#endif500}501502void AP_ADSB_Sagetech_MXS::send_install_msg()503{504// MXS must be in OFF mode to change ICAO or Registration505if (mxs_state.op.opMode != modeOff) {506// GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: unable to send installation data while not in OFF mode.");507return;508}509510mxs_state.inst.icao = (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get();511mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get());512mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get();513mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots);514mxs_state.inst.antenna = sg_antenna_t::antBottom;515516last.msg.type = SG_MSG_TYPE_HOST_INSTALL;517518#if SAGETECH_USE_MXS_SDK519uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {};520sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id);521msg_write(txComBuffer, SG_MSG_LEN_INSTALL);522#endif523}524525void AP_ADSB_Sagetech_MXS::send_flight_id_msg()526{527if (!strlen((char*) _frontend.out_state.ctrl.callsign)) {528return;529}530snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", (char*) _frontend.out_state.ctrl.callsign);531532last.msg.type = SG_MSG_TYPE_HOST_FLIGHT;533534#if SAGETECH_USE_MXS_SDK535uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {};536sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id);537msg_write(txComBuffer, SG_MSG_LEN_FLIGHT);538#endif539}540541void AP_ADSB_Sagetech_MXS::send_operating_msg()542{543if (!_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&544!_frontend.out_state.ctrl.modeSEnabled && !_frontend.out_state.ctrl.es1090TxEnabled) {545mxs_state.op.opMode = modeStby;546}547if (_frontend.out_state.ctrl.modeAEnabled && !_frontend.out_state.ctrl.modeCEnabled &&548_frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {549mxs_state.op.opMode = modeOn;550}551if (_frontend.out_state.ctrl.modeAEnabled && _frontend.out_state.ctrl.modeCEnabled &&552_frontend.out_state.ctrl.modeSEnabled && _frontend.out_state.ctrl.es1090TxEnabled) {553mxs_state.op.opMode = modeAlt;554}555if ((_frontend.out_state.cfg.rfSelect & 1) == 0) {556mxs_state.op.opMode = modeOff;557}558559mxs_state.op.squawk = AP_ADSB::convert_base_to_decimal(8, last.operating_squawk);560mxs_state.op.emergcType = (sg_emergc_t) _frontend.out_state.ctrl.emergencyState;561562const auto &my_loc = _frontend._my_loc;563564populate_op_altitude(my_loc);565populate_op_climbrate(my_loc);566populate_op_airspeed_and_heading(my_loc);567568mxs_state.op.identOn = _frontend.out_state.ctrl.identActive;569_frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request570571last.msg.type = SG_MSG_TYPE_HOST_OPMSG;572573#if SAGETECH_USE_MXS_SDK574uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {};575sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id);576msg_write(txComBuffer, SG_MSG_LEN_OPMSG);577#endif578}579580void AP_ADSB_Sagetech_MXS::send_gps_msg()581{582sg_gps_t gps {};583const AP_ADSB::Loc &ap_gps { _frontend._my_loc };584float hAcc, vAcc, velAcc;585586gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown587gps.hfom = ap_gps.horizontal_accuracy(hAcc) ? hAcc : SAGETECH_HFOM_UNKNOWN; // HFOM over 18,520 specifies unknown588gps.vfom = ap_gps.vertical_accuracy(vAcc) ? vAcc : SAGETECH_VFOM_UNKNOWN; // VFOM over 150 specifies unknown589gps.nacv = sg_nacv_t::nacvUnknown;590if (ap_gps.speed_accuracy(velAcc)) {591if (velAcc >= 10.0 || velAcc < 0) {592gps.nacv = sg_nacv_t::nacvUnknown;593}594else if (velAcc >= 3.0) {595gps.nacv = sg_nacv_t::nacv10dot0;596}597else if (velAcc >= 1.0) {598gps.nacv = sg_nacv_t::nacv3dot0;599}600else if (velAcc >= 0.3) {601gps.nacv = sg_nacv_t::nacv1dot0;602}603else { //if (velAcc >= 0.0)604gps.nacv = sg_nacv_t::nacv0dot3;605}606}607608// Get Vehicle Longitude and Latitude and Convert to string609const int32_t longitude = _frontend._my_loc.lng;610const int32_t latitude = _frontend._my_loc.lat;611const double lon_deg = longitude * (double)1.0e-7 * (longitude < 0 ? -1 : 1);612const double lon_minutes = (lon_deg - int(lon_deg)) * 60;613snprintf((char*)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, unsigned((lon_minutes - (int)lon_minutes) * 1.0E5));614615const double lat_deg = latitude * (double)1.0e-7 * (latitude < 0 ? -1 : 1);616const double lat_minutes = (lat_deg - int(lat_deg)) * 60;617snprintf((char*)&gps.latitude, 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));618619const Vector2f speed = _frontend._my_loc.groundspeed_vector();620const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;621snprintf((char*)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));622623if (!is_zero(speed_knots)) {624cog = wrap_360(degrees(speed.angle()));625}626snprintf((char*)&gps.grdTrack, 9, "%03u.%04u", unsigned(cog), unsigned((cog - (int)cog) * 1.0E4));627628629gps.latNorth = (latitude >= 0 ? true: false);630gps.lngEast = (longitude >= 0 ? true: false);631632gps.gpsValid = ap_gps.status() >= AP_GPS_FixType::FIX_2D;633634uint64_t time_usec = ap_gps.epoch_from_rtc_us;635if (ap_gps.have_epoch_from_rtc_us) {636const time_t time_sec = time_usec * 1E-6;637struct tm tmd {};638struct tm* tm = gmtime_r(&time_sec, &tmd);639640snprintf((char*)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);641} else {642strncpy(gps.timeOfFix, " . ", 11);643}644645int32_t height;646if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {647gps.height = height * 0.01;648} else {649gps.height = 0.0;650}651652last.msg.type = SG_MSG_TYPE_HOST_GPS;653654#if SAGETECH_USE_MXS_SDK655uint8_t txComBuffer[SG_MSG_LEN_GPS] {};656sgEncodeGPS(txComBuffer, &gps, ++last.msg.id);657msg_write(txComBuffer, SG_MSG_LEN_GPS);658#else659(void)gps;660#endif661}662663void AP_ADSB_Sagetech_MXS::send_targetreq_msg()664{665mxs_state.treq.reqType = sg_reporttype_t::reportAuto;666mxs_state.treq.transmitPort = sg_transmitport_t::transmitCom1;667mxs_state.treq.maxTargets = _frontend.in_state.list_size_param;668mxs_state.treq.icao = _frontend._special_ICAO_target.get();669mxs_state.treq.stateVector = true;670mxs_state.treq.modeStatus = true;671mxs_state.treq.targetState = false;672mxs_state.treq.airRefVel = false;673mxs_state.treq.tisb = false;674mxs_state.treq.military = false;675mxs_state.treq.commA = false;676mxs_state.treq.ownship = true;677678last.msg.type = SG_MSG_TYPE_HOST_TARGETREQ;679680#if SAGETECH_USE_MXS_SDK681uint8_t txComBuffer[SG_MSG_LEN_TARGETREQ] {};682sgEncodeTargetReq(txComBuffer, &mxs_state.treq, ++last.msg.id);683msg_write(txComBuffer, SG_MSG_LEN_TARGETREQ);684#endif685}686687sg_emitter_t AP_ADSB_Sagetech_MXS::convert_emitter_type_to_sg(const uint8_t emitterType) const688{689uint8_t result = SG_EMIT_OFFSET_D;690691if (emitterType < 8) {692result = emitterType;693} else if (emitterType < 13 && emitterType >= 8) {694result = (emitterType - 8) + SG_EMIT_OFFSET_B;695} else if (emitterType < 16 && emitterType >= 14) {696result = (emitterType - 14) + (SG_EMIT_OFFSET_B + 6); // Subtracting 14 because SG emitter types don't include the reserved state at value 13.697} else if (emitterType < 21 && emitterType >= 16) {698result = (emitterType - 16) + SG_EMIT_OFFSET_C;699}700return (sg_emitter_t)result;701}702703uint8_t AP_ADSB_Sagetech_MXS::convert_sg_emitter_type_to_adsb(const sg_emitter_t sgEmitterType) const704{705if (sgEmitterType < SG_EMIT_OFFSET_B) {706return sgEmitterType;707} else if ((sgEmitterType < (SG_EMIT_OFFSET_B + 6)) && (sgEmitterType >= SG_EMIT_OFFSET_B)) {708return (sgEmitterType - SG_EMIT_OFFSET_B) + 8;709} else if ((sgEmitterType < SG_EMIT_OFFSET_C) && (sgEmitterType >= SG_EMIT_OFFSET_B + 6)) {710return (sgEmitterType - (SG_EMIT_OFFSET_B + 6)) + 14; // Starts at UAV = 14711} else if ((sgEmitterType < SG_EMIT_OFFSET_D) && (sgEmitterType >= SG_EMIT_OFFSET_C)) {712return (sgEmitterType - SG_EMIT_OFFSET_C) + 16;713} else {714return 0;715}716}717718sg_airspeed_t AP_ADSB_Sagetech_MXS::convert_airspeed_knots_to_sg(const float maxAirSpeed) const719{720const int32_t airspeed = (int) maxAirSpeed;721722if (airspeed < 0) {723return sg_airspeed_t::speedUnknown;724} else if (airspeed < 75) {725return sg_airspeed_t::speed75kt;726} else if (airspeed < 150) {727return sg_airspeed_t::speed150kt;728} else if (airspeed < 300) {729return sg_airspeed_t::speed300kt;730} else if (airspeed < 600) {731return sg_airspeed_t::speed600kt;732} else if (airspeed < 1200) {733return sg_airspeed_t::speed1200kt;734} else { //if (airspeed >= 1200)735return sg_airspeed_t::speedGreater;736}737}738739void AP_ADSB_Sagetech_MXS::populate_op_altitude(const AP_ADSB::Loc &loc)740{741int32_t height;742if (loc.initialised() && loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) {743mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet744} else {745mxs_state.op.altitude = 0;746}747}748749void AP_ADSB_Sagetech_MXS::populate_op_climbrate(const AP_ADSB::Loc &my_loc)750{751float vertRateD;752if (my_loc.get_vert_pos_rate_D(vertRateD)) {753// convert from down to up, and scale appropriately:754mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN;755mxs_state.op.climbValid = true;756} else {757mxs_state.op.climbValid = false;758mxs_state.op.climbRate = -CLIMB_RATE_LIMIT;759}760}761762void AP_ADSB_Sagetech_MXS::populate_op_airspeed_and_heading(const AP_ADSB::Loc &my_loc)763{764const Vector2f speed = my_loc.groundspeed_vector();765if (!speed.is_nan() && !speed.is_zero()) {766mxs_state.op.headingValid = true;767mxs_state.op.airspdValid = true;768} else {769mxs_state.op.headingValid = false;770mxs_state.op.airspdValid = false;771}772const uint16_t speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;773double heading = wrap_360(degrees(speed.angle()));774mxs_state.op.airspd = speed_knots;775mxs_state.op.heading = heading;776}777778#endif // HAL_ADSB_SAGETECH_MXS_ENABLED779780781782