diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1fde667ec6..f180b82387 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1594,6 +1594,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("PING", 0.1f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.0f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); + configure_stream_local("RAW_RPM", 2.0f); configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); @@ -1640,6 +1641,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("PING", 1.0f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream_local("SYS_STATUS", 5.0f); @@ -1760,6 +1762,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); configure_stream_local("PING", 1.0f); configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 10.0f); configure_stream_local("SCALED_IMU", 25.0f); configure_stream_local("SCALED_IMU2", 25.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4267339ce8..a95517d1b3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -89,6 +89,7 @@ #include #include #include +#include #include #include #include @@ -5178,6 +5179,70 @@ protected: } }; + + +class MavlinkStreamRawRpm : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamRawRpm::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "RAW_RPM"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RAW_RPM; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRawRpm(mavlink); + } + + unsigned get_size() override + { + return _rpm_sub.advertised() ? MAVLINK_MSG_ID_RAW_RPM + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + + /* do not allow top copying this class */ + MavlinkStreamRawRpm(MavlinkStreamRawRpm &) = delete; + MavlinkStreamRawRpm &operator = (const MavlinkStreamRawRpm &) = delete; + +protected: + explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + rpm_s rpm; + + if (_rpm_sub.update(&rpm)) { + mavlink_raw_rpm_t msg{}; + + msg.frequency = rpm.indicated_frequency_rpm; + + mavlink_msg_raw_rpm_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), @@ -5241,7 +5306,8 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), - create_stream_list_item() + create_stream_list_item(), + create_stream_list_item() }; const char *get_stream_name(const uint16_t msg_id)