diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3061eb2a4f..17d3b6e0ec 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2037,7 +2037,7 @@ void EKF2::PublishFusionControl(const hrt_abstime ×tamp) msg.rng_active = cs.rng_hgt; msg.mag_active = cs.mag; msg.aspd_active = cs.fuse_aspd; - // msg.rngbcn_active = cs.rngbcn_fusion; // waiting for RangeBeacon PR + msg.rngbcn_active = cs.rngbcn_fusion; #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 802bc40858..be66f307c8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1466,6 +1466,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ESC_EEPROM", unlimited_rate); #endif configure_stream_local("ESTIMATOR_STATUS", 0.5f); +#if defined(MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS) + configure_stream_local("ESTIMATOR_SENSOR_FUSION_STATUS", 0.5f); +#endif configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); @@ -1545,6 +1548,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); +#if defined(MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS) + configure_stream_local("ESTIMATOR_SENSOR_FUSION_STATUS", 1.0f); +#endif configure_stream_local("EXTENDED_SYS_STATE", 5.0f); configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f); configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); @@ -1714,6 +1720,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ESC_EEPROM", unlimited_rate); #endif configure_stream_local("ESTIMATOR_STATUS", 5.0f); +#if defined(MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS) + configure_stream_local("ESTIMATOR_SENSOR_FUSION_STATUS", 1.0f); +#endif configure_stream_local("EXTENDED_SYS_STATE", 2.0f); configure_stream_local("GLOBAL_POSITION_INT", 10.0f); #if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e05cfa99c5..8eb1f42eea 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -70,6 +70,9 @@ #include "streams/COMPONENT_METADATA.hpp" #include "streams/DISTANCE_SENSOR.hpp" #include "streams/EFI_STATUS.hpp" +#if defined(MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS) +#include "streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp" +#endif #include "streams/ESC_INFO.hpp" #include "streams/ESC_STATUS.hpp" #include "streams/ESTIMATOR_STATUS.hpp" @@ -340,6 +343,9 @@ static const StreamListItem streams_list[] = { #if defined(ESTIMATOR_STATUS_HPP) create_stream_list_item(), #endif // ESTIMATOR_STATUS_HPP +#if defined(ESTIMATOR_SENSOR_FUSION_STATUS_HPP) + create_stream_list_item(), +#endif // ESTIMATOR_SENSOR_FUSION_STATUS_HPP #if defined(VIBRATION_HPP) create_stream_list_item(), #endif // VIBRATION_HPP diff --git a/src/modules/mavlink/streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp b/src/modules/mavlink/streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp new file mode 100644 index 0000000000..859a509dab --- /dev/null +++ b/src/modules/mavlink/streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef ESTIMATOR_SENSOR_FUSION_STATUS_HPP +#define ESTIMATOR_SENSOR_FUSION_STATUS_HPP + +#include +#include + +/** + * Array index = ESTIMATOR_SENSOR_FUSION_SOURCE - 1: + * [0] GPS [1] OF [2] EV [3] AGP + * [4] BARO [5] RNG [6] MAG [7] ASPD [8] RNGBCN + * + * Each element is a per-instance bitmask (bit 0 = instance 0, etc.). + */ + +class MavlinkStreamEstimatorSensorFusionStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamEstimatorSensorFusionStatus(mavlink); } + + static constexpr const char *get_name_static() { return "ESTIMATOR_SENSOR_FUSION_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _estimator_fusion_control_sub.advertised() ? MAVLINK_MSG_ID_ESTIMATOR_SENSOR_FUSION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + // Array indices matching ESTIMATOR_SENSOR_FUSION_SOURCE - 1 + static constexpr uint8_t IDX_GPS = 0; + static constexpr uint8_t IDX_OF = 1; + static constexpr uint8_t IDX_EV = 2; + static constexpr uint8_t IDX_AGP = 3; + static constexpr uint8_t IDX_BARO = 4; + static constexpr uint8_t IDX_RNG = 5; + static constexpr uint8_t IDX_MAG = 6; + static constexpr uint8_t IDX_ASPD = 7; + static constexpr uint8_t IDX_RNGBCN = 8; + + explicit MavlinkStreamEstimatorSensorFusionStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _estimator_fusion_control_sub{ORB_ID(estimator_fusion_control)}; + + bool send() override + { + estimator_fusion_control_s fc; + + if (_estimator_fusion_control_sub.update(&fc)) { + mavlink_estimator_sensor_fusion_status_t msg{}; + + for (int i = 0; i < 9; i++) { msg.test_ratio[i] = NAN; } + + // intended: sensor enabled by user AND CTRL param not disabled + for (int i = 0; i < 2; i++) { + if (fc.gps_intended[i]) { msg.intended[IDX_GPS] |= (1u << i); } + } + + msg.intended[IDX_OF] = fc.of_intended; + msg.intended[IDX_EV] = fc.ev_intended; + msg.intended[IDX_BARO] = fc.baro_intended; + msg.intended[IDX_RNG] = fc.rng_intended; + msg.intended[IDX_MAG] = fc.mag_intended; + msg.intended[IDX_ASPD] = fc.aspd_intended; + msg.intended[IDX_RNGBCN] = fc.rngbcn_intended; + + for (int i = 0; i < 4; i++) { + if (fc.agp_intended[i]) { msg.intended[IDX_AGP] |= (1u << i); } + } + + // active: estimator is actually fusing data from this source + for (int i = 0; i < 2; i++) { + if (fc.gps_active[i]) { msg.active[IDX_GPS] |= (1u << i); } + } + + msg.active[IDX_OF] = fc.of_active; + msg.active[IDX_EV] = fc.ev_active; + + for (int i = 0; i < 4; i++) { + if (fc.agp_active[i]) { msg.active[IDX_AGP] |= (1u << i); } + } + + msg.active[IDX_BARO] = fc.baro_active; + msg.active[IDX_RNG] = fc.rng_active; + msg.active[IDX_MAG] = fc.mag_active; + msg.active[IDX_ASPD] = fc.aspd_active; + msg.active[IDX_RNGBCN] = fc.rngbcn_active; + + mavlink_msg_estimator_sensor_fusion_status_send_struct(_mavlink->get_channel(), &msg); + return true; + } + + return false; + } +}; + +#endif // ESTIMATOR_SENSOR_FUSION_STATUS_HPP