From ed0a01d5dad86b3e4137b26104d8a27e9723a66f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Mar 2020 00:10:42 +0100 Subject: [PATCH] Commander: Add reporting of preflight and prearming state This is essential for the operator to know if the system is ready to fly. --- msg/subsystem_info.msg | 1 + src/modules/commander/Commander.cpp | 14 +++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index aecdd98830..afbc05d510 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -28,6 +28,7 @@ uint64 SUBSYSTEM_TYPE_LOGGING = 16777216 uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432 uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864 uint64 SUBSYSTEM_TYPE_SATCOM = 134217728 +uint64 SUBSYSTEM_TYPE_PREARM_CHECK = 268435456 uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912 bool present diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5e9a78593c..3a605cf87b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2251,6 +2251,16 @@ Commander::run() /* publish vehicle_status_flags */ status_flags.timestamp = hrt_absolute_time(); + + // Evaluate current prearm status + if (!armed.armed) { + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, status, status_flags, true, true, true, 30_s); + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, status_flags, safety_s{}, + PreFlightCheck::arm_requirements_t{}, status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res + && prearm_check_res), status); + } + _vehicle_status_flags_pub.publish(status_flags); } @@ -3748,7 +3758,6 @@ void Commander::avoidance_check() set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled, sensor_oa_healthy, status); - } void Commander::battery_status_check() @@ -3826,6 +3835,9 @@ void Commander::battery_status_check() // No currently-connected batteries have any warning && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, + (_battery_warning == battery_status_s::BATTERY_WARNING_NONE), status); + // execute battery failsafe if the state has gotten worse while we are armed if (battery_warning_level_increased_while_armed) { battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,