dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander+mavlink: implement MAVLink standard modes

This commit is contained in:
Beat Küng 2023-07-12 11:06:51 +02:00
parent b46e1d744b
commit bb900264e0
14 changed files with 593 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/VehicleCommand.msg
View File

@ -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)|

3
msg/VehicleStatus.msg
View File

@ -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

97
src/lib/modes/standard_modes.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_status.h>
#include <stdint.h>
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

102
src/lib/modes/ui.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_status.h>
#include <stdint.h>
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

21
src/modules/commander/Commander.cpp
View File

@ -48,6 +48,8 @@
#include "px4_custom_mode.h"
#include "ModeUtil/control_mode.hpp"
#include "ModeUtil/conversions.hpp"
#include <lib/modes/ui.hpp>
#include <lib/modes/standard_modes.hpp>
/* PX4 headers */
#include <drivers/drv_hrt.h>
@ -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);

26
src/modules/commander/ModeManagement.cpp
View File

@ -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 */

3
src/modules/commander/ModeManagement.hpp
View File

@ -42,7 +42,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/config_overrides.h>
#include "ModeUtil/ui.hpp"
#include <lib/modes/ui.hpp>
#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);

34
src/modules/commander/ModeUtil/conversions.hpp
View File

@ -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

5
src/modules/commander/px4_custom_mode.h
View File

@ -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:

15
src/modules/mavlink/mavlink_main.cpp
View File

@ -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);

13
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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<MavlinkStreamUavionixADSBOutCfg>(),
#endif // UAVIONIX_ADSB_OUT_CFG_HPP
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
#if defined(AVAILABLE_MODES_HPP)
create_stream_list_item<MavlinkStreamAvailableModes>(),
#endif // AVAILABLE_MODES_HPP
#if defined(CURRENT_MODE_HPP)
create_stream_list_item<MavlinkStreamCurrentMode>(),
#endif // CURRENT_MODE_HPP
};
const char *get_stream_name(const uint16_t msg_id)

2
src/modules/mavlink/mavlink_messages.h
View File

@ -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_ */

235
src/modules/mavlink/streams/AVAILABLE_MODES.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_status.h>
#include <uORB/topics/register_ext_component_reply.h>
#include <lib/modes/standard_modes.hpp>
#include <lib/modes/ui.hpp>
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

78
src/modules/mavlink/streams/CURRENT_MODE.hpp Normal file
View File

@ -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 <uORB/topics/vehicle_status.h>
#include <lib/modes/standard_modes.hpp>
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(), &current_mode);
return true;
}
return false;
}
};
#endif // CURRENT_MODE_HPP