diff --git a/msg/px4_msgs_old/msg/VehicleStatusV2.msg b/msg/px4_msgs_old/msg/VehicleStatusV2.msg new file mode 100644 index 0000000000..3420ada11c --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV2.msg @@ -0,0 +1,135 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 2 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_LANDING = 6 +uint8 ARM_DISARM_REASON_PREFLIGHT_INACTION = 7 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 8 +uint8 ARM_DISARM_REASON_RC_BUTTON = 13 +uint8 ARM_DISARM_REASON_FAILSAFE = 14 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) +uint8 nav_state_display # User-visible nav state sent via MAVLink (executor state if active, otherwise nav_state) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNSPECIFIED = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool traffic_avoidance_system_present + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index c383142ab0..b8d658cbbf 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -21,3 +21,4 @@ #include "translation_vehicle_local_position_v1.h" #include "translation_vehicle_status_v1.h" #include "translation_vehicle_status_v2.h" +#include "translation_vehicle_status_v3.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v2.h b/msg/translation_node/translations/translation_vehicle_status_v2.h index da4b315566..816e5d3cdd 100644 --- a/msg/translation_node/translations/translation_vehicle_status_v2.h +++ b/msg/translation_node/translations/translation_vehicle_status_v2.h @@ -6,14 +6,14 @@ // Translate VehicleStatus v1 <--> v2 #include -#include +#include class VehicleStatusV2Translation { public: using MessageOlder = px4_msgs_old::msg::VehicleStatusV1; static_assert(MessageOlder::MESSAGE_VERSION == 1); - using MessageNewer = px4_msgs::msg::VehicleStatus; + using MessageNewer = px4_msgs_old::msg::VehicleStatusV2; static_assert(MessageNewer::MESSAGE_VERSION == 2); static constexpr const char* kTopic = "fmu/out/vehicle_status"; diff --git a/msg/translation_node/translations/translation_vehicle_status_v3.h b/msg/translation_node/translations/translation_vehicle_status_v3.h new file mode 100644 index 0000000000..98935947c0 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v3.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * Copyright (c) 2026 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v2 <--> v3 +#include +#include + +class VehicleStatusV3Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV2; + static_assert(MessageOlder::MESSAGE_VERSION == 2); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 3); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.nav_state_display = msg_older.nav_state_display; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + // failure_detector_status removed (use separate FailureDetectorStatus topic) + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.traffic_avoidance_system_present = msg_older.traffic_avoidance_system_present; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.nav_state_display = msg_newer.nav_state_display; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.failure_detector_status = 0; // Removed in v3, use separate FailureDetectorStatus topic + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.traffic_avoidance_system_present = msg_newer.traffic_avoidance_system_present; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV3Translation); diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 3420ada11c..888ba6cde8 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,6 +1,6 @@ # Encodes the system state of the vehicle published by commander -uint32 MESSAGE_VERSION = 2 +uint32 MESSAGE_VERSION = 3 uint64 timestamp # time since system start (microseconds) @@ -68,18 +68,6 @@ uint8 nav_state_display # User-visible nav state sent vi uint32 valid_nav_states_mask # Bitmask for all valid nav_state values uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select -# Bitmask of detected failures -uint16 failure_detector_status -uint16 FAILURE_NONE = 0 -uint16 FAILURE_ROLL = 1 # (1 << 0) -uint16 FAILURE_PITCH = 2 # (1 << 1) -uint16 FAILURE_ALT = 4 # (1 << 2) -uint16 FAILURE_EXT = 8 # (1 << 3) -uint16 FAILURE_ARM_ESC = 16 # (1 << 4) -uint16 FAILURE_BATTERY = 32 # (1 << 5) -uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) -uint16 FAILURE_MOTOR = 128 # (1 << 7) - uint8 hil_state uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e522090622..e5135fe086 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1925,7 +1925,6 @@ void Commander::run() // Check for failure detector status if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) { - _vehicle_status.failure_detector_status = _failure_detector.getStatus().value; _status_changed = true; } @@ -1998,7 +1997,7 @@ void Commander::run() send_parachute_command(); } - // publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed + // publish states (armed, control_mode, vehicle_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)) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp index 1a4cc3e59b..0b5715662d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.cpp @@ -35,7 +35,13 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter) { - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) { + failure_detector_status_s fd_status; + + if (!_failure_detector_status_sub.copy(&fd_status)) { + return; + } + + if (fd_status.fd_roll) { /* EVENT * @description * The vehicle exceeded the maximum configured roll angle. @@ -51,7 +57,7 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)"); } - } else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) { + } else if (fd_status.fd_pitch) { /* EVENT * @description * The vehicle exceeded the maximum configured pitch angle. @@ -68,7 +74,7 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor } } - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) { + if (fd_status.fd_alt) { /* EVENT */ reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"), @@ -79,7 +85,7 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor } } - if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) { + if (fd_status.fd_ext) { /* EVENT * @description * @@ -94,12 +100,10 @@ void FailureDetectorChecks::checkAndReport(const Context &context, Report &repor } } - reporter.failsafeFlags().fd_critical_failure = context.status().failure_detector_status & - (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH | vehicle_status_s::FAILURE_ALT | - vehicle_status_s::FAILURE_EXT); + reporter.failsafeFlags().fd_critical_failure = fd_status.fd_roll || fd_status.fd_pitch || fd_status.fd_alt || + fd_status.fd_ext; - reporter.failsafeFlags().fd_imbalanced_prop = context.status().failure_detector_status & - vehicle_status_s::FAILURE_IMBALANCED_PROP; + reporter.failsafeFlags().fd_imbalanced_prop = fd_status.fd_imbalanced_prop; if (reporter.failsafeFlags().fd_imbalanced_prop) { /* EVENT diff --git a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp index ebad706c37..7be96f7267 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/failureDetectorCheck.hpp @@ -35,6 +35,8 @@ #include "../Common.hpp" +#include +#include class FailureDetectorChecks : public HealthAndArmingCheckBase { @@ -45,4 +47,5 @@ public: void checkAndReport(const Context &context, Report &reporter) override; private: + uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; }; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 74ca48d554..022fd517eb 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -135,6 +136,7 @@ private: updated |= write_heading_if_updated(&msg); updated |= write_mission_result_if_updated(&msg); updated |= write_failsafe_flags(&msg); + updated |= write_failure_detector_status(&msg); // these topics are already updated in update_data() and thus we just copy them here write_airspeed(&msg); @@ -454,10 +456,6 @@ private: } } - if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) { - msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; - } - // flight mode union px4_custom_mode custom_mode {get_px4_custom_mode(status.nav_state)}; msg->custom_mode = custom_mode.custom_mode_hl; @@ -492,6 +490,21 @@ private: return false; } + bool write_failure_detector_status(mavlink_high_latency2_t *msg) + { + failure_detector_status_s fd_status; + + if (_failure_detector_status_sub.update(&fd_status)) { + if (fd_status.fd_motor) { + msg->failure_flags |= HL_FAILURE_FLAG_ENGINE; + } + + return true; + } + + return false; + } + bool write_wind(mavlink_high_latency2_t *msg) { wind_s wind; @@ -665,6 +678,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 _failure_detector_status_sub{ORB_ID(failure_detector_status)}; 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)};