From bb900264e032e7cfbef7af4cc61bd8d70b85d8fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 12 Jul 2023 11:06:51 +0200 Subject: [PATCH] commander+mavlink: implement MAVLink standard modes --- msg/VehicleCommand.msg | 1 + msg/VehicleStatus.msg | 3 + src/lib/modes/standard_modes.hpp | 97 ++++++++ src/lib/modes/ui.hpp | 102 ++++++++ src/modules/commander/Commander.cpp | 21 ++ src/modules/commander/ModeManagement.cpp | 26 ++ src/modules/commander/ModeManagement.hpp | 3 +- .../commander/ModeUtil/conversions.hpp | 34 --- src/modules/commander/px4_custom_mode.h | 5 +- src/modules/mavlink/mavlink_main.cpp | 15 +- src/modules/mavlink/mavlink_messages.cpp | 13 +- src/modules/mavlink/mavlink_messages.h | 2 - .../mavlink/streams/AVAILABLE_MODES.hpp | 235 ++++++++++++++++++ src/modules/mavlink/streams/CURRENT_MODE.hpp | 78 ++++++ 14 files changed, 593 insertions(+), 42 deletions(-) create mode 100644 src/lib/modes/standard_modes.hpp create mode 100644 src/lib/modes/ui.hpp create mode 100644 src/modules/mavlink/streams/AVAILABLE_MODES.hpp create mode 100644 src/modules/mavlink/streams/CURRENT_MODE.hpp diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 3c8d778c12..e5a9c0fdc2 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -71,6 +71,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| +uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode |MAV_STANDARD_MODE| uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 8b79003efe..6190346da4 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -64,6 +64,9 @@ uint8 NAVIGATION_STATE_MAX = 31 uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + # Bitmask of detected failures uint16 failure_detector_status uint16 FAILURE_NONE = 0 diff --git a/src/lib/modes/standard_modes.hpp b/src/lib/modes/standard_modes.hpp new file mode 100644 index 0000000000..df27f92735 --- /dev/null +++ b/src/lib/modes/standard_modes.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +namespace mode_util +{ + +// This matches the definition from MAVLink MAV_STANDARD_MODE +enum class StandardMode : uint8_t { + NON_STANDARD = 0, + POSITION_HOLD = 1, + ORBIT = 2, + CRUISE = 3, + ALTITUDE_HOLD = 4, + RETURN_HOME = 5, + SAFE_RECOVERY = 6, + MISSION = 7, + LAND = 8, + TAKEOFF = 9, +}; + +/** + * @return Get MAVLink standard mode from nav_state + */ +static inline StandardMode getStandardModeFromNavState(uint8_t nav_state) +{ + switch (nav_state) { + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF; + // Note: all other standard modes do not directly map, or are vehicle-type specific + } + + return StandardMode::NON_STANDARD; +} + +/** + * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported + */ +static inline uint8_t getNavStateFromStandardMode(StandardMode mode) +{ + switch (mode) { + case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + + case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + + case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + + case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + + default: break; + } + + return vehicle_status_s::NAVIGATION_STATE_MAX; +} + + +} // namespace mode_util diff --git a/src/lib/modes/ui.hpp b/src/lib/modes/ui.hpp new file mode 100644 index 0000000000..7eb3578b50 --- /dev/null +++ b/src/lib/modes/ui.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +namespace mode_util +{ + +/** + * @return Bitmask with all valid modes + */ +static inline uint32_t getValidNavStates() +{ + return (1u << vehicle_status_s::NAVIGATION_STATE_MANUAL) | + (1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) | + (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) | + (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) | + (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) | + (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) | + (1u << vehicle_status_s::NAVIGATION_STATE_STAB) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) | + (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) | + (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + + static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update"); +} + +const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { + "Manual", + "Altitude", + "Position", + "Mission", + "Hold", + "Return", + "6: unallocated", + "7: unallocated", + "8: unallocated", + "9: unallocated", + "Acro", + "11: UNUSED", + "Descend", + "Termination", + "Offboard", + "Stabilized", + "16: UNUSED2", + "Takeoff", + "Land", + "Follow Target", + "Precision Landing", + "Orbit", + "VTOL Takeoff", + "External 1", + "External 2", + "External 3", + "External 4", + "External 5", + "External 6", + "External 7", + "External 8", +}; + +} // namespace mode_util diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 77d75df25e..d3ac4c1776 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -48,6 +48,8 @@ #include "px4_custom_mode.h" #include "ModeUtil/control_mode.hpp" #include "ModeUtil/conversions.hpp" +#include +#include /* PX4 headers */ #include @@ -1403,6 +1405,24 @@ Commander::handle_command(const vehicle_command_s &cmd) break; } + case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: { + mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1); + uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode); + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) { + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); + + } else { + if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) { + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + } + } + } + break; + case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS: _health_and_arming_checks.update(true); answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1864,6 +1884,7 @@ void Commander::run() updateControlMode(); // vehicle_status publish (after prearm/preflight updates above) + _mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask); _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index ba28e550ac..42e8c37e34 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -534,4 +534,30 @@ void ModeManagement::checkConfigOverrides() } } +void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const +{ + valid_nav_state_mask = mode_util::getValidNavStates(); + can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION); + + // Add external modes + for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) { + if (_modes.valid(i)) { + valid_nav_state_mask |= 1u << i; + can_set_nav_state_mask |= 1u << i; + const Modes::Mode &cur_mode = _modes.mode(i); + + if (cur_mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) { + // Hide the internal mode if it's replaced + can_set_nav_state_mask &= ~(1u << cur_mode.replaces_nav_state); + } + + } else { + // Still set the mode as valid but not as selectable. This is because an external mode could still + // be selected via RC when not yet running, so we make sure to display some mode label indicating it's not + // available. + valid_nav_state_mask |= 1u << i; + } + } +} + #endif /* CONSTRAINED_FLASH */ diff --git a/src/modules/commander/ModeManagement.hpp b/src/modules/commander/ModeManagement.hpp index 7576501f3d..3c9293dfbf 100644 --- a/src/modules/commander/ModeManagement.hpp +++ b/src/modules/commander/ModeManagement.hpp @@ -42,7 +42,7 @@ #include #include -#include "ModeUtil/ui.hpp" +#include #include "UserModeIntention.hpp" #include "HealthAndArmingChecks/checks/externalChecks.hpp" @@ -155,6 +155,7 @@ public: void printStatus() const; + void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const; void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out); diff --git a/src/modules/commander/ModeUtil/conversions.hpp b/src/modules/commander/ModeUtil/conversions.hpp index 8f44a96d55..5997b97adc 100644 --- a/src/modules/commander/ModeUtil/conversions.hpp +++ b/src/modules/commander/ModeUtil/conversions.hpp @@ -98,38 +98,4 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state) return navigation_mode_t::unknown; } -const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { - "MANUAL", - "ALTCTL", - "POSCTL", - "AUTO_MISSION", - "AUTO_LOITER", - "AUTO_RTL", - "6: unallocated", - "7: unallocated", - "AUTO_LANDENGFAIL", - "9: unallocated", - "ACRO", - "11: UNUSED", - "DESCEND", - "TERMINATION", - "OFFBOARD", - "STAB", - "16: UNUSED2", - "AUTO_TAKEOFF", - "AUTO_LAND", - "AUTO_FOLLOW_TARGET", - "AUTO_PRECLAND", - "ORBIT", - "AUTO_VTOL_TAKEOFF", - "EXTERNAL1", - "EXTERNAL2", - "EXTERNAL3", - "EXTERNAL4", - "EXTERNAL5", - "EXTERNAL6", - "EXTERNAL7", - "EXTERNAL8", -}; - } // namespace mode_util diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index f73a493c5b..3d350e77de 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_STABILIZED, PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY, - PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */ + PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */ + PX4_CUSTOM_MAIN_MODE_TERMINATION }; enum PX4_CUSTOM_SUB_MODE_AUTO { @@ -139,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) break; case vehicle_status_s::NAVIGATION_STATE_TERMINATION: - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_TERMINATION; break; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8a10a40c36..d258a059e8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1189,8 +1189,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return OK; } - /* if we reach here, the stream list does not contain the stream */ -#if defined(CONSTRAINED_FLASH) // flash constrained target's don't include all streams + // if we reach here, the stream list does not contain the stream. + // flash constrained target's don't include all streams, and some are only available for the development dialect +#if defined(CONSTRAINED_FLASH) || !defined(MAVLINK_DEVELOPMENT_H) return PX4_OK; #else PX4_WARN("stream %s not found", stream_name); @@ -1402,9 +1403,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ATTITUDE", 15.0f); configure_stream_local("ATTITUDE_QUATERNION", 10.0f); configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("DISTANCE_SENSOR", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESC_INFO", 1.0f); @@ -1472,9 +1475,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ATTITUDE_QUATERNION", 50.0f); configure_stream_local("ATTITUDE_TARGET", 10.0f); + configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 2.0f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 5.0f); @@ -1546,9 +1551,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); @@ -1628,9 +1635,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ATTITUDE", 50.0f); configure_stream_local("ATTITUDE_QUATERNION", 50.0f); configure_stream_local("ATTITUDE_TARGET", 8.0f); + configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate); configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("EFI_STATUS", 10.0f); configure_stream_local("ESC_INFO", 10.0f); configure_stream_local("ESC_STATUS", 10.0f); @@ -1723,8 +1732,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ADSB_VEHICLE", unlimited_rate); configure_stream_local("ATTITUDE_TARGET", 2.0f); + configure_stream_local("AVAILABLE_MODES", 0.3f); configure_stream_local("BATTERY_STATUS", 0.5f); configure_stream_local("COLLISION", unlimited_rate); + configure_stream_local("CURRENT_MODE", 0.5f); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 1.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 c98526bff9..d9a9dc2139 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,6 +124,11 @@ #include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp" #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#ifdef MAVLINK_MSG_ID_AVAILABLE_MODES // Only defined if development.xml is used +#include "streams/AVAILABLE_MODES.hpp" +#include "streams/CURRENT_MODE.hpp" +#endif + #if !defined(CONSTRAINED_FLASH) # include "streams/ADSB_VEHICLE.hpp" # include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp" @@ -496,8 +501,14 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_CFG_HPP #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP) - create_stream_list_item() + create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP +#if defined(AVAILABLE_MODES_HPP) + create_stream_list_item(), +#endif // AVAILABLE_MODES_HPP +#if defined(CURRENT_MODE_HPP) + create_stream_list_item(), +#endif // CURRENT_MODE_HPP }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index cc246ecef1..de5dcdf9be 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -75,6 +75,4 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink); MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink); -union px4_custom_mode get_px4_custom_mode(uint8_t nav_state); - #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/streams/AVAILABLE_MODES.hpp b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp new file mode 100644 index 0000000000..a96ab5cf7f --- /dev/null +++ b/src/modules/mavlink/streams/AVAILABLE_MODES.hpp @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 AVAILABLE_MODES_HPP +#define AVAILABLE_MODES_HPP + +#include +#include +#include +#include + +class MavlinkStreamAvailableModes : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAvailableModes(mavlink); } + + ~MavlinkStreamAvailableModes() { delete[] _external_mode_names; } + + static constexpr const char *get_name_static() { return "AVAILABLE_MODES"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AVAILABLE_MODES; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _had_dynamic_update ? MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + static constexpr int MAX_NUM_EXTERNAL_MODES = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8 - + vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1; + + explicit MavlinkStreamAvailableModes(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + struct ExternalModeName { + char name[sizeof(register_ext_component_reply_s::name)] {}; + }; + ExternalModeName *_external_mode_names{nullptr}; + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)}; + + bool _had_dynamic_update{false}; + uint8_t _dynamic_update_seq{0}; + uint32_t _last_valid_nav_states_mask{0}; + uint32_t _last_can_set_nav_states_mask{0}; + + void send_single_mode(const vehicle_status_s &vehicle_status, int mode_index, int total_num_modes, uint8_t nav_state) + { + mavlink_available_modes_t available_modes{}; + available_modes.mode_index = mode_index; + available_modes.number_modes = total_num_modes; + px4_custom_mode custom_mode{get_px4_custom_mode(nav_state)}; + available_modes.custom_mode = custom_mode.data; + const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0; + + // Set the mode name if not a standard mode + available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state); + + if (available_modes.standard_mode == MAV_STANDARD_MODE_NON_STANDARD) { + static_assert(sizeof(available_modes.mode_name) >= sizeof(ExternalModeName::name), "mode name too short"); + + // Is it an external mode? + unsigned external_mode_index = nav_state - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; + + if (nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 && external_mode_index < MAX_NUM_EXTERNAL_MODES) { + if (cannot_be_selected) { + // If not selectable, it's not registered + strcpy(available_modes.mode_name, "(Mode not available)"); + + } else if (_external_mode_names) { + strncpy(available_modes.mode_name, _external_mode_names[external_mode_index].name, sizeof(available_modes.mode_name)); + available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0'; + } + + } else { // Internal + if (nav_state < sizeof(mode_util::nav_state_names) / sizeof(mode_util::nav_state_names[0])) { + strncpy(available_modes.mode_name, mode_util::nav_state_names[nav_state], sizeof(available_modes.mode_name)); + available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0'; + } + } + } + + if (cannot_be_selected) { + available_modes.properties |= MAV_MODE_PROPERTY_NOT_USER_SELECTABLE; + } + + mavlink_msg_available_modes_send_struct(_mavlink->get_channel(), &available_modes); + } + + bool request_message(float param2, float param3, float param4, + float param5, float param6, float param7) override + { + bool ret = false; + int mode_index = roundf(param2); + PX4_DEBUG("AVAILABLE_MODES request (%i)", mode_index); + + vehicle_status_s vehicle_status; + + if (!_vehicle_status_sub.copy(&vehicle_status)) { + return false; + } + + int total_num_modes = math::countSetBits(vehicle_status.valid_nav_states_mask); + + if (mode_index == 0) { // All + int cur_mode_index = 1; + + for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) { + if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) { + send_single_mode(vehicle_status, cur_mode_index, total_num_modes, nav_state); + ++cur_mode_index; + } + } + + ret = true; + + } else if (mode_index <= total_num_modes) { + // Find index + int cur_index = 0; + uint8_t nav_state = 0; + + for (; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) { + if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) { + if (++cur_index == mode_index) { + break; + } + } + } + + if (nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) { + send_single_mode(vehicle_status, mode_index, total_num_modes, nav_state); + } + + ret = true; + } + + return ret; + } + + void update_data() override + { + // Keep track of externally registered modes + register_ext_component_reply_s reply; + bool dynamic_update = false; + + if (_register_ext_component_reply_sub.update(&reply)) { + if (reply.success && reply.mode_id != -1) { + if (!_external_mode_names) { + _external_mode_names = new ExternalModeName[MAX_NUM_EXTERNAL_MODES]; + } + + unsigned mode_index = reply.mode_id - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1; + + if (_external_mode_names && mode_index < MAX_NUM_EXTERNAL_MODES) { + memcpy(_external_mode_names[mode_index].name, reply.name, sizeof(ExternalModeName::name)); + } + + dynamic_update = true; + } + } + + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + if (_last_valid_nav_states_mask == 0) { + _last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask; + } + + if (_last_can_set_nav_states_mask == 0) { + _last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask; + } + + if (vehicle_status.valid_nav_states_mask != _last_valid_nav_states_mask) { + dynamic_update = true; + _last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask; + } + + if (vehicle_status.can_set_nav_states_mask != _last_can_set_nav_states_mask) { + dynamic_update = true; + _last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask; + } + } + + if (dynamic_update) { + _had_dynamic_update = true; + ++_dynamic_update_seq; + } + } + + bool send() override + { + if (_had_dynamic_update) { + mavlink_available_modes_monitor_t monitor{}; + monitor.seq = _dynamic_update_seq; + mavlink_msg_available_modes_monitor_send_struct(_mavlink->get_channel(), &monitor); + return true; + } + + return false; + } +}; + +#endif // AVAILABLE_MODES_HPP diff --git a/src/modules/mavlink/streams/CURRENT_MODE.hpp b/src/modules/mavlink/streams/CURRENT_MODE.hpp new file mode 100644 index 0000000000..5527801670 --- /dev/null +++ b/src/modules/mavlink/streams/CURRENT_MODE.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 CURRENT_MODE_HPP +#define CURRENT_MODE_HPP + +#include +#include + +class MavlinkStreamCurrentMode : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCurrentMode(mavlink); } + + static constexpr const char *get_name_static() { return "CURRENT_MODE"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CURRENT_MODE; } + + const char *get_name() const override { return get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_CURRENT_MODE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + explicit MavlinkStreamCurrentMode(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + bool send() override + { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + mavlink_current_mode_t current_mode{}; + current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data; + current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data; + current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state); + mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), ¤t_mode); + return true; + } + + return false; + } +}; + +#endif // CURRENT_MODE_HPP