fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 14:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

standard_modes: add vehicle-type specific standard modes

Browse Source 
See https://mavlink.io/en/messages/development.html#MAV_STANDARD_MODE.
The only standard mode that is not set is MAV_STANDARD_MODE_SAFE_RECOVERY,
as PX4 uses RTL for that (with configuration parameters).
This commit is contained in:
Beat Küng
2024-11-21 11:05:56 +01:00
parent 90b968ba4e
commit aca5a70964
4 changed files with 68 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/modes/standard_modes.hpp
+62 -3
View File
@@ -57,7 +57,7 @@ enum class StandardMode : uint8_t {
/**
* @return Get MAVLink standard mode from nav_state
*/
static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
static inline StandardMode getStandardModeFromNavState(uint8_t nav_state, uint8_t vehicle_type, bool is_vtol)
{
switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME;
@@ -67,7 +67,33 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
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
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return StandardMode::ALTITUDE_HOLD;
}
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return StandardMode::POSITION_HOLD;
}
if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return StandardMode::CRUISE;
}
break;
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return StandardMode::ORBIT;
}
break;
}
return StandardMode::NON_STANDARD;
@@ -76,7 +102,7 @@ static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
/**
* @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)
static inline uint8_t getNavStateFromStandardMode(StandardMode mode, uint8_t vehicle_type, bool is_vtol)
{
switch (mode) {
case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -87,6 +113,39 @@ static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
case StandardMode::ALTITUDE_HOLD:
if (is_vtol || vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|| vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return vehicle_status_s::NAVIGATION_STATE_ALTCTL;
}
break;
case StandardMode::POSITION_HOLD:
if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
break;
case StandardMode::CRUISE:
if (!is_vtol && vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
break;
case StandardMode::ORBIT:
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
return vehicle_status_s::NAVIGATION_STATE_ORBIT;
}
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
return vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
break;
default: break;
}
src/modules/commander/Commander.cpp
+2 -1
View File
@@ -1450,7 +1450,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
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);
uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode, _vehicle_status.vehicle_type,
_vehicle_status.is_vtol);
if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
src/modules/mavlink/streams/AVAILABLE_MODES.hpp
+2 -1
View File
@@ -86,7 +86,8 @@ private:
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);
available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state, vehicle_status.vehicle_type,
vehicle_status.is_vtol);
if (mode_util::isAdvanced(nav_state)) {
available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED;
src/modules/mavlink/streams/CURRENT_MODE.hpp
+2 -1
View File
@@ -66,7 +66,8 @@ private:
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);
current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state,
vehicle_status.vehicle_type, vehicle_status.is_vtol);
mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), &current_mode);
return true;
}