Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

Mavlink: GLOBAL_POSITION for aux positioning (#26307)

Browse Source 
* mavlink: add GLOBAL_POSITION message handling
- Add handler for incoming GLOBAL_POSITION MAVLink messages
- Publish received positions to aux_global_position uORB topic
- Update GLOBAL_POSITION stream to use aux_global_position topic
* correctly handle multi AGP in mavlink pub
* move from GLOBAL_POSITION to GLOBAL_POSITION_SENSOR
* mavlink: update submodule to include GLOBAL_POSITION_SENSOR in common.xml
This commit is contained in:
Marco Hauswirth
2026-02-27 17:50:45 +01:00
committed by GitHub
parent 726cb2c6eb
commit fab9874183
6 changed files with 84 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink
+1 -1
Submodule src/modules/mavlink/mavlink updated: 1ba67b3c9c...51a47ee9ae
src/modules/mavlink/mavlink_main.cpp
+3 -9
View File
@@ -1436,9 +1436,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 5.0f);
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
configure_stream_local("GLOBAL_POSITION", 5.0f);
#endif
configure_stream_local("GLOBAL_POSITION_SENSOR", 5.0f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
configure_stream_local("GNSS_INTEGRITY", 1.0f);
@@ -1779,9 +1777,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("CURRENT_MODE", 0.5f);
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
configure_stream_local("GLOBAL_POSITION", 10.0f);
#endif
configure_stream_local("GLOBAL_POSITION_SENSOR", 10.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
@@ -1847,9 +1843,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
configure_stream_local("EXTENDED_SYS_STATE", 0.5f);
configure_stream_local("GLOBAL_POSITION_INT", 2.0f);
#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
configure_stream_local("GLOBAL_POSITION", 2.0f);
#endif
configure_stream_local("GLOBAL_POSITION_SENSOR", 2.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS2_RAW", 2.0f);
configure_stream_local("GPS_RAW_INT", 2.0f);
src/modules/mavlink/mavlink_messages.cpp
+4 -6
View File
@@ -75,9 +75,7 @@
#include "streams/ESTIMATOR_STATUS.hpp"
#include "streams/EXTENDED_SYS_STATE.hpp"
#include "streams/FLIGHT_INFORMATION.hpp"
#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
#include "streams/GLOBAL_POSITION.hpp"
#endif //MAVLINK_MSG_ID_GLOBAL_POSITION
#include "streams/GLOBAL_POSITION_SENSOR.hpp"
#include "streams/GLOBAL_POSITION_INT.hpp"
#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
#include "streams/GNSS_INTEGRITY.hpp"
@@ -519,9 +517,9 @@ static const StreamListItem streams_list[] = {
#if defined(CURRENT_MODE_HPP)
create_stream_list_item<MavlinkStreamCurrentMode>(),
#endif // CURRENT_MODE_HPP
#if defined(GLOBAL_POSITION_HPP)
create_stream_list_item<MavlinkStreamGLobalPosition>(),
#endif // GLOBAL_POSITION_HPP
#if defined(GLOBAL_POSITION_SENSOR_HPP)
create_stream_list_item<MavlinkStreamGlobalPositionSensor>(),
#endif // GLOBAL_POSITION_SENSOR_HPP
};
const char *get_stream_name(const uint16_t msg_id)
src/modules/mavlink/mavlink_receiver.cpp
+29
View File
@@ -213,6 +213,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_follow_target(msg);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR:
handle_message_global_position_sensor(msg);
break;
case MAVLINK_MSG_ID_LANDING_TARGET:
handle_message_landing_target(msg);
break;
@@ -2492,6 +2496,31 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
_sensor_gps_pub.publish(gps);
}
void
MavlinkReceiver::handle_message_global_position_sensor(mavlink_message_t *msg)
{
mavlink_global_position_sensor_t global_pos;
mavlink_msg_global_position_sensor_decode(msg, &global_pos);
aux_global_position_s aux_global_position{};
const hrt_abstime now = hrt_absolute_time();
aux_global_position.timestamp = now;
aux_global_position.timestamp_sample = now;
aux_global_position.id = global_pos.id;
aux_global_position.source = global_pos.source;
aux_global_position.lat = global_pos.lat * 1e-7;
aux_global_position.lon = global_pos.lon * 1e-7;
aux_global_position.alt = global_pos.alt;
aux_global_position.lat_lon_reset_counter = 0;
aux_global_position.eph = global_pos.eph;
aux_global_position.epv = global_pos.epv;
_aux_global_position_pub.publish(aux_global_position);
}
void
MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+3 -2
View File
@@ -111,6 +111,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/velocity_limits.h>
#include <uORB/topics/aux_global_position.h>
#if !defined(CONSTRAINED_FLASH)
# include <uORB/topics/debug_array.h>
@@ -208,7 +209,7 @@ private:
void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
void handle_message_gimbal_device_information(mavlink_message_t *msg);
void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg);
void handle_message_global_position_sensor(mavlink_message_t *msg);
#if !defined(CONSTRAINED_FLASH)
void handle_message_debug(mavlink_message_t *msg);
void handle_message_debug_float_array(mavlink_message_t *msg);
@@ -329,7 +330,6 @@ private:
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
#if !defined(CONSTRAINED_FLASH)
uORB::Publication<debug_array_s> _debug_array_pub {ORB_ID(debug_array)};
uORB::Publication<debug_key_value_s> _debug_key_value_pub{ORB_ID(debug_key_value)};
@@ -339,6 +339,7 @@ private:
// ORB publications (multi)
uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti<aux_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
uORB::PublicationMulti<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
uORB::PublicationMulti<input_rc_s> _rc_pub{ORB_ID(input_rc)};
uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
src/modules/mavlink/streams/GLOBAL_POSITION.hpp → src/modules/mavlink/streams/GLOBAL_POSITION_SENSOR.hpp
+44 -40
View File
@@ -31,75 +31,79 @@
*
****************************************************************************/
#ifndef GLOBAL_POSITION_HPP
#define GLOBAL_POSITION_HPP
#ifndef GLOBAL_POSITION_SENSOR_HPP
#define GLOBAL_POSITION_SENSOR_HPP
#include <stdint.h>
#include <uORB/topics/aux_global_position.h>
class MavlinkStreamGLobalPosition : public MavlinkStream
class MavlinkStreamGlobalPositionSensor : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGLobalPosition(mavlink); }
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGlobalPositionSensor(mavlink); }
static constexpr const char *get_name_static() { return "GLOBAL_POSITION"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION; }
static constexpr const char *get_name_static() { return "GLOBAL_POSITION_SENSOR"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _aux_global_position_sub.advertised() ? (MAVLINK_MSG_ID_GLOBAL_POSITION_LEN +
return _aux_global_position_sub.advertised() ? (MAVLINK_MSG_ID_GLOBAL_POSITION_SENSOR_LEN +
MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
explicit MavlinkStreamGLobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {}
explicit MavlinkStreamGlobalPositionSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _aux_global_position_sub{ORB_ID(aux_global_position)};
uORB::SubscriptionMultiArray<aux_global_position_s, 4> _aux_global_position_sub{ORB_ID::aux_global_position};
bool send() override
{
vehicle_global_position_s pos{};
aux_global_position_s pos{};
bool sent = false;
if (_aux_global_position_sub.update(&pos)) {
mavlink_global_position_t msg{};
for (int i = 0; i < _aux_global_position_sub.size(); i++) {
if (_aux_global_position_sub[i].update(&pos)) {
mavlink_global_position_sensor_t msg{};
msg.id = UINT8_C(1);
msg.time_usec = pos.timestamp;
msg.source = GLOBAL_POSITION_UNKNOWN;
msg.flags = 0;
msg.target_system = 0;
msg.target_component = 0;
msg.id = pos.id;
msg.time_usec = pos.timestamp;
msg.source = pos.source;
if (PX4_ISFINITE(pos.lat)) {
msg.lat = static_cast<int32_t>(pos.lat * 1e7);
if (PX4_ISFINITE(pos.lat)) {
msg.lat = static_cast<int32_t>(pos.lat * 1e7);
} else {
msg.lat = INT32_MAX;
} else {
msg.lat = INT32_MAX;
}
if (PX4_ISFINITE(pos.lon)) {
msg.lon = static_cast<int32_t>(pos.lon * 1e7);
} else {
msg.lon = INT32_MAX;
}
msg.alt = pos.alt;
msg.alt_ellipsoid = pos.alt;
msg.eph = pos.eph;
msg.epv = pos.epv;
mavlink_msg_global_position_sensor_send_struct(_mavlink->get_channel(), &msg);
sent = true;
}
if (PX4_ISFINITE(pos.lon)) {
msg.lon = static_cast<int32_t>(pos.lon * 1e7);
} else {
msg.lon = INT32_MAX;
}
msg.alt = pos.alt;
msg.alt_ellipsoid = pos.alt_ellipsoid;
msg.eph = pos.eph;
msg.epv = pos.epv;
mavlink_msg_global_position_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
return sent;
}
};
#endif // GLOBAL_POSITION_HPP
#endif // GLOBAL_POSITION_SENSOR_HPP