diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index dc0cb16b2e..3ca9af98a9 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -958,6 +958,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 0ebf1b1ce4..de87f7d01c 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -207,6 +207,8 @@ public: // 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. diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp index 67d3e73e25..39fd88810b 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(); @@ -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 bb510a738a..1a574ed23b 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;