diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index dc0cb16b2ec8dc..02570a23050f1a 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -176,6 +176,13 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @User: Advanced AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0), + // @Param: MODE3_ONLY + // @DisplayName: Transmit in Mode 3A/C only + // @Description: Transmit in traditional Mode 3A/C only and inhibit Mode-S and ES (ADSB) transmissions. This is useful when an ATC (Air Traffic control) requests it in certain scenarios. + // @Values: 0:Transmit all Modes (3A 3C S ES), 1:Transmit Mode 3A/C only (no ADSB). + // @User: Advanced + AP_GROUPINFO("MODE3_ONLY", 16, AP_ADSB, out_state.cfg.mode3AC_only, 0), + AP_GROUPEND }; @@ -958,6 +965,26 @@ bool AP_ADSB::Loc::vertical_accuracy(float &vacc) const return true; } +uint8_t AP_ADSB::convert_maxknots_to_enum(const float maxAircraftSpeed_knots) +{ + if (maxAircraftSpeed_knots <= 0) { + // not set or unknown. no bits set + return 0; + } else if (maxAircraftSpeed_knots <= 75) { + return 1; + } else if (maxAircraftSpeed_knots <= 150) { + return 2; + } else if (maxAircraftSpeed_knots <= 300) { + return 3; + } else if (maxAircraftSpeed_knots <= 600) { + return 4; + } else if (maxAircraftSpeed_knots <= 1200) { + return 5; + } else { + return 6; + } +} + AP_ADSB *AP::ADSB() { return AP_ADSB::get_singleton(); diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 0ebf1b1ce413cc..e05d5af16aee08 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -207,6 +207,8 @@ class AP_ADSB { // confirm a value is a valid callsign static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED; + static uint8_t convert_maxknots_to_enum(const float maxAircraftSpeed_knots); + // Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value // stored on a GCS as a string field in different format, but then transmitted // over mavlink as a float which is always a decimal. @@ -311,6 +313,7 @@ class AP_ADSB { float maxAircraftSpeed_knots; AP_Int8 rf_capable; bool was_set_externally; + AP_Int8 mode3AC_only; } cfg; struct { diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp index 67d3e73e25d8ce..de259927156b60 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp @@ -120,7 +120,7 @@ void AP_ADSB_Sagetech_MXS::update() (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) + mxs_state.inst.maxSpeed != (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots) )) { last.packet_initialize_ms = now_ms; send_install_msg(); @@ -150,7 +150,7 @@ void AP_ADSB_Sagetech_MXS::update() 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.modeSEnabled = _frontend.out_state.cfg.mode3AC_only ? 0 : _frontend.out_state.ctrl.modeSEnabled; last.operating_alt = _frontend._my_loc.alt; last.packet_Operating_ms = now_ms; @@ -358,7 +358,7 @@ void AP_ADSB_Sagetech_MXS::auto_config_installation() 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.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_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; @@ -511,7 +511,7 @@ void AP_ADSB_Sagetech_MXS::send_install_msg() 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.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots); mxs_state.inst.antenna = sg_antenna_t::antBottom; last.msg.type = SG_MSG_TYPE_HOST_INSTALL; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp index bb510a738aa1d2..1a574ed23b2ebb 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp @@ -177,22 +177,7 @@ uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char() uint8_t encoded_null = 0; - if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) { - // not set or unknown. no bits set - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) { - encoded_null |= 0x01; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) { - encoded_null |= 0x02; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) { - encoded_null |= 0x03; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) { - encoded_null |= 0x04; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) { - encoded_null |= 0x05; - } else { - encoded_null |= 0x06; - } - + encoded_null = AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots); if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) { encoded_null |= 0x10; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 0f92293c519f24..7f426985135e01 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -411,8 +411,8 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled; msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled; - msg.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled; - msg.es1090TxEnabled = _frontend.out_state.ctrl.es1090TxEnabled; + msg.modeSEnabled = _frontend.out_state.cfg.mode3AC_only ? 0 : _frontend.out_state.ctrl.modeSEnabled; + msg.es1090TxEnabled = _frontend.out_state.cfg.mode3AC_only ? 0 : _frontend.out_state.ctrl.es1090TxEnabled; // if enabled via param ADSB_OPTIONS, use squawk 7400 while in any Loss-Comms related failsafe // https://www.faa.gov/documentLibrary/media/Notice/N_JO_7110.724_5-2-9_UAS_Lost_Link_2.pdf