You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
786 lines
32 KiB
786 lines
32 KiB
/* |
|
* Copyright (C) 2022 Sagetech Avionics Inc. All rights reserved. |
|
* |
|
* This file is free software: you can redistribute it and/or modify it |
|
* under the terms of the GNU General Public License as published by the |
|
* Free Software Foundation, either version 3 of the License, or |
|
* (at your option) any later version. |
|
* |
|
* This file is distributed in the hope that it will be useful, but |
|
* WITHOUT ANY WARRANTY; without even the implied warranty of |
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
|
* See the GNU General Public License for more details. |
|
* |
|
* You should have received a copy of the GNU General Public License along |
|
* with this program. If not, see <http://www.gnu.org/licenses/>. |
|
* |
|
* SDK specification |
|
* https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf |
|
* |
|
* Authors: 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> |
|
#include <AP_Vehicle/AP_Vehicle_Type.h> |
|
|
|
#define SAGETECH_USE_MXS_SDK !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) |
|
|
|
#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) |
|
{ |
|
#if SAGETECH_USE_MXS_SDK |
|
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; |
|
} |
|
#endif // SAGETECH_USE_MXS_SDK |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; |
|
sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_OPMSG); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; |
|
sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_INSTALL); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; |
|
sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_DATAREQ] {}; |
|
sgEncodeDataReq(txComBuffer, &dataReq, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_DATAREQ); |
|
#else |
|
(void)dataReq; |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; |
|
sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_INSTALL); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; |
|
sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; |
|
sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_OPMSG); |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_GPS] {}; |
|
sgEncodeGPS(txComBuffer, &gps, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_GPS); |
|
#else |
|
(void)gps; |
|
#endif |
|
} |
|
|
|
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; |
|
|
|
#if SAGETECH_USE_MXS_SDK |
|
uint8_t txComBuffer[SG_MSG_LEN_TARGETREQ] {}; |
|
sgEncodeTargetReq(txComBuffer, &mxs_state.treq, ++last.msg.id); |
|
msg_write(txComBuffer, SG_MSG_LEN_TARGETREQ); |
|
#endif |
|
} |
|
|
|
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 |
|
|
|
|