fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(mavlink): ESTIMATOR_SENSOR_FUSION_STATUS stream

Browse Source 
Add MAVLink stream that maps EstimatorFusionControl uORB message to
ESTIMATOR_SENSOR_FUSION_STATUS, exposing per-sensor intended/active
bitmasks to the GCS.
This commit is contained in:
Marco Hauswirth 2026-04-01 16:44:27 +02:00 committed by Marco Hauswirth
parent 6306c78f79
commit 6a7e39aa64
4 changed files with 148 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF2.cpp
View File

@ -2037,7 +2037,7 @@ void EKF2::PublishFusionControl(const hrt_abstime &timestamp)
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)
{

9
src/modules/mavlink/mavlink_main.cpp
View File

@ -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)

6
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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<MavlinkStreamEstimatorStatus>(),
#endif // ESTIMATOR_STATUS_HPP
#if defined(ESTIMATOR_SENSOR_FUSION_STATUS_HPP)
create_stream_list_item<MavlinkStreamEstimatorSensorFusionStatus>(),
#endif // ESTIMATOR_SENSOR_FUSION_STATUS_HPP
#if defined(VIBRATION_HPP)
create_stream_list_item<MavlinkStreamVibration>(),
#endif // VIBRATION_HPP

132
src/modules/mavlink/streams/ESTIMATOR_SENSOR_FUSION_STATUS.hpp Normal file
View File

@ -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 <uORB/topics/estimator_fusion_control.h>
#include <cmath>
/**
* 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