mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-08 19:41:35 +08:00
WIP commander+mavlink: implement MAVLink standard modes
# Conflicts: # src/modules/mavlink/mavlink
This commit is contained in:
Beat Küng
@@ -48,6 +48,8 @@
|
||||
#include "px4_custom_mode.h"
|
||||
#include "ModeUtil/control_mode.hpp"
|
||||
#include "ModeUtil/conversions.hpp"
|
||||
#include "ModeUtil/ui.hpp"
|
||||
#include "ModeUtil/standard_modes.hpp"
|
||||
|
||||
/* PX4 headers */
|
||||
#include <drivers/drv_hrt.h>
|
||||
@@ -1380,6 +1382,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);
|
||||
@@ -1857,6 +1877,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.executor_in_charge = _mode_management.modeExecutorInCharge();
|
||||
_vehicle_status.arming_state = _arm_state_machine.getArmState();
|
||||
_vehicle_status.timestamp = hrt_absolute_time();
|
||||
|
||||
@@ -516,4 +516,24 @@ 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* CONSTRAINED_FLASH */
|
||||
|
||||
@@ -154,6 +154,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);
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user