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

navigator: add failure enum

Browse Source
This commit is contained in:
bresch 2024-08-13 17:57:29 +02:00 committed by Mathieu Bresciani
parent 25fcb3c913
commit 4ed3e9e210
5 changed files with 22 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/NavigatorStatus.msg
View File

@ -3,4 +3,7 @@
uint64 timestamp # time since system start (microseconds)
uint8 nav_state # Source mode (values in VehicleStatus)
bool failure # True when the current mode cannot continue
uint8 failure # Navigator failure enum
uint8 FAILURE_NONE = 0
uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground

13
src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp
View File

@ -42,6 +42,17 @@ void NavigatorChecks::checkAndReport(const Context &context, Report &reporter)
}
if (context.status().nav_state == status.nav_state) {
reporter.failsafeFlags().navigator_failure = status.failure;
reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE);
if (status.failure == navigator_status_s::FAILURE_HAGL) {
/* EVENT
*/
reporter.armingCheckFailure(NavModes::All,
health_component_t::system,
events::ID("check_navigator_failure_hagl"),
events::Log::Error,
"Waypoint above maximum height");
}
}
}

2
src/modules/navigator/navigator.h
View File

@ -288,7 +288,7 @@ public:
void sendWarningDescentStoppedDueToTerrain();
void trigger_failsafe(uint8_t nav_state);
void trigger_hagl_failsafe(uint8_t nav_state);
private:

8
src/modules/navigator/navigator_main.cpp
View File

@ -1358,10 +1358,10 @@ void Navigator::set_mission_failure_heading_timeout()
}
}
void Navigator::trigger_failsafe(const uint8_t nav_state)
void Navigator::trigger_hagl_failsafe(const uint8_t nav_state)
{
if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) {
_navigator_status.failure = true;
if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) {
_navigator_status.failure = navigator_status_s::FAILURE_HAGL;
_navigator_status.nav_state = nav_state;
_navigator_status_updated = true;
@ -1378,7 +1378,7 @@ void Navigator::publish_navigator_status()
if (_navigator_status.nav_state != current_nav_state) {
_navigator_status.nav_state = current_nav_state;
_navigator_status.failure = false;
_navigator_status.failure = navigator_status_s::FAILURE_NONE;
_navigator_status_updated = true;
}

2
src/modules/navigator/rtl_direct.cpp
View File

@ -164,7 +164,7 @@ void RtlDirect::set_rtl_item()
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t");
events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL");
_navigator->trigger_failsafe(getNavigatorStateId());
_navigator->trigger_hagl_failsafe(getNavigatorStateId());
_rtl_state = RTLState::IDLE;
}