fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 19:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander+mavlink: implement MAVLink standard modes

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/ModeManagement.cpp
+26
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 */