diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 9bfb23f68d..f3e3f3ca46 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -937,7 +937,7 @@ void printTopics() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags" || true' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true' } diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 883ba8ebae..d459488399 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -80,6 +80,7 @@ set(msg_files estimator_status_flags.msg event.msg failure_detector_status.msg + failsafe_flags.msg follow_target.msg follow_target_estimator.msg follow_target_status.msg @@ -201,7 +202,6 @@ set(msg_files vehicle_rates_setpoint.msg vehicle_roi.msg vehicle_status.msg - vehicle_status_flags.msg vehicle_thrust_setpoint.msg vehicle_torque_setpoint.msg vehicle_trajectory_bezier.msg diff --git a/msg/vehicle_status_flags.msg b/msg/failsafe_flags.msg similarity index 100% rename from msg/vehicle_status_flags.msg rename to msg/failsafe_flags.msg diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 5fc8f2792e..e52c634b18 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -39,7 +39,6 @@ #include #include #include -#include typedef enum { TRANSITION_DENIED = -1, diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2309e0906b..f53acbe1d8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -542,7 +542,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks && !_arm_state_machine.isArmed()) { if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && - !_vehicle_status_flags.manual_control_signal_lost && _is_throttle_above_center) { + !_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t"); events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info}, @@ -552,7 +552,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } if (!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_vehicle_status_flags.manual_control_signal_lost && !_is_throttle_low + !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), @@ -1052,7 +1052,7 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; // check if current mission and first item are valid - if (!_vehicle_status_flags.auto_mission_missing) { + if (!_failsafe_flags.auto_mission_missing) { // requested first mission item valid if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { @@ -1791,7 +1791,7 @@ void Commander::run() _actuator_armed.prearmed = getPrearmState(); - // publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed + // publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed if (now - _vehicle_status.timestamp >= 500_ms || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { @@ -1869,7 +1869,7 @@ void Commander::checkForMissionUpdate() if (mission_result_ok) { /* Only evaluate mission state if home is set */ - if (!_vehicle_status_flags.home_position_invalid && + if (!_failsafe_flags.home_position_invalid && (prev_mission_instance_count != mission_result.instance_count)) { if (!auto_mission_available) { @@ -2196,7 +2196,7 @@ bool Commander::handleModeIntentionAndFailsafe() uint8_t updated_user_intented_mode = _failsafe.update(hrt_absolute_time(), state, mode_change_requested, _failsafe_user_override_request, - _vehicle_status_flags); + _failsafe_flags); _failsafe_user_override_request = false; // Force intended mode if changed by the failsafe state machine @@ -2339,7 +2339,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_color = led_control_s::COLOR_RED; } else { - if (!_vehicle_status_flags.home_position_invalid && !_vehicle_status_flags.global_position_invalid) { + if (!_failsafe_flags.home_position_invalid && !_failsafe_flags.global_position_invalid) { led_color = led_control_s::COLOR_GREEN; } else { @@ -2698,9 +2698,9 @@ void Commander::dataLinkCheck() void Commander::battery_status_check() { // Handle shutdown request from emergency battery action - if (_battery_warning != _vehicle_status_flags.battery_warning) { + if (_battery_warning != _failsafe_flags.battery_warning) { - if (_vehicle_status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) { @@ -2723,7 +2723,7 @@ void Commander::battery_status_check() } } - _battery_warning = _vehicle_status_flags.battery_warning; + _battery_warning = _failsafe_flags.battery_warning; } void Commander::manualControlCheck() @@ -2786,7 +2786,7 @@ void Commander::manualControlCheck() void Commander::offboardControlCheck() { if (_offboard_control_mode_sub.update()) { - if (_vehicle_status_flags.offboard_control_signal_lost) { + if (_failsafe_flags.offboard_control_signal_lost) { // Run arming checks immediately to allow for offboard mode activation _status_changed = true; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f82bf86579..b7201d70d5 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -209,8 +209,8 @@ private: UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks}; WorkerThread _worker_thread{}; - const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()}; - HomePosition _home_position{_vehicle_status_flags}; + const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()}; + HomePosition _home_position{_failsafe_flags}; Hysteresis _auto_disarm_landed{false}; diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 9849944744..84e2b6b856 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -178,7 +178,7 @@ void Report::reset() void Report::prepare(uint8_t vehicle_type) { // Get mode requirements before running any checks (in particular the mode checks require them) - mode_util::getModeRequirements(vehicle_type, _status_flags); + mode_util::getModeRequirements(vehicle_type, _failsafe_flags); } NavModes Report::getModeGroup(uint8_t nav_state) const diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 519857c557..91197170b9 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -188,11 +188,11 @@ public: } }; - Report(vehicle_status_flags_s &status_flags, hrt_abstime min_reporting_interval = 2_s) - : _min_reporting_interval(min_reporting_interval), _status_flags(status_flags) { } + Report(failsafe_flags_s &failsafe_flags, hrt_abstime min_reporting_interval = 2_s) + : _min_reporting_interval(min_reporting_interval), _failsafe_flags(failsafe_flags) { } ~Report() = default; - vehicle_status_flags_s &failsafeFlags() { return _status_flags; } + failsafe_flags_s &failsafeFlags() { return _failsafe_flags; } orb_advert_t *mavlink_log_pub() { return _mavlink_log_pub; } @@ -255,7 +255,7 @@ public: const HealthResults &healthResults() const { return _results[_current_result].health; } const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; } - bool modePreventsArming(uint8_t nav_state) const { return _status_flags.mode_req_prevent_arming & (1u << nav_state); } + bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); } private: /** @@ -346,7 +346,7 @@ private: Results _results[2]; ///< Previous and current results to check for changes int _current_result{0}; - vehicle_status_flags_s &_status_flags; + failsafe_flags_s &_failsafe_flags; orb_advert_t *_mavlink_log_pub{nullptr}; ///< mavlink log publication for legacy reporting }; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index ccb089b637..084376d130 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include "checks/accelerometerCheck.hpp" #include "checks/airspeedCheck.hpp" @@ -98,7 +98,7 @@ public: */ bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); } - const vehicle_status_flags_s &failsafeFlags() const { return _failsafe_flags; } + const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; } protected: void updateParams() override; @@ -107,10 +107,10 @@ private: Report _reporter; orb_advert_t _mavlink_log_pub{nullptr}; - vehicle_status_flags_s _failsafe_flags{}; + failsafe_flags_s _failsafe_flags{}; uORB::Publication _health_report_pub{ORB_ID(health_report)}; - uORB::Publication _failsafe_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::Publication _failsafe_flags_pub{ORB_ID(failsafe_flags)}; // all checks AccelerometerChecks _accelerometer_checks; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index 33f754cc86..bc919080bf 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -63,8 +63,8 @@ public: TEST_F(ReporterTest, basic_no_checks) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); reporter.reset(); @@ -83,8 +83,8 @@ TEST_F(ReporterTest, basic_no_checks) TEST_F(ReporterTest, basic_fail_all_modes) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; // ensure arming is always denied with a NavModes::All failure for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) { @@ -101,8 +101,8 @@ TEST_F(ReporterTest, basic_fail_all_modes) TEST_F(ReporterTest, arming_checks_mode_category) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; // arming must still be possible for non-relevant failures reporter.reset(); @@ -130,8 +130,8 @@ TEST_F(ReporterTest, arming_checks_mode_category) TEST_F(ReporterTest, arming_checks_mode_category2) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; // A matching mode category must deny arming reporter.reset(); @@ -153,8 +153,8 @@ TEST_F(ReporterTest, arming_checks_mode_category2) TEST_F(ReporterTest, reporting) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; uORB::Subscription event_sub{ORB_ID(event)}; event_sub.subscribe(); @@ -247,8 +247,8 @@ TEST_F(ReporterTest, reporting) TEST_F(ReporterTest, reporting_multiple) { - vehicle_status_flags_s status_flags{}; - Report reporter{status_flags, 0_s}; + failsafe_flags_s failsafe_flags{}; + Report reporter{failsafe_flags, 0_s}; uORB::Subscription event_sub{ORB_ID(event)}; event_sub.subscribe(); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index f1c7e4c832..4d1ff833e8 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -670,7 +670,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, - const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_flags_s &failsafe_flags) + const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags) { // The following flags correspond to mode requirements, and are reported in the corresponding mode checks diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 1d8f699018..f591cf03da 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -69,7 +69,7 @@ private: void gpsNoLongerValid(const Context &context, Report &reporter) const; void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, - vehicle_status_flags_s &failsafe_flags); + failsafe_flags_s &failsafe_flags); bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, const float required_accuracy, diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index bd4e2887bd..26e704b0ae 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -37,8 +37,8 @@ #include #include "commander_helper.h" -HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags) - : _vehicle_status_flags(vehicle_status_flags) +HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags) + : _failsafe_flags(failsafe_flags) { } @@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation() eph = gpos.eph; epv = gpos.epv; - } else if (!_vehicle_status_flags.gps_position_invalid) { + } else if (!_failsafe_flags.gps_position_invalid) { sensor_gps_s gps; _vehicle_gps_position_sub.copy(&gps); const double lat = static_cast(gps.lat) * 1e-7; @@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force) bool updated = false; home_position_s home{}; - if (!_vehicle_status_flags.local_position_invalid) { + if (!_failsafe_flags.local_position_invalid) { // Set home position in local coordinates const vehicle_local_position_s &lpos = _local_position_sub.get(); _heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here @@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force) updated = true; } - if (!_vehicle_status_flags.global_position_invalid) { + if (!_failsafe_flags.global_position_invalid) { // Set home using the global position estimate (fused INS/GNSS) const vehicle_global_position_s &gpos = _global_position_sub.get(); fillGlobalHomePos(home, gpos); setHomePosValid(); updated = true; - } else if (!_vehicle_status_flags.gps_position_invalid) { + } else if (!_failsafe_flags.gps_position_invalid) { // Set home using GNSS position sensor_gps_s gps_pos; _vehicle_gps_position_sub.copy(&gps_pos); @@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition() const bool local_home_valid = home.valid_lpos; if (local_home_valid && !global_home_valid) { - if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.global_position_invalid) { + if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.global_position_invalid) { // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS fused) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); @@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition() home.timestamp = hrt_absolute_time(); _home_position_pub.update(); - } else if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.gps_position_invalid) { + } else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) { // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS raw) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); @@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition() } else if (!local_home_valid && global_home_valid) { const vehicle_local_position_s &lpos = _local_position_sub.get(); - if (!_vehicle_status_flags.local_position_invalid && lpos.xy_global && lpos.z_global) { + if (!_failsafe_flags.local_position_invalid && lpos.xy_global && lpos.z_global) { // Back-compute x, y and z of home position given the global home position // and the global reference of the local frame MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon}; @@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) } if (check_if_changed && set_automatically) { - const bool can_set_home_lpos_first_time = !home.valid_lpos && !_vehicle_status_flags.local_position_invalid; + const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid; const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt) - && (!_vehicle_status_flags.global_position_invalid || !_vehicle_status_flags.gps_position_invalid)); + && (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid)); const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global); if (can_set_home_lpos_first_time diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index c6db4f646a..46a683530e 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -39,12 +39,12 @@ #include #include #include -#include +#include class HomePosition { public: - HomePosition(const vehicle_status_flags_s &vehicle_status_flags); + HomePosition(const failsafe_flags_s &failsafe_flags); ~HomePosition() = default; bool setHomePosition(bool force = false); @@ -74,5 +74,5 @@ private: uint8_t _heading_reset_counter{0}; bool _valid{false}; - const vehicle_status_flags_s &_vehicle_status_flags; + const failsafe_flags_s &_failsafe_flags; }; diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 12301ad578..ba29627f87 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -43,7 +43,7 @@ static inline void setRequirement(uint8_t nav_state, uint32_t &mode_requirement) } -void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags) +void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) { flags.mode_req_angular_velocity = 0; flags.mode_req_attitude = 0; diff --git a/src/modules/commander/ModeUtil/mode_requirements.hpp b/src/modules/commander/ModeUtil/mode_requirements.hpp index 4edd1c9285..26151b158a 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.hpp +++ b/src/modules/commander/ModeUtil/mode_requirements.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include namespace mode_util { @@ -43,7 +43,7 @@ namespace mode_util * @param vehicle_type one of vehicle_status_s::VEHICLE_TYPE_* * @param flags output flags, all mode_req_* entries are set */ -void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags); +void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags); } // namespace mode_util diff --git a/src/modules/commander/failsafe/CMakeLists.txt b/src/modules/commander/failsafe/CMakeLists.txt index 68da3412c0..23363265b1 100644 --- a/src/modules/commander/failsafe/CMakeLists.txt +++ b/src/modules/commander/failsafe/CMakeLists.txt @@ -31,20 +31,20 @@ # ############################################################################ -# Extract information from status msg file -set(status_msg_file ${PX4_SOURCE_DIR}/msg/vehicle_status_flags.msg) +# Extract information from failsafe msg file +set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg) set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h) set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html) set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html) add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file} COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py - ${status_msg_file} ${generated_uorb_struct_field_mapping_header} + ${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header} ${html_template_file} ${html_output_file} DEPENDS - ${status_msg_file} + ${failsafe_flags_msg_file} ${html_template_file} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py - COMMENT "Extracting info from status msg file" + COMMENT "Extracting info from failsafe flags msg file" ) add_custom_target(failsafe_uorb_struct_header DEPENDS ${generated_uorb_struct_field_mapping_header}) diff --git a/src/modules/commander/failsafe/emscripten.cpp b/src/modules/commander/failsafe/emscripten.cpp index d078b64630..f9d56e5479 100644 --- a/src/modules/commander/failsafe/emscripten.cpp +++ b/src/modules/commander/failsafe/emscripten.cpp @@ -167,7 +167,7 @@ void set_param_value_float(const std::string &name, float value) int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finished, bool user_override, uint8_t user_intended_mode, uint8_t vehicle_type, - vehicle_status_flags_s status_flags) + failsafe_flags_s status_flags) { uint64_t time_ms = emscripten_date_now(); FailsafeBase::State state{}; @@ -203,7 +203,7 @@ std::string action_str(int action) EMSCRIPTEN_BINDINGS(failsafe) { - class_("state") + class_("state") .constructor<>() UORB_STRUCT_FIELD_MAPPING ; diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 0f6917a5a4..12556a0e86 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -324,7 +324,7 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t } void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, - const vehicle_status_flags_s &status_flags) + const failsafe_flags_s &status_flags) { updateArmingState(time_us, state.armed, status_flags); @@ -445,7 +445,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always)); } -void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags) +void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags) { if (!_was_armed && armed) { _armed_time = time_us; @@ -459,7 +459,7 @@ void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const v _was_armed = armed; } -FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &status_flags, +FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const { Action action = Action::None; diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 7c18bac6b6..1032d17edc 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -44,14 +44,14 @@ public: protected: void checkStateAndMode(const hrt_abstime &time_us, const State &state, - const vehicle_status_flags_s &status_flags) override; - Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const override; + const failsafe_flags_s &status_flags) override; + Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override; uint8_t modifyUserIntendedMode(Action previous_action, Action current_action, uint8_t user_intended_mode) const override; private: - void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags); + void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags); enum class ManualControlLossExceptionBits : int32_t { Mission = (1 << 0), diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index b393d2aac7..3777a55c9e 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -48,7 +48,7 @@ public: protected: void checkStateAndMode(const hrt_abstime &time_us, const State &state, - const vehicle_status_flags_s &status_flags) override + const failsafe_flags_s &status_flags) override { CHECK_FAILSAFE(status_flags, manual_control_signal_lost, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm)); @@ -65,7 +65,7 @@ protected: && status_flags.fd_critical_failure, Action::Terminate); } - Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const override + Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override { return Action::None; } @@ -96,7 +96,7 @@ TEST_F(FailsafeTest, general) { FailsafeTester failsafe(nullptr); - vehicle_status_flags_s failsafe_flags{}; + failsafe_flags_s failsafe_flags{}; FailsafeBase::State state{}; state.armed = true; state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; @@ -153,7 +153,7 @@ TEST_F(FailsafeTest, takeover) { FailsafeTester failsafe(nullptr); - vehicle_status_flags_s failsafe_flags{}; + failsafe_flags_s failsafe_flags{}; FailsafeBase::State state{}; state.armed = true; state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; @@ -210,7 +210,7 @@ TEST_F(FailsafeTest, takeover_denied) { FailsafeTester failsafe(nullptr); - vehicle_status_flags_s failsafe_flags{}; + failsafe_flags_s failsafe_flags{}; FailsafeBase::State state{}; state.armed = true; state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index c50ca11000..a8cbcd2cbc 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -52,7 +52,7 @@ FailsafeBase::FailsafeBase(ModuleParams *parent) : ModuleParams(parent) uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated, bool rc_sticks_takeover_request, - const vehicle_status_flags_s &status_flags) + const failsafe_flags_s &status_flags) { if (_last_update == 0) { _last_update = time_us; @@ -390,7 +390,7 @@ void FailsafeBase::removeNonActivatedActions() } -void FailsafeBase::getSelectedAction(const State &state, const vehicle_status_flags_s &status_flags, +void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s &status_flags, bool user_intended_mode_updated, bool rc_sticks_takeover_request, SelectedActionState &returned_state) const @@ -566,7 +566,7 @@ bool FailsafeBase::actionAllowsUserTakeover(Action action) const } void FailsafeBase::clearDelayIfNeeded(const State &state, - const vehicle_status_flags_s &status_flags) + const failsafe_flags_s &status_flags) { // Clear delay if one of the following is true: // - Already in a failsafe @@ -611,7 +611,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended return user_intended_mode; } -bool FailsafeBase::modeCanRun(const vehicle_status_flags_s &status_flags, uint8_t mode) +bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode) { uint32_t mode_mask = 1u << mode; return diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index bc077baefe..e1e9dfb50d 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -33,14 +33,14 @@ #pragma once -#include +#include #include #include #include #define CHECK_FAILSAFE(status_flags, flag_name, options) \ - checkFailsafe((int)offsetof(vehicle_status_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options) + checkFailsafe((int)offsetof(failsafe_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options) class FailsafeBase: public ModuleParams { @@ -139,7 +139,7 @@ public: */ uint8_t update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated, bool rc_sticks_takeover_request, - const vehicle_status_flags_s &status_flags); + const failsafe_flags_s &status_flags); bool inFailsafe() const { return _selected_action != Action::None; } @@ -185,21 +185,21 @@ protected: }; virtual void checkStateAndMode(const hrt_abstime &time_us, const State &state, - const vehicle_status_flags_s &status_flags) = 0; - virtual Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const = 0; + const failsafe_flags_s &status_flags) = 0; + virtual Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const = 0; - const vehicle_status_flags_s &lastStatusFlags() const { return _last_status_flags; } + const failsafe_flags_s &lastStatusFlags() const { return _last_status_flags; } bool checkFailsafe(int caller_id, bool last_state_failure, bool cur_state_failure, const ActionOptions &options); - virtual void getSelectedAction(const State &state, const vehicle_status_flags_s &status_flags, + virtual void getSelectedAction(const State &state, const failsafe_flags_s &status_flags, bool user_intended_mode_updated, bool rc_sticks_takeover_request, SelectedActionState &returned_state) const; int genCallerId() { return ++_next_caller_id; } - static bool modeCanRun(const vehicle_status_flags_s &status_flags, uint8_t mode); + static bool modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode); /** * Allows to modify the user intended mode. Use only in limited cases. @@ -228,7 +228,7 @@ private: void updateDelay(const hrt_abstime &elapsed_us); void notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause); - void clearDelayIfNeeded(const State &state, const vehicle_status_flags_s &status_flags); + void clearDelayIfNeeded(const State &state, const failsafe_flags_s &status_flags); bool actionAllowsUserTakeover(Action action) const; @@ -240,7 +240,7 @@ private: hrt_abstime _last_update{}; bool _last_armed{false}; uint8_t _last_user_intended_mode{0}; - vehicle_status_flags_s _last_status_flags{}; + failsafe_flags_s _last_status_flags{}; Action _selected_action{Action::None}; bool _user_takeover_active{false}; bool _notification_required{false}; @@ -248,7 +248,7 @@ private: hrt_abstime _current_start_delay{0}; ///< _current_delay is set to this value when starting the delay hrt_abstime _current_delay{0}; ///< If > 0, stay in Hold, and take action once delay reaches 0 - int _next_caller_id{sizeof(vehicle_status_flags_s) + 1}; + int _next_caller_id{sizeof(failsafe_flags_s) + 1}; bool _duplicate_reported_once{false}; orb_advert_t _mavlink_log_pub{nullptr}; diff --git a/src/modules/commander/failsafe/parse_flags_from_msg.py b/src/modules/commander/failsafe/parse_flags_from_msg.py index 2f6a0114bd..c7cace101e 100755 --- a/src/modules/commander/failsafe/parse_flags_from_msg.py +++ b/src/modules/commander/failsafe/parse_flags_from_msg.py @@ -12,7 +12,7 @@ import re if __name__ == "__main__": parser = argparse.ArgumentParser(description='Extract fields from .msg files') - parser.add_argument('msg_file', help='vehicle_status_flags.msg file') + parser.add_argument('msg_file', help='failsafe_flags.msg file') parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h') parser.add_argument('html_template', help='HTML template input file') parser.add_argument('html_output', help='HTML output file') diff --git a/src/modules/events/rc_loss_alarm.cpp b/src/modules/events/rc_loss_alarm.cpp index f21142fcfb..c2894a6124 100644 --- a/src/modules/events/rc_loss_alarm.cpp +++ b/src/modules/events/rc_loss_alarm.cpp @@ -58,9 +58,9 @@ void RC_Loss_Alarm::process() return; } - vehicle_status_flags_s status_flags{}; + failsafe_flags_s failsafe_flags{}; - _vehicle_status_flags_sub.copy(&status_flags); + _failsafe_flags_sub.copy(&failsafe_flags); if (!_was_armed && status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { @@ -68,12 +68,12 @@ void RC_Loss_Alarm::process() _was_armed = true; // Once true, impossible to go back to false } - if (!_had_manual_control && !status_flags.manual_control_signal_lost) { + if (!_had_manual_control && !failsafe_flags.manual_control_signal_lost) { _had_manual_control = true; } - if (_was_armed && _had_manual_control && status_flags.manual_control_signal_lost && + if (_was_armed && _had_manual_control && failsafe_flags.manual_control_signal_lost && status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { play_tune(); _alarm_playing = true; diff --git a/src/modules/events/rc_loss_alarm.h b/src/modules/events/rc_loss_alarm.h index a4db621a05..585c54411e 100644 --- a/src/modules/events/rc_loss_alarm.h +++ b/src/modules/events/rc_loss_alarm.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include namespace events @@ -68,7 +68,7 @@ private: uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; bool _was_armed = false; bool _had_manual_control = false; // Don't trigger alarm for systems without RC diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index f272a87b92..20541f6fbb 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -54,8 +54,8 @@ namespace status void StatusDisplay::set_leds() { - bool gps_lock_valid = !_vehicle_status_flags_sub.get().global_position_invalid; - bool home_position_valid = !_vehicle_status_flags_sub.get().home_position_invalid; + bool gps_lock_valid = !_failsafe_flags_sub.get().global_position_invalid; + bool home_position_valid = !_failsafe_flags_sub.get().home_position_invalid; int nav_state = _vehicle_status_sub.get().nav_state; #if defined(BOARD_FRONT_LED_MASK) diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp index e2f127314f..527ad0e19c 100644 --- a/src/modules/events/status_display.cpp +++ b/src/modules/events/status_display.cpp @@ -73,7 +73,7 @@ bool StatusDisplay::check_for_updates() got_updates = true; } - if (_vehicle_status_flags_sub.update()) { + if (_failsafe_flags_sub.update()) { got_updates = true; } diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index 259b422900..8275e6ce87 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include namespace events { @@ -82,7 +82,7 @@ protected: uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; uORB::SubscriptionData _cpu_load_sub{ORB_ID(cpuload)}; uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::SubscriptionData _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::SubscriptionData _failsafe_flags_sub{ORB_ID(failsafe_flags)}; led_control_s _led_control{}; diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index 4123ba17f5..84ab3ae15a 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -72,10 +72,10 @@ bool Sticks::checkAndUpdateStickInputs() _input_available = valid_sticks; } else { - vehicle_status_flags_s vehicle_status_flags; + failsafe_flags_s failsafe_flags; - if (_vehicle_status_flags_sub.update(&vehicle_status_flags)) { - if (vehicle_status_flags.manual_control_signal_lost) { + if (_failsafe_flags_sub.update(&failsafe_flags)) { + if (failsafe_flags.manual_control_signal_lost) { _input_available = false; } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp index 32ce1b6196..a43fbfef1d 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include class Sticks : public ModuleParams { @@ -92,7 +92,7 @@ private: matrix::Vector _positions_expo; ///< modified manual sticks using expo function uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_hold_dz, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 6facc97b71..7d78da5d5b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -61,6 +61,7 @@ void LoggedTopics::add_default_topics() add_topic("cpuload"); add_optional_topic("esc_status", 250); add_topic("failure_detector_status", 100); + add_topic("failsafe_flags"); add_optional_topic("follow_target", 500); add_optional_topic("follow_target_estimator", 200); add_optional_topic("follow_target_status", 400); @@ -118,7 +119,6 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_rates_setpoint", 20); add_topic("vehicle_roi", 1000); add_topic("vehicle_status"); - add_topic("vehicle_status_flags"); add_optional_topic("vtol_vehicle_status", 200); add_topic("wind", 1000); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 8434fcdb50..50d97fd9d4 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include @@ -135,7 +135,7 @@ private: updated |= write_mission_result(&msg); updated |= write_tecs_status(&msg); updated |= write_vehicle_status(&msg); - updated |= write_vehicle_status_flags(&msg); + updated |= write_failsafe_flags(&msg); updated |= write_wind(&msg); if (updated) { @@ -443,24 +443,24 @@ private: return false; } - bool write_vehicle_status_flags(mavlink_high_latency2_t *msg) + bool write_failsafe_flags(mavlink_high_latency2_t *msg) { - vehicle_status_flags_s status_flags; + failsafe_flags_s failsafe_flags; - if (_status_flags_sub.update(&status_flags)) { - if (status_flags.gps_position_invalid) { + if (_failsafe_flags_sub.update(&failsafe_flags)) { + if (failsafe_flags.gps_position_invalid) { msg->failure_flags |= HL_FAILURE_FLAG_GPS; } - if (status_flags.offboard_control_signal_lost) { + if (failsafe_flags.offboard_control_signal_lost) { msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK; } - if (status_flags.mission_failure) { + if (failsafe_flags.mission_failure) { msg->failure_flags |= HL_FAILURE_FLAG_MISSION; } - if (status_flags.manual_control_signal_lost) { + if (failsafe_flags.manual_control_signal_lost) { msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER; } @@ -634,7 +634,7 @@ private: uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; diff --git a/src/systemcmds/microbench/test_microbench_uorb.cpp b/src/systemcmds/microbench/test_microbench_uorb.cpp index d198b3b303..b724c4a0ba 100644 --- a/src/systemcmds/microbench/test_microbench_uorb.cpp +++ b/src/systemcmds/microbench/test_microbench_uorb.cpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include namespace MicroBenchORB { @@ -105,7 +105,7 @@ private: void reset(); - vehicle_status_flags_s status; + failsafe_flags_s status; vehicle_local_position_s lpos; sensor_gyro_s gyro; sensor_gyro_fifo_s gyro_fifo; @@ -146,7 +146,7 @@ ut_declare_test_c(test_microbench_uorb, MicroBenchORB) bool MicroBenchORB::time_px4_uorb() { - int fd_status = orb_subscribe(ORB_ID(vehicle_status_flags)); + int fd_status = orb_subscribe(ORB_ID(failsafe_flags)); int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position)); int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro)); int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo)); @@ -155,7 +155,7 @@ bool MicroBenchORB::time_px4_uorb() bool updated = false; PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 100); - PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status_flags), fd_status, &status), 100); + PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(failsafe_flags), fd_status, &status), 100); printf("\n"); @@ -198,9 +198,9 @@ bool MicroBenchORB::time_px4_uorb_direct() { bool ret = false; - uORB::Subscription vehicle_status_flags{ORB_ID(vehicle_status_flags)}; - PERF("uORB::Subscription orb_check vehicle_status", ret = vehicle_status_flags.updated(), 100); - PERF("uORB::Subscription orb_copy vehicle_status", ret = vehicle_status_flags.copy(&status), 100); + uORB::Subscription failsafe_flags{ORB_ID(failsafe_flags)}; + PERF("uORB::Subscription orb_check vehicle_status", ret = failsafe_flags.updated(), 100); + PERF("uORB::Subscription orb_copy vehicle_status", ret = failsafe_flags.copy(&status), 100); printf("\n");