diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3bee66456a..10931d5ca2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -75,6 +75,7 @@ set(msg_files estimator_status_flags.msg event.msg follow_target.msg + failure_detector_status.msg generator_status.msg geofence_result.msg gimbal_device_attitude_status.msg diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg new file mode 100644 index 0000000000..a7f23b89ff --- /dev/null +++ b/msg/failure_detector_status.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) + +# FailureDetector status +uint8 FAILURE_NONE = 0 +uint8 FAILURE_ROLL = 1 # (1 << 0) +uint8 FAILURE_PITCH = 2 # (1 << 1) +uint8 FAILURE_ALT = 4 # (1 << 2) +uint8 FAILURE_EXT = 8 # (1 << 3) +uint8 FAILURE_ARM_ESC = 16 # (1 << 4) +uint8 FAILURE_HIGH_WIND = 32 # (1 << 5) +uint8 FAILURE_BATTERY = 64 # (1 << 6) +uint8 FAILURE_IMBALANCED_PROP = 128 # (1 << 7) + +uint8 failure_status # Bitmask containing FailureDetector status + +float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 117e1e8024..cdb158854f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2776,7 +2776,7 @@ Commander::run() _failsafe_old = _status.failsafe; } - /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags) at 2 Hz or immediately when changed */ + /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed */ if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) { update_control_mode(); @@ -2839,6 +2839,13 @@ Commander::run() /* publish vehicle_status_flags */ _status_flags.timestamp = hrt_absolute_time(); _vehicle_status_flags_pub.publish(_status_flags); + + /* publish failure_detector data */ + failure_detector_status_s fd_status{}; + fd_status.timestamp = hrt_absolute_time(); + fd_status.failure_status = _failure_detector.getStatus(); + fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); + _failure_detector_status_pub.publish(fd_status); } /* play arming and battery warning tunes */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4ea3c3ab64..41fe75aaa0 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -49,6 +49,7 @@ // publications #include #include +#include #include #include #include @@ -433,6 +434,7 @@ private: // Publications uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; + uORB::Publication _failure_detector_status_pub{ORB_ID(failure_detector_status)}; uORB::Publication _test_motor_pub{ORB_ID(test_motor)}; uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 6378916c9a..229129e4c7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -60,6 +60,7 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("cpuload"); add_topic("esc_status", 250); + add_topic("failure_detector_status", 100); add_topic("follow_target", 500); add_topic("generator_status"); add_topic("heater_status");