diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bc6d9d877b..101c3617a0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1587,6 +1587,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f); configure_stream_local("DEBUG_VECT", 1.0f); configure_stream_local("DISTANCE_SENSOR", 0.5f); + configure_stream_local("ESC_INFO", 1.0f); + configure_stream_local("ESC_STATUS", 1.0f); configure_stream_local("ESTIMATOR_STATUS", 0.5f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); @@ -1620,6 +1622,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ATTITUDE", 100.0f); configure_stream_local("ALTITUDE", 10.0f); configure_stream_local("DISTANCE_SENSOR", 10.0f); + configure_stream_local("ESC_INFO", 10.0f); + configure_stream_local("ESC_STATUS", 10.0f); configure_stream_local("MOUNT_ORIENTATION", 10.0f); configure_stream_local("OBSTACLE_DISTANCE", 10.0f); configure_stream_local("ODOMETRY", 30.0f); @@ -1755,6 +1759,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("DEBUG", 50.0f); configure_stream_local("DEBUG_FLOAT_ARRAY", 50.0f); configure_stream_local("DEBUG_VECT", 50.0f); + configure_stream_local("ESC_INFO", 10.0f); + configure_stream_local("ESC_STATUS", 10.0f); configure_stream_local("ESTIMATOR_STATUS", 5.0f); configure_stream_local("EXTENDED_SYS_STATE", 2.0f); configure_stream_local("GLOBAL_POSITION_INT", 10.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c0bf19006b..fea40925dc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -5314,6 +5315,181 @@ protected: } }; +class MavlinkStreamESCInfo : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamESCInfo::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "ESC_INFO"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_ESC_INFO; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamESCInfo(mavlink); + } + + unsigned get_size() override + { + static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + } + +private: + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + + /* do not allow top copying this class */ + MavlinkStreamESCInfo(MavlinkStreamESCInfo &) = delete; + MavlinkStreamESCInfo &operator = (const MavlinkStreamESCInfo &) = delete; + +protected: + uint8_t _number_of_batches{0}; + + explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + + esc_status_s esc_status; + + uint8_t batch_size = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN; + + if (_esc_status_sub.update(&esc_status)) { + + mavlink_esc_info_t msg = {}; + + msg.time_usec = esc_status.timestamp; + msg.counter = esc_status.counter; + msg.count = esc_status.esc_count; + msg.connection_type = esc_status.esc_connectiontype; + msg.info = esc_status.esc_online_flags; + + // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc + _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + + for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { + + msg.index = batch_number * batch_size; + + for (int esc_index = 0; esc_index < batch_size ; esc_index++) { + + msg.failure_flags[esc_index] = esc_status.esc[esc_index].failures; + msg.error_count[esc_index] = esc_status.esc[esc_index].esc_errorcount; + msg.temperature[esc_index] = esc_status.esc[esc_index].esc_temperature; + } + + mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg); + + } + + return true; + } + + return false; + } +}; + +class MavlinkStreamESCStatus : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamESCStatus::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "ESC_STATUS"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_ESC_STATUS; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamESCStatus(mavlink); + } + + unsigned get_size() override + { + static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + } + +private: + uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; + + /* do not allow top copying this class */ + MavlinkStreamESCStatus(MavlinkStreamESCStatus &) = delete; + MavlinkStreamESCStatus &operator = (const MavlinkStreamESCStatus &) = delete; + +protected: + + uint8_t _number_of_batches{0}; + + explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + + esc_status_s esc_status; + uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; + + if (_esc_status_sub.update(&esc_status)) { + + mavlink_esc_status_t msg = {}; + + msg.time_usec = esc_status.timestamp; + + // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc + _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + + for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { + + msg.index = batch_number * batch_size; + + for (int esc_index = 0; esc_index < batch_size ; esc_index++) { + + msg.rpm[esc_index] = esc_status.esc[esc_index].esc_rpm; + msg.voltage[esc_index] = esc_status.esc[esc_index].esc_voltage; + msg.current[esc_index] = esc_status.esc[esc_index].esc_current; + } + + mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg); + + } + + return true; + } + + return false; + } +}; + + static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), @@ -5375,6 +5551,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(), create_stream_list_item(), create_stream_list_item(),