diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8ad3fe7cff..40f6cbf819 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1557,7 +1557,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("OBSTACLE_DISTANCE", 10.0f); configure_stream_local("ODOMETRY", 30.0f); - configure_stream_local("ACTUATOR_CONTROL_TARGET0", 10.0f); configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ATTITUDE_QUATERNION", 50.0f); configure_stream_local("ATTITUDE_TARGET", 10.0f); @@ -1706,7 +1705,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("MOUNT_ORIENTATION", 10.0f); configure_stream_local("ODOMETRY", 30.0f); - configure_stream_local("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ALTITUDE", 10.0f); configure_stream_local("ATTITUDE", 50.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2de0273cc7..e172024879 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -56,7 +56,6 @@ #include #include -#include "streams/ACTUATOR_CONTROL_TARGET.hpp" #include "streams/ACTUATOR_OUTPUT_STATUS.hpp" #include "streams/ALTITUDE.hpp" #include "streams/ATTITUDE.hpp" @@ -370,10 +369,6 @@ 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/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5137c1035c..85ff50a0c0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -166,10 +166,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_attitude_target(msg); break; - case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET: - handle_message_set_actuator_control_target(msg); - break; - case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: handle_message_vision_position_estimate(msg); break; @@ -1202,70 +1198,6 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t } } -void -MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *msg) -{ - // TODO -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - PX4_ERR("SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled"); - PX4_ERR("Please disable lockstep for actuator offboard control:"); - PX4_ERR("https://docs.px4.io/main/en/simulation/#disable-lockstep-simulation"); - return; -#endif - - mavlink_set_actuator_control_target_t actuator_target; - mavlink_msg_set_actuator_control_target_decode(msg, &actuator_target); - - if (_mavlink->get_forward_externalsp() && - (mavlink_system.sysid == actuator_target.target_system || actuator_target.target_system == 0) && - (mavlink_system.compid == actuator_target.target_component || actuator_target.target_component == 0) - ) { - /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ - //bool ignore_setpoints = bool(actuator_target.group_mlx != 2); - - offboard_control_mode_s offboard_control_mode{}; - offboard_control_mode.actuator = true; - offboard_control_mode.timestamp = hrt_absolute_time(); - _offboard_control_mode_pub.publish(offboard_control_mode); - - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); - - // Publish actuator controls only once in OFFBOARD - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) { - - actuator_controls_s actuator_controls{}; - actuator_controls.timestamp = hrt_absolute_time(); - - /* Set duty cycles for the servos in the actuator_controls message */ - for (size_t i = 0; i < 8; i++) { - actuator_controls.control[i] = actuator_target.controls[i]; - } - - switch (actuator_target.group_mlx) { - case 0: - _actuator_controls_pubs[0].publish(actuator_controls); - break; - - case 1: - _actuator_controls_pubs[1].publish(actuator_controls); - break; - - case 2: - _actuator_controls_pubs[2].publish(actuator_controls); - break; - - case 3: - _actuator_controls_pubs[3].publish(actuator_controls); - break; - - default: - break; - } - } - } -} - void MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index f179d76885..77980f215a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -187,7 +187,6 @@ private: void handle_message_rc_channels(mavlink_message_t *msg); void handle_message_rc_channels_override(mavlink_message_t *msg); void handle_message_serial_control(mavlink_message_t *msg); - void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_set_position_target_global_int(mavlink_message_t *msg); @@ -288,7 +287,6 @@ private: uint16_t _mavlink_status_last_packet_rx_drop_count{0}; // ORB publications - uORB::Publication _actuator_controls_pubs[3] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2)}; uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _camera_status_pub{ORB_ID(camera_status)}; diff --git a/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp b/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp deleted file mode 100644 index 3b751f16ee..0000000000 --- a/src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp +++ /dev/null @@ -1,121 +0,0 @@ -/**************************************************************************** - * - * 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"; - } - - return "ACTUATOR_CONTROL_TARGET"; - } - - 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; - } - } - - ~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 diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index 66df116a1e..e09be516e1 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -97,8 +97,6 @@ private: // VFR_HUD throttle should only be used for operator feedback. // VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a // a single throttle value, but this should still be a useful heuristic for operator awareness. - // - // Use ACTUATOR_CONTROL_TARGET if accurate states are needed. msg.throttle = 100 * math::max( -vehicle_thrust_setpoint_0.xyz[2], vehicle_thrust_setpoint_1.xyz[0]);