diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 1ba67b3c9c46..51a47ee9aed1 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 1ba67b3c9c46f56a644e6f4e8a4c359172ed14bc +Subproject commit 51a47ee9aed1216532f2eefbfe8d260a2e3cabcc diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 20031892973b..cbc7793e8c49 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a68c1387477b..558b3a0e3f33 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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(), #endif // CURRENT_MODE_HPP -#if defined(GLOBAL_POSITION_HPP) - create_stream_list_item(), -#endif // GLOBAL_POSITION_HPP +#if defined(GLOBAL_POSITION_SENSOR_HPP) + create_stream_list_item(), +#endif // GLOBAL_POSITION_SENSOR_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3f3577636875..f0c3268cc7f0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 50a38a8c1a99..a21a9f313c34 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -111,6 +111,7 @@ #include #include #include +#include #if !defined(CONSTRAINED_FLASH) # include @@ -208,7 +209,7 @@ class MavlinkReceiver : public ModuleParams 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 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)}; @@ -339,6 +339,7 @@ class MavlinkReceiver : public ModuleParams // ORB publications (multi) uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; uORB::PublicationMulti _gps_inject_data_pub{ORB_ID(gps_inject_data)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_SENSOR.hpp similarity index 63% rename from src/modules/mavlink/streams/GLOBAL_POSITION.hpp rename to src/modules/mavlink/streams/GLOBAL_POSITION_SENSOR.hpp index ed8a43cfdf61..835e4fae9b24 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_SENSOR.hpp @@ -31,75 +31,79 @@ * ****************************************************************************/ -#ifndef GLOBAL_POSITION_HPP -#define GLOBAL_POSITION_HPP +#ifndef GLOBAL_POSITION_SENSOR_HPP +#define GLOBAL_POSITION_SENSOR_HPP #include #include -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_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(pos.lat * 1e7); + if (PX4_ISFINITE(pos.lat)) { + msg.lat = static_cast(pos.lat * 1e7); - } else { - msg.lat = INT32_MAX; - } + } else { + msg.lat = INT32_MAX; + } - if (PX4_ISFINITE(pos.lon)) { - msg.lon = static_cast(pos.lon * 1e7); + if (PX4_ISFINITE(pos.lon)) { + msg.lon = static_cast(pos.lon * 1e7); - } else { - msg.lon = INT32_MAX; - } + } else { + msg.lon = INT32_MAX; + } - msg.alt = pos.alt; - msg.alt_ellipsoid = pos.alt_ellipsoid; + msg.alt = pos.alt; + msg.alt_ellipsoid = pos.alt; - msg.eph = pos.eph; - msg.epv = pos.epv; + msg.eph = pos.eph; + msg.epv = pos.epv; - mavlink_msg_global_position_send_struct(_mavlink->get_channel(), &msg); + mavlink_msg_global_position_sensor_send_struct(_mavlink->get_channel(), &msg); - return true; + sent = true; + } } - return false; + return sent; } }; -#endif // GLOBAL_POSITION_HPP +#endif // GLOBAL_POSITION_SENSOR_HPP