diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index adc194cdf1..1de240d0f6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -101,6 +101,7 @@ using matrix::Vector3f; using matrix::wrap_2pi; +#include "streams/ACTUATOR_CONTROL_TARGET.hpp" #include "streams/ACTUATOR_OUTPUT_STATUS.hpp" #include "streams/ALTITUDE.hpp" #include "streams/ATTITUDE.hpp" @@ -3226,111 +3227,6 @@ protected: } }; -template -class MavlinkStreamActuatorControlTarget : public MavlinkStream -{ -public: - const char *get_name() const override - { - return MavlinkStreamActuatorControlTarget::get_name_static(); - } - - static constexpr const char *get_name_static() - { - switch (N) { - case 0: - return "ACTUATOR_CONTROL_TARGET0"; - - case 1: - return "ACTUATOR_CONTROL_TARGET1"; - - case 2: - return "ACTUATOR_CONTROL_TARGET2"; - - case 3: - return "ACTUATOR_CONTROL_TARGET3"; - } - } - - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamActuatorControlTarget(mavlink); - } - - unsigned get_size() override - { - return (_act_ctrl_sub - && _act_ctrl_sub->advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; - } - -private: - uORB::Subscription *_act_ctrl_sub{nullptr}; - - /* do not allow top copying this class */ - MavlinkStreamActuatorControlTarget(MavlinkStreamActuatorControlTarget &) = delete; - MavlinkStreamActuatorControlTarget &operator = (const MavlinkStreamActuatorControlTarget &) = delete; - -protected: - explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink) - { - // XXX this can be removed once the multiplatform system remaps topics - switch (N) { - case 0: - _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_0)}; - break; - - case 1: - _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_1)}; - break; - - case 2: - _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_2)}; - break; - - case 3: - _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_3)}; - break; - } - } - - ~MavlinkStreamActuatorControlTarget() override - { - delete _act_ctrl_sub; - } - - bool send() override - { - actuator_controls_s act_ctrl; - - if (_act_ctrl_sub && _act_ctrl_sub->update(&act_ctrl)) { - mavlink_actuator_control_target_t msg{}; - - msg.time_usec = act_ctrl.timestamp; - msg.group_mlx = N; - - for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) { - msg.controls[i] = act_ctrl.control[i]; - } - - mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - class MavlinkStreamCameraCapture : public MavlinkStream { public: @@ -3477,8 +3373,10 @@ static const StreamListItem streams_list[] = { #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP +#if defined(ACTUATOR_CONTROL_TARGET_HPP) create_stream_list_item >(), create_stream_list_item >(), +#endif // ACTUATOR_CONTROL_TARGET_HPP #if defined(NAMED_VALUE_FLOAT_HPP) create_stream_list_item(), #endif // NAMED_VALUE_FLOAT_HPP diff --git a/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp b/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp new file mode 100644 index 0000000000..64f6eb9438 --- /dev/null +++ b/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 ACTUATOR_CONTROL_TARGET_HPP +#define ACTUATOR_CONTROL_TARGET_HPP + +#include + +template +class MavlinkStreamActuatorControlTarget : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamActuatorControlTarget(mavlink); } + + static constexpr const char *get_name_static() + { + switch (N) { + case 0: + return "ACTUATOR_CONTROL_TARGET0"; + + case 1: + return "ACTUATOR_CONTROL_TARGET1"; + + case 2: + return "ACTUATOR_CONTROL_TARGET2"; + + case 3: + return "ACTUATOR_CONTROL_TARGET3"; + } + } + + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return (_act_ctrl_sub + && _act_ctrl_sub->advertised()) ? (MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + explicit MavlinkStreamActuatorControlTarget(Mavlink *mavlink) : MavlinkStream(mavlink) + { + // XXX this can be removed once the multiplatform system remaps topics + switch (N) { + case 0: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_0)}; + break; + + case 1: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_1)}; + break; + + case 2: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_2)}; + break; + + case 3: + _act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_3)}; + break; + } + } + + ~MavlinkStreamActuatorControlTarget() override + { + delete _act_ctrl_sub; + } + + uORB::Subscription *_act_ctrl_sub{nullptr}; + + bool send() override + { + actuator_controls_s act_ctrl; + + if (_act_ctrl_sub && _act_ctrl_sub->update(&act_ctrl)) { + mavlink_actuator_control_target_t msg{}; + + msg.time_usec = act_ctrl.timestamp; + msg.group_mlx = N; + + for (unsigned i = 0; i < sizeof(msg.controls) / sizeof(msg.controls[0]); i++) { + msg.controls[i] = act_ctrl.control[i]; + } + + mavlink_msg_actuator_control_target_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +#endif // ACTUATOR_CONTROL_TARGET_HPP