Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-19 23:28:32 -04:00
Code Issues Packages Projects Releases Wiki Activity
c0b2b679a1
ardupilot/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp
Chuck Faber c0b2b679a1 AP_ADSB: Added Sagtetech MXS Driver and it's SDK
2022-06-14 07:10:17 -07:00

758 lines
32 KiB
C++
Raw Blame History

/*
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
Reference in New Issue View Git Blame Copy Permalink