From 674cd1e3b50de12d8298cf73c7d4c321a9d4c990 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Oct 2020 15:25:01 -0400 Subject: [PATCH] mavlink: move EXTENDED_SYS_STATE stream to separate file --- src/modules/mavlink/CMakeLists.txt | 2 +- src/modules/mavlink/mavlink_messages.cpp | 125 +-------------- .../mavlink/streams/EXTENDED_SYS_STATE.hpp | 149 ++++++++++++++++++ 3 files changed, 158 insertions(+), 118 deletions(-) create mode 100644 src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 840d6cbf50..b740193b29 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -58,8 +58,8 @@ px4_add_module( mavlink_shell.cpp mavlink_simple_analyzer.cpp mavlink_stream.cpp - mavlink_ulog.cpp mavlink_timesync.cpp + mavlink_ulog.cpp tune_publisher.cpp MODULE_CONFIG module.yaml diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3c38b42724..4dc38f8a75 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -45,11 +45,6 @@ #include "mavlink_simple_analyzer.h" #include "mavlink_high_latency2.h" -#include "streams/autopilot_version.h" -#include "streams/flight_information.h" -#include "streams/protocol_version.h" -#include "streams/storage_information.h" - #include #include #include @@ -123,6 +118,12 @@ using matrix::Vector3f; using matrix::wrap_2pi; +#include "streams/autopilot_version.h" +#include "streams/flight_information.h" +#include "streams/protocol_version.h" +#include "streams/storage_information.h" +#include "streams/EXTENDED_SYS_STATE.hpp" + // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align static_assert(MAV_SENSOR_ROTATION_NONE == static_cast(ROTATION_NONE), "Roll: 0, Pitch: 0, Yaw: 0"); @@ -4665,118 +4666,6 @@ protected: } }; -class MavlinkStreamExtendedSysState : public MavlinkStream -{ -public: - const char *get_name() const override - { - return MavlinkStreamExtendedSysState::get_name_static(); - } - - static constexpr const char *get_name_static() - { - return "EXTENDED_SYS_STATE"; - } - - static constexpr uint16_t get_id_static() - { - return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamExtendedSysState(mavlink); - } - - unsigned get_size() override - { - return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; - mavlink_extended_sys_state_t _msg; - - /* do not allow top copying this class */ - MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &) = delete; - MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &) = delete; - -protected: - explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), - _msg() - { - _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; - _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; - } - - bool send(const hrt_abstime t) override - { - bool updated = false; - - vehicle_status_s status; - - if (_status_sub.copy(&status)) { - updated = true; - - if (status.is_vtol) { - if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _msg.vtol_state = MAV_VTOL_STATE_MC; - - } else if (!status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_FW; - - } else if (status.in_transition_mode && status.in_transition_to_fw) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; - - } else if (status.in_transition_mode) { - _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; - } - } - } - - vehicle_land_detected_s land_detected; - - if (_landed_sub.copy(&land_detected)) { - updated = true; - - if (land_detected.landed) { - _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; - - } else if (!land_detected.landed) { - _msg.landed_state = MAV_LANDED_STATE_IN_AIR; - - vehicle_control_mode_s control_mode; - position_setpoint_triplet_s pos_sp_triplet; - - if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) { - if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { - if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - _msg.landed_state = MAV_LANDED_STATE_TAKEOFF; - - } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - _msg.landed_state = MAV_LANDED_STATE_LANDING; - } - } - } - } - } - - if (updated) { - mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg); - } - - return updated; - } -}; - class MavlinkStreamAltitude : public MavlinkStream { public: @@ -5627,7 +5516,9 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), +#ifdef EXTENDED_SYS_STATE_HPP create_stream_list_item(), +#endif // EXTENDED_SYS_STATE_HPP create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), diff --git a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp new file mode 100644 index 0000000000..8dde7caced --- /dev/null +++ b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 EXTENDED_SYS_STATE_HPP +#define EXTENDED_SYS_STATE_HPP + +class MavlinkStreamExtendedSysState : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamExtendedSysState::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "EXTENDED_SYS_STATE"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamExtendedSysState(mavlink); + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + mavlink_extended_sys_state_t _msg; + + /* do not allow top copying this class */ + MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &) = delete; + MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &) = delete; + +protected: + explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), + _msg() + { + _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + } + + bool send(const hrt_abstime t) override + { + bool updated = false; + + vehicle_status_s status; + + if (_status_sub.copy(&status)) { + updated = true; + + if (status.is_vtol) { + if (!status.in_transition_mode && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _msg.vtol_state = MAV_VTOL_STATE_MC; + + } else if (!status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_FW; + + } else if (status.in_transition_mode && status.in_transition_to_fw) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW; + + } else if (status.in_transition_mode) { + _msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC; + } + } + } + + vehicle_land_detected_s land_detected; + + if (_landed_sub.copy(&land_detected)) { + updated = true; + + if (land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; + + } else if (!land_detected.landed) { + _msg.landed_state = MAV_LANDED_STATE_IN_AIR; + + vehicle_control_mode_s control_mode; + position_setpoint_triplet_s pos_sp_triplet; + + if (_control_mode_sub.copy(&control_mode) && _pos_sp_triplet_sub.copy(&pos_sp_triplet)) { + if (control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + _msg.landed_state = MAV_LANDED_STATE_TAKEOFF; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _msg.landed_state = MAV_LANDED_STATE_LANDING; + } + } + } + } + } + + if (updated) { + mavlink_msg_extended_sys_state_send_struct(_mavlink->get_channel(), &_msg); + } + + return updated; + } +}; + +#endif // EXTENDED_SYS_STATE_HPP